diff --git a/.dockerignore b/.dockerignore index 6647fc9fac..39ba3cccd9 100644 --- a/.dockerignore +++ b/.dockerignore @@ -1,4 +1,3 @@ **/Dockerfile* **/.dockerignore **/.git -**/.travis.yml diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index babe3c9383..ce73cbdeb3 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,14 +1,2 @@ -Want to contribute? Great! Make sure you've read, understood and considered all -the points below before creating your PR: - -- Keep your PR under 200 lines of code and address a single concern. -- Add unit test(s) and documentation (these do not count toward your 200 lines). -- Adhere to the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). -- Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate. -- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis - runs for every update which blocks merging of other changes. Our merge bot - will rebase your branch, reformat your source code and merge as the last step - in the review process. -- Please replace this template text with the commit message you want for your - PR. You and/or the reviewer should keep it updated during the course of the - review using the GitHub edit feature. +Want to contribute? Great! Make sure you've read and understood +[CONTRIBUTING.md](https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md). diff --git a/.github/workflows/ci-bionic.yaml b/.github/workflows/ci-bionic.yaml new file mode 100644 index 0000000000..76b6c90f52 --- /dev/null +++ b/.github/workflows/ci-bionic.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 18.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.bionic diff --git a/.github/workflows/ci-bullseye.yaml b/.github/workflows/ci-bullseye.yaml new file mode 100644 index 0000000000..2144453776 --- /dev/null +++ b/.github/workflows/ci-bullseye.yaml @@ -0,0 +1,15 @@ +name: Debian Bullseye CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.bullseye diff --git a/.github/workflows/ci-buster.yaml b/.github/workflows/ci-buster.yaml new file mode 100644 index 0000000000..0faf26eddc --- /dev/null +++ b/.github/workflows/ci-buster.yaml @@ -0,0 +1,15 @@ +name: Debian Buster CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.buster diff --git a/.github/workflows/ci-focal.yaml b/.github/workflows/ci-focal.yaml new file mode 100644 index 0000000000..e1bc5cbf4a --- /dev/null +++ b/.github/workflows/ci-focal.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 20.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.focal diff --git a/.github/workflows/ci-jammy.yaml b/.github/workflows/ci-jammy.yaml new file mode 100644 index 0000000000..104ed9561a --- /dev/null +++ b/.github/workflows/ci-jammy.yaml @@ -0,0 +1,15 @@ +name: Ubuntu 22.04 CI + +on: + push: + branches: ["master"] + pull_request: + branches: ["master"] + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - name: Run the Dockerfile + run: docker build . -f Dockerfile.jammy diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index e81cd9f8c9..0000000000 --- a/.travis.yml +++ /dev/null @@ -1,36 +0,0 @@ -# Copyright 2016 The Cartographer Authors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -sudo: required -services: docker - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html -cache: - directories: - - /home/travis/docker/ - -env: - - LSB_RELEASE=trusty DOCKER_CACHE_FILE=/home/travis/docker/trusty-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=xenial DOCKER_CACHE_FILE=/home/travis/docker/xenial-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=jessie DOCKER_CACHE_FILE=/home/travis/docker/jessie-cache.tar.gz CC=gcc CXX=g++ - - LSB_RELEASE=stretch DOCKER_CACHE_FILE=/home/travis/docker/stretch-cache.tar.gz CC=gcc CXX=g++ - -before_install: scripts/load_docker_cache.sh - -install: true -script: - - docker build ${TRAVIS_BUILD_DIR} -t cartographer:${LSB_RELEASE} -f Dockerfile.${LSB_RELEASE} - --build-arg cc=$CC --build-arg cxx=$CXX - - scripts/save_docker_cache.sh diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 0e47fa611a..05e6da5f6e 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,8 +2,12 @@ Changelog for package cartographer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.0.0 (2021-03-09) +------------------ +https://github.com/cartographer-project/cartographer/compare/1.0.0...2.0.0 + 1.0.0 (2018-06-01) ----------------------- +------------------ https://github.com/googlecartographer/cartographer/compare/0.3.0...1.0.0 0.3.0 (2017-11-23) diff --git a/CMakeLists.txt b/CMakeLists.txt index de3645d5cd..ef2fcb3e69 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ include("${PROJECT_SOURCE_DIR}/cmake/functions.cmake") google_initialize_cartographer_project() google_enable_testing() -find_package(Abseil REQUIRED) +find_package(absl REQUIRED) set(BOOST_COMPONENTS iostreams) if(WIN32) list(APPEND BOOST_COMPONENTS zlib) @@ -268,7 +268,20 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PROTOBUF_INCLUDE_DIR}) # TODO(hrapp): This should not explicitly list pthread and use # PROTOBUF_LIBRARIES, but that failed on first try. -target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} standalone_absl) +target_link_libraries(${PROJECT_NAME} PUBLIC ${PROTOBUF_LIBRARY} + absl::algorithm + absl::base + absl::debugging + absl::flat_hash_map + absl::memory + absl::meta + absl::numeric + absl::str_format + absl::strings + absl::synchronization + absl::time + absl::utility +) if (NOT WIN32) target_link_libraries(${PROJECT_NAME} PUBLIC pthread) endif() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 2827b7d3fa..8495b5217e 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,27 +1,51 @@ -Want to contribute? Great! First, read this page (including the small print at the end). +Want to contribute? Great! First, read this page. ### Before you contribute -Before we can use your code, you must sign the -[Google Individual Contributor License Agreement] -(https://cla.developers.google.com/about/google-individual) -(CLA), which you can do online. The CLA is necessary mainly because you own the -copyright to your changes, even after your contribution becomes part of our -codebase, so we need your permission to use and distribute your code. We also -need to be sure of various other things—for instance that you'll tell us if you -know that your code infringes on other people's patents. You don't have to sign -the CLA until after you've submitted your code for review and a member has -approved it, but you must do it before we can put your code into our codebase. -Before you start working on a larger contribution, you should get in touch with -us first through the issue tracker with your idea so that we can help out and -possibly guide you. Coordinating up front makes it much easier to avoid -frustration later on. + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0): + +``` +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +``` + +### Developer Certificate of Origin + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). +You can sign-off a commit via `git commit -s`. ### Code reviews -All submissions, including submissions by project members, require review. We -use Github pull requests for this purpose. - -### The small print -Contributions made by corporations are covered by a different agreement than -the one above, the -[Software Grant and Corporate Contributor License Agreement] -(https://cla.developers.google.com/about/google-corporate). + +All submissions, including submissions by project members, require review. +We use GitHub pull requests for this purpose. Make sure you've read, +understood and considered all the points below before creating your PR. + +#### Style guide + +C++ code should adhere to the +[Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html). +You can handle the formatting part of the style guide via `git clang-format`. + +#### Best practices + +When preparing your PR and also during the code review make sure to follow +[best practices](https://google.github.io/eng-practices/review/developer/). +Most importantly, keep your PR under 200 lines of code and address a single +concern. + +#### Testing + +- Add unit tests and documentation (these do not count toward your 200 lines). +- Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate. +- Keep rebasing (or merging) of master branch to a minimum. It triggers Travis + runs for every update which blocks merging of other changes. diff --git a/Dockerfile.stretch b/Dockerfile.bionic similarity index 70% rename from Dockerfile.stretch rename to Dockerfile.bionic index 91d52117b7..f6e44347ba 100644 --- a/Dockerfile.stretch +++ b/Dockerfile.bionic @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# Copyright 2020 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM debian:stretch +FROM ubuntu:bionic ARG cc ARG cxx @@ -21,13 +21,14 @@ ARG cxx ENV CC=$cc ENV CXX=$cxx -# This base image doesn't ship with sudo. -RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_proto3.sh cartographer/scripts/ RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf COPY . cartographer diff --git a/Dockerfile.trusty b/Dockerfile.bionic.grpc similarity index 76% rename from Dockerfile.trusty rename to Dockerfile.bionic.grpc index ac96e93f9f..d79a257be2 100644 --- a/Dockerfile.trusty +++ b/Dockerfile.bionic.grpc @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# Copyright 2020 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM ubuntu:trusty +FROM ubuntu:bionic ARG cc ARG cxx @@ -21,10 +21,14 @@ ARG cxx ENV CC=$cc ENV CXX=$cxx +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY scripts/install_proto3.sh cartographer/scripts/ RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf COPY scripts/install_grpc.sh cartographer/scripts/ diff --git a/Dockerfile.jessie b/Dockerfile.bullseye similarity index 77% rename from Dockerfile.jessie rename to Dockerfile.bullseye index aca374222c..4e9fd92ba1 100644 --- a/Dockerfile.jessie +++ b/Dockerfile.bullseye @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# Copyright 2022 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -FROM debian:jessie +FROM debian:bullseye ARG cc ARG cxx @@ -26,9 +26,5 @@ RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf COPY . cartographer RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.xenial b/Dockerfile.buster similarity index 77% rename from Dockerfile.xenial rename to Dockerfile.buster index 715287b10f..f587e9ee89 100644 --- a/Dockerfile.xenial +++ b/Dockerfile.buster @@ -1,4 +1,4 @@ -# Copyright 2016 The Cartographer Authors +# Copyright 2020 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. - -FROM ubuntu:xenial +FROM debian:buster ARG cc ARG cxx @@ -27,9 +26,7 @@ RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/* COPY scripts/install_debs_cmake.sh cartographer/scripts/ RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* -COPY scripts/install_ceres.sh cartographer/scripts/ -RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver -COPY scripts/install_proto3.sh cartographer/scripts/ -RUN cartographer/scripts/install_proto3.sh && rm -rf protobuf +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* COPY . cartographer RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.focal b/Dockerfile.focal new file mode 100644 index 0000000000..e7575241fb --- /dev/null +++ b/Dockerfile.focal @@ -0,0 +1,33 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM ubuntu:focal + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY scripts/install_abseil.sh cartographer/scripts/ +RUN cartographer/scripts/install_abseil.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/Dockerfile.jammy b/Dockerfile.jammy new file mode 100644 index 0000000000..7e5fb9bc8a --- /dev/null +++ b/Dockerfile.jammy @@ -0,0 +1,31 @@ +# Copyright 2020 The Cartographer Authors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +FROM ubuntu:jammy + +ARG cc +ARG cxx + +# Set the preferred C/C++ compiler toolchain, if given (otherwise default). +ENV CC=$cc +ENV CXX=$cxx + +# This base image doesn't ship with sudo, apt-utils. tzdata is installed here to avoid hanging later +# when it would wait for user input. +RUN apt-get update && apt-get install -y sudo apt-utils tzdata && rm -rf /var/lib/apt/lists/* + +COPY scripts/install_debs_cmake.sh cartographer/scripts/ +RUN cartographer/scripts/install_debs_cmake.sh && rm -rf /var/lib/apt/lists/* +COPY . cartographer +RUN cartographer/scripts/install_cartographer_cmake.sh && rm -rf cartographer diff --git a/README.rst b/README.rst index b482e2a69e..a1f8b9c94f 100644 --- a/README.rst +++ b/README.rst @@ -16,7 +16,7 @@ Cartographer ============ -|build| |docs| |license| +|build-jammy| |build-focal| |build-bionic| |build-bullseye| |build-buster| |docs| |license| Purpose ======= @@ -27,9 +27,26 @@ configurations. |video| -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping +A Note for ROS Users +==================== + +**Cartographer is no longer actively maintained.** +On rare occasions critical pull requests may be merged, but no new development is currently taking place, including issue response. +If you are installing Cartographer in ROS 1 / ROS 2 using a binary package that package is a fork of this repository. +The ROS fork of Cartographer is only maintained in a limited capacity. +No new development takes place on this fork, but pull requests may be merged at the maintainers' discretion. + +The ROS fork of Cartographer, and the ROS wrapper library, can be found at these locations: + +- `Cartographer Fork `_ +- `Cartographer ROS `_ + +Additional discussion can be found in `this ROS Discourse thread`_. + + Getting started =============== @@ -37,19 +54,23 @@ Getting started * You can ask a question by `creating an issue`_. .. _our Read the Docs site: https://google-cartographer.readthedocs.io -.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question +.. _creating an issue: https://github.com/cartographer-project/cartographer_ros/issues/new?labels=question -Open house -========== +Contributing +============ + +You can find information about contributing to Cartographer at `our Contribution +page`_. -We regularly meet in an open-for-all Google hangout to discuss progress and plans for Cartographer. -You can join the `mailing list`_ to receive announcements. +.. _our Contribution page: https://github.com/cartographer-project/cartographer/blob/master/CONTRIBUTING.md -The next Cartographer Open House Hangout is on **Thursday, March 14th 2019, 5pm CET (9am PDT)** [`Hangouts link`_]. +Open house slide archive +======================== -.. _mailing list: https://groups.google.com/forum/#!forum/google-cartographer -.. _Hangouts link: https://staging.talkgadget.google.com/hangouts/_/google.com/cartographeropenhouse +In the past there had been regular open-for-all meetings to discuss progress and plans for Cartographer. +Slides of these Cartographer Open House meetings are listed below. +- March 14, 2019: `Slides `_ - February 21, 2019: `Slides `_ - January 17, 2019: `Slides `_ - November 22, 2018: `Slides `_ @@ -79,18 +100,26 @@ The next Cartographer Open House Hangout is on **Thursday, March 14th 2019, 5pm - June 22, 2017: `Slides `_ - June 8, 2017: `Slides `_ -Contributing -============ - -You can find information about contributing to Cartographer at `our Contribution -page`_. - -.. _our Contribution page: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md - -.. |build| image:: https://travis-ci.org/googlecartographer/cartographer.svg?branch=master - :alt: Build Status +.. |build-jammy| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-jammy.yaml/badge.svg + :alt: Ubuntu 22.04 Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-jammy.yaml +.. |build-focal| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml/badge.svg + :alt: Ubuntu 20.04 Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-focal.yaml +.. |build-bionic| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bionic.yaml/badge.svg + :alt: Ubuntu 18.04 Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bionic.yaml +.. |build-bullseye| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bullseye.yaml/badge.svg + :alt: Debian Bullseye Build Status + :scale: 100% + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-bullseye.yaml +.. |build-buster| image:: https://github.com/cartographer-project/cartographer/actions/workflows/ci-buster.yaml/badge.svg + :alt: Debian Buster Build Status :scale: 100% - :target: https://travis-ci.org/googlecartographer/cartographer + :target: https://github.com/cartographer-project/cartographer/actions/workflows/ci-buster.yaml .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest :alt: Documentation Status :scale: 100% @@ -98,7 +127,7 @@ page`_. .. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg :alt: Apache 2 license. :scale: 100% - :target: https://github.com/googlecartographer/cartographer/blob/master/LICENSE + :target: https://github.com/cartographer-project/cartographer/blob/master/LICENSE .. |video| image:: https://j.gifs.com/wp3BJM.gif :alt: Cartographer 3D SLAM Demo :scale: 100% diff --git a/RELEASING.rst b/RELEASING.rst index 5a3a179606..19a87263f3 100644 --- a/RELEASING.rst +++ b/RELEASING.rst @@ -5,7 +5,7 @@ Steps for Releasing catkin_generate_changelog * Update changelog to point to GitHub release log (e.g. - https://github.com/googlecartographer/cartographer/compare/0.1.0...0.2.0) + https://github.com/cartographer-project/cartographer/compare/0.1.0...0.2.0) .. code-block:: bash git commit -am"Update changelog for release" diff --git a/azure-pipelines.yml b/azure-pipelines.yml index 096ed61ff5..cd6c908d0e 100644 --- a/azure-pipelines.yml +++ b/azure-pipelines.yml @@ -19,7 +19,7 @@ jobs: timeoutInMinutes: 360 steps: - script: | - choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1 + choco sources add -n=roswin -s https://aka.ms/ros/public --priority 1 rem Azure VM runs out of space on C:, so use D: for ros and rosdeps mkdir D:\opt && mklink /J C:\opt D:\opt choco upgrade %ROS_METAPACKAGE% -y diff --git a/bazel/repositories.bzl b/bazel/repositories.bzl index 8e66a81a54..3be7834950 100644 --- a/bazel/repositories.bzl +++ b/bazel/repositories.bzl @@ -20,22 +20,20 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_nelhage_rules_boost", - sha256 = "371f49e7b29e44a718baf8b9a2dd3eca865005a851c9ecf8fb6a10a715aa58dd", - strip_prefix = "rules_boost-a5a95642f6097f8949020646ffe89d7243008981", + sha256 = "f7d620c0061631d5b7685cd1065f2e2bf0768559555010a75e8e4720006f5867", + strip_prefix = "rules_boost-c3fae06e819ed8b93e31b150387dce4864758643", urls = [ - "https://mirror.bazel.build/github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", - "https://github.com/nelhage/rules_boost/archive/a5a95642f6097f8949020646ffe89d7243008981.tar.gz", + "https://github.com/nelhage/rules_boost/archive/c3fae06e819ed8b93e31b150387dce4864758643.tar.gz", ], ) _maybe( http_archive, name = "com_github_antonovvk_bazel_rules", - sha256 = "ba75b07d3fd297375a6688e9a16583eb616e7a74b3d5e8791e7a222cf36ab26e", - strip_prefix = "bazel_rules-98ddd7e4f7c63ea0868f08bcc228463dac2f9f12", + sha256 = "2f5327a2dc9a0cc8ead93953a5d2ae2e0308aece685e46cc89c27538a7e9a73a", + strip_prefix = "bazel_rules-c76e47ebe6f0a03b9dd99e245d5a0611813c36f9", urls = [ - "https://mirror.bazel.build/github.com/antonovvk/bazel_rules/archive/98ddd7e4f7c63ea0868f08bcc228463dac2f9f12.tar.gz", - "https://github.com/antonovvk/bazel_rules/archive/98ddd7e4f7c63ea0868f08bcc228463dac2f9f12.tar.gz", + "https://github.com/drigz/bazel_rules/archive/c76e47ebe6f0a03b9dd99e245d5a0611813c36f9.tar.gz", ], ) @@ -204,15 +202,24 @@ def cartographer_repositories(): ], ) + _maybe( + http_archive, + name = "bazel_skylib", + sha256 = "e5d90f0ec952883d56747b7604e2a15ee36e288bb556c3d0ed33e818a4d971f2", + strip_prefix = "bazel-skylib-1.0.2", + urls = ["https://github.com/bazelbuild/bazel-skylib/archive/1.0.2.tar.gz"], + ) + _maybe( http_archive, name = "com_google_protobuf", - sha256 = "2244b0308846bb22b4ff0bcc675e99290ff9f1115553ae9671eba1030af31bc0", - strip_prefix = "protobuf-3.6.1.2", + sha256 = "1c744a6a1f2c901e68c5521bc275e22bdc66256eeb605c2781923365b7087e5f", + strip_prefix = "protobuf-3.13.0", urls = [ - "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.6.1.2.tar.gz", - "https://github.com/google/protobuf/archive/v3.6.1.2.tar.gz", + "https://mirror.bazel.build/github.com/google/protobuf/archive/v3.13.0.zip", + "https://github.com/google/protobuf/archive/v3.13.0.zip", ], + repo_mapping = {"@zlib": "@net_zlib_zlib"}, ) _maybe( @@ -230,21 +237,21 @@ def cartographer_repositories(): _maybe( http_archive, name = "com_github_grpc_grpc", - sha256 = "e699efa9422e071a42f052ba8369fbc810e6f7c6fb0a5b1c021f54ac1a92a1f3", - strip_prefix = "grpc-b250f34b1225cde1bb19496c5cc5d66e40111052", + sha256 = "f869c648090e8bddaa1260a271b1089caccbe735bf47ac9cd7d44d35a02fb129", + strip_prefix = "grpc-1.19.1", urls = [ - "https://mirror.bazel.build/github.com/grpc/grpc/archive/b250f34b1225cde1bb19496c5cc5d66e40111052.tar.gz", - "https://github.com/grpc/grpc/archive/b250f34b1225cde1bb19496c5cc5d66e40111052.tar.gz", + "https://mirror.bazel.build/github.com/grpc/grpc/archive/v1.19.1.tar.gz", + "https://github.com/grpc/grpc/archive/v1.19.1.tar.gz", ], ) _maybe( http_archive, name = "com_github_jupp0r_prometheus_cpp", - sha256 = "6604ea0b5ef75f405c09218f13805d4141f6506eaf0da76f5f64625f62acfcd3", - strip_prefix = "prometheus-cpp-4e0814ee3f93b796356a51a4795a332568940a72", + sha256 = "07a704819cb90ed619cbf1a2713ba39faab27b8898b4561cc11a3c8b3ace83ea", + strip_prefix = "prometheus-cpp-4b11ee7a0aa7157494df06c4a324bf6d11bd0eec", urls = [ - "https://github.com/jupp0r/prometheus-cpp/archive/4e0814ee3f93b796356a51a4795a332568940a72.tar.gz", + "https://github.com/jupp0r/prometheus-cpp/archive/4b11ee7a0aa7157494df06c4a324bf6d11bd0eec.tar.gz", ], ) @@ -254,7 +261,7 @@ def cartographer_repositories(): sha256 = "83c2a27c92979787f38810adc4b6bb67aa09607c53dbadca3430a5f29e0a1cd3", strip_prefix = "async_grpc-771af45374af7f7bfc3b622ed7efbe29a4aba403", urls = [ - "https://github.com/googlecartographer/async_grpc/archive/771af45374af7f7bfc3b622ed7efbe29a4aba403.tar.gz", + "https://github.com/cartographer-project/async_grpc/archive/771af45374af7f7bfc3b622ed7efbe29a4aba403.tar.gz", ], ) @@ -266,6 +273,17 @@ def cartographer_repositories(): urls = ["https://github.com/abseil/abseil-cpp/archive/5441bbe1db5d0f2ca24b5b60166367b0966790af.tar.gz"], ) + _maybe( + http_archive, + name = "rules_python", + sha256 = "e5470e92a18aa51830db99a4d9c492cc613761d5bdb7131c04bd92b9834380f6", + strip_prefix = "rules_python-4b84ad270387a7c439ebdccfd530e2339601ef27", + urls = [ + "https://mirror.bazel.build/github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + "https://github.com/bazelbuild/rules_python/archive/4b84ad270387a7c439ebdccfd530e2339601ef27.tar.gz", + ], + ) + # TODO(rodrigoq): remove these binds once grpc#14140 has been merged, as well # as removing `use_external` in cartographer_grpc/BUILD.bazel. # https://github.com/grpc/grpc/pull/14140 diff --git a/cartographer-config.cmake.in b/cartographer-config.cmake.in index 1986207a53..98193da912 100644 --- a/cartographer-config.cmake.in +++ b/cartographer-config.cmake.in @@ -39,7 +39,7 @@ find_package(Ceres ${QUIET_OR_REQUIRED_OPTION} HINTS ${CERES_DIR_HINTS}) if (WIN32) find_package(glog REQUIRED) endif() -find_package(Abseil ${QUIET_OR_REQUIRED_OPTION}) +find_package(absl ${QUIET_OR_REQUIRED_OPTION}) if(CARTOGRAPHER_HAS_GRPC) find_package(async_grpc ${QUIET_OR_REQUIRED_OPTION}) endif() diff --git a/cartographer/BUILD.bazel b/cartographer/BUILD.bazel index 131fc1ce6e..33d54fdf2f 100644 --- a/cartographer/BUILD.bazel +++ b/cartographer/BUILD.bazel @@ -96,9 +96,10 @@ cc_library( ":cc_protos", "@boost//:iostreams", "@com_google_absl//absl/base", - "@com_google_absl//absl/strings", "@com_google_absl//absl/container:flat_hash_map", "@com_google_absl//absl/container:flat_hash_set", + "@com_google_absl//absl/strings", + "@com_google_absl//absl/strings:str_format", "@com_google_absl//absl/synchronization", "@com_google_absl//absl/types:optional", "@com_google_glog//:glog", diff --git a/cartographer/cloud/BUILD.bazel b/cartographer/cloud/BUILD.bazel index c38f0a901f..57d8f7b6a2 100644 --- a/cartographer/cloud/BUILD.bazel +++ b/cartographer/cloud/BUILD.bazel @@ -91,6 +91,8 @@ cc_binary( name = src.replace("/", "_").replace(".cc", ""), srcs = [src], data = ["//:configuration_files"], + # TODO(gaschler): Fix UplinkServerRestarting test for Bazel. + args = ["--gtest_filter=-ClientServerTestByGridType/ClientServerTestByGridType.LocalSlam2DUplinkServerRestarting*"], flaky = True, # :internal_client_server_test sometimes fails. # Tests cannot run concurrently as some of them open the same port. tags = ["exclusive"], diff --git a/cartographer/cloud/internal/client/pose_graph_stub.cc b/cartographer/cloud/internal/client/pose_graph_stub.cc index 76f1c88d38..23cacb2e93 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.cc +++ b/cartographer/cloud/internal/client/pose_graph_stub.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/client/pose_graph_stub.h" + #include "async_grpc/client.h" #include "cartographer/cloud/internal/handlers/delete_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/get_all_submap_poses.h" @@ -51,6 +52,11 @@ PoseGraphStub::GetAllSubmapData() const { LOG(FATAL) << "Not implemented"; } +mapping::PoseGraphInterface::SubmapData PoseGraphStub::GetSubmapData( + const mapping::SubmapId& submap_id) const { + LOG(FATAL) << "Not implemented"; +} + mapping::MapById PoseGraphStub::GetAllSubmapPoses() const { google::protobuf::Empty request; diff --git a/cartographer/cloud/internal/client/pose_graph_stub.h b/cartographer/cloud/internal/client/pose_graph_stub.h index b2e6550867..bcca67c11f 100644 --- a/cartographer/cloud/internal/client/pose_graph_stub.h +++ b/cartographer/cloud/internal/client/pose_graph_stub.h @@ -34,6 +34,7 @@ class PoseGraphStub : public ::cartographer::mapping::PoseGraphInterface { void RunFinalOptimization() override; mapping::MapById GetAllSubmapData() const override; + SubmapData GetSubmapData(const mapping::SubmapId& submap_id) const override; mapping::MapById GetAllSubmapPoses() const override; transform::Rigid3d GetLocalToGlobalTransform( diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.cc b/cartographer/cloud/internal/client/trajectory_builder_stub.cc index 104d92c910..b12caa86b8 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.cc +++ b/cartographer/cloud/internal/client/trajectory_builder_stub.cc @@ -18,7 +18,7 @@ #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/cloud/internal/client/trajectory_builder_stub.h b/cartographer/cloud/internal/client/trajectory_builder_stub.h index 95251a9147..c57a4f3a9a 100644 --- a/cartographer/cloud/internal/client/trajectory_builder_stub.h +++ b/cartographer/cloud/internal/client/trajectory_builder_stub.h @@ -26,7 +26,7 @@ #include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" #include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" #include "cartographer/cloud/internal/handlers/receive_local_slam_results_handler.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "grpc++/grpc++.h" #include "pose_graph_stub.h" diff --git a/cartographer/cloud/internal/client_server_test.cc b/cartographer/cloud/internal/client_server_test.cc index 90c2dd7f46..ade3c98fb7 100644 --- a/cartographer/cloud/internal/client_server_test.cc +++ b/cartographer/cloud/internal/client_server_test.cc @@ -21,20 +21,21 @@ #include "cartographer/cloud/internal/map_builder_server.h" #include "cartographer/cloud/map_builder_server_options.h" #include "cartographer/io/internal/in_memory_proto_stream.h" +#include "cartographer/io/internal/testing/test_helpers.h" #include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream_deserializer.h" -#include "cartographer/io/testing/test_helpers.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/testing/mock_map_builder.h" #include "cartographer/mapping/internal/testing/mock_pose_graph.h" #include "cartographer/mapping/internal/testing/mock_trajectory_builder.h" #include "cartographer/mapping/internal/testing/test_helpers.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/map_builder.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "gtest/gtest.h" using ::cartographer::io::testing::ProtoReaderFromStrings; -using ::cartographer::mapping::MapBuilder; +using ::cartographer::mapping::CreateMapBuilder; using ::cartographer::mapping::MapBuilderInterface; using ::cartographer::mapping::PoseGraphInterface; using ::cartographer::mapping::TrajectoryBuilderInterface; @@ -55,7 +56,7 @@ constexpr double kDuration = 4.; // Seconds. constexpr double kTimeStep = 0.1; // Seconds. constexpr double kTravelDistance = 1.2; // Meters. -constexpr char kSerializationHeaderProtoString[] = "format_version: 1"; +constexpr char kSerializationHeaderProtoString[] = "format_version: 2"; constexpr char kPoseGraphProtoString[] = R"(pose_graph { trajectory: { trajectory_id: 0 @@ -139,15 +140,15 @@ class ClientServerTestBase : public T { } void InitializeRealServer() { - auto map_builder = absl::make_unique( - map_builder_server_options_.map_builder_options()); + auto map_builder = + CreateMapBuilder(map_builder_server_options_.map_builder_options()); server_ = absl::make_unique(map_builder_server_options_, std::move(map_builder)); EXPECT_TRUE(server_ != nullptr); } void InitializeRealUploadingServer() { - auto map_builder = absl::make_unique( + auto map_builder = CreateMapBuilder( uploading_map_builder_server_options_.map_builder_options()); uploading_server_ = absl::make_unique( uploading_map_builder_server_options_, std::move(map_builder)); diff --git a/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc index f92d2c7608..33c0dbf0cf 100644 --- a/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_fixed_frame_pose_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc index 679f206860..4052a86331 100644 --- a/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_imu_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_imu_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc index 0b72934692..5f772cd64a 100644 --- a/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_landmark_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_landmark_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc index 6187e8ac52..e0571a8b4f 100644 --- a/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_odometry_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_odometry_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc index a7af01e089..63b0642a3b 100644 --- a/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_rangefinder_data_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_rangefinder_data_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc b/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc index cb315b1b42..c277428bcf 100644 --- a/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc +++ b/cartographer/cloud/internal/handlers/add_sensor_data_batch_handler.cc @@ -20,7 +20,7 @@ #include "async_grpc/rpc_handler.h" #include "cartographer/cloud/internal/map_builder_context_interface.h" #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/metrics/counter.h" #include "cartographer/sensor/internal/dispatchable.h" #include "cartographer/sensor/timed_point_cloud_data.h" diff --git a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc b/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc index d05ec5d600..4bbb209662 100644 --- a/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc +++ b/cartographer/cloud/internal/handlers/add_trajectory_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" + #include "cartographer/cloud/internal/sensor/serialization.h" #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" diff --git a/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc b/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc index 20067fae95..be45875afa 100644 --- a/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc +++ b/cartographer/cloud/internal/handlers/get_landmark_poses_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/get_landmark_poses_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc index 015b5171d1..485c594331 100644 --- a/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc +++ b/cartographer/cloud/internal/handlers/set_landmark_pose_handler_test.cc @@ -15,6 +15,7 @@ */ #include "cartographer/cloud/internal/handlers/set_landmark_pose_handler.h" + #include "cartographer/cloud/internal/testing/handler_test.h" #include "cartographer/cloud/internal/testing/test_helpers.h" #include "cartographer/transform/rigid_transform_test_helpers.h" diff --git a/cartographer/cloud/internal/local_trajectory_uploader.cc b/cartographer/cloud/internal/local_trajectory_uploader.cc index 4a0dc692a7..3f5a160451 100644 --- a/cartographer/cloud/internal/local_trajectory_uploader.cc +++ b/cartographer/cloud/internal/local_trajectory_uploader.cc @@ -25,7 +25,7 @@ #include "cartographer/cloud/internal/handlers/add_trajectory_handler.h" #include "cartographer/cloud/internal/handlers/finish_trajectory_handler.h" #include "cartographer/cloud/internal/sensor/serialization.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "glog/logging.h" #include "grpc++/grpc++.h" diff --git a/cartographer/cloud/internal/local_trajectory_uploader_test.cc b/cartographer/cloud/internal/local_trajectory_uploader_test.cc index 83b8abccbd..b333dc9fc5 100644 --- a/cartographer/cloud/internal/local_trajectory_uploader_test.cc +++ b/cartographer/cloud/internal/local_trajectory_uploader_test.cc @@ -33,7 +33,7 @@ const int kLocalTrajectoryId = 3; TEST(LocalTrajectoryUploaderTest, HandlesInvalidUplink) { auto uploader = CreateLocalTrajectoryUploader("invalid-uplink-address:50051", - /*batch_size=*/1, false, ""); + /*batch_size=*/1, false, false); uploader->Start(); mapping::proto::TrajectoryBuilderOptions options; auto status = uploader->AddTrajectory( diff --git a/cartographer/cloud/internal/map_builder_context_impl.cc b/cartographer/cloud/internal/map_builder_context_impl.cc index 4606046f12..3f3c46212c 100644 --- a/cartographer/cloud/internal/map_builder_context_impl.cc +++ b/cartographer/cloud/internal/map_builder_context_impl.cc @@ -14,8 +14,6 @@ * limitations under the License. */ -#include "cartographer/cloud/internal/map_builder_server.h" - #include "cartographer/cloud/internal/map_builder_server.h" #include "cartographer/mapping/internal/2d/local_slam_result_2d.h" #include "cartographer/mapping/internal/3d/local_slam_result_3d.h" diff --git a/cartographer/cloud/internal/map_builder_context_interface.h b/cartographer/cloud/internal/map_builder_context_interface.h index ee6fe2d466..bbcd11132c 100644 --- a/cartographer/cloud/internal/map_builder_context_interface.h +++ b/cartographer/cloud/internal/map_builder_context_interface.h @@ -19,7 +19,7 @@ #include "async_grpc/execution_context.h" #include "cartographer/cloud/internal/local_trajectory_uploader.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/proto/serialization.pb.h" diff --git a/cartographer/cloud/internal/map_builder_server.h b/cartographer/cloud/internal/map_builder_server.h index 83c7e3c1fd..03a70aa405 100644 --- a/cartographer/cloud/internal/map_builder_server.h +++ b/cartographer/cloud/internal/map_builder_server.h @@ -23,13 +23,12 @@ #include "cartographer/cloud/internal/map_builder_context_interface.h" #include "cartographer/cloud/map_builder_server_interface.h" #include "cartographer/cloud/proto/map_builder_server_options.pb.h" -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/common/time.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" -#include "cartographer/mapping/map_builder.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/internal/dispatchable.h" diff --git a/cartographer/cloud/internal/sensor/serialization.h b/cartographer/cloud/internal/sensor/serialization.h index 46ee12686e..9da7fe2cea 100644 --- a/cartographer/cloud/internal/sensor/serialization.h +++ b/cartographer/cloud/internal/sensor/serialization.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_CLOUD_INTERNAL_SENSOR_SERIALIZATION_H #include "cartographer/cloud/proto/map_builder_service.pb.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/imu_data.h" diff --git a/cartographer/cloud/internal/testing/handler_test.h b/cartographer/cloud/internal/testing/handler_test.h index 189a70dbfb..b114a6a5bd 100644 --- a/cartographer/cloud/internal/testing/handler_test.h +++ b/cartographer/cloud/internal/testing/handler_test.h @@ -19,11 +19,11 @@ #include "absl/memory/memory.h" #include "async_grpc/testing/rpc_handler_test_server.h" +#include "cartographer/cloud/internal/testing/mock_local_trajectory_uploader.h" +#include "cartographer/cloud/internal/testing/mock_map_builder_context.h" #include "cartographer/mapping/internal/testing/mock_map_builder.h" #include "cartographer/mapping/internal/testing/mock_pose_graph.h" #include "gtest/gtest.h" -#include "mock_local_trajectory_uploader.h" -#include "mock_map_builder_context.h" namespace cartographer { namespace cloud { diff --git a/cartographer/cloud/internal/testing/mock_map_builder_context.h b/cartographer/cloud/internal/testing/mock_map_builder_context.h index 9ec8788ed5..bb0e1d7abe 100644 --- a/cartographer/cloud/internal/testing/mock_map_builder_context.h +++ b/cartographer/cloud/internal/testing/mock_map_builder_context.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_CLOUD_INTERNAL_TESTING_MOCK_MAP_BUILDER_CONTEXT_H #include "cartographer/cloud/internal/map_builder_context_interface.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "gtest/gtest.h" diff --git a/cartographer/cloud/map_builder_server_main.cc b/cartographer/cloud/map_builder_server_main.cc index 67d4051556..81bf87eca0 100644 --- a/cartographer/cloud/map_builder_server_main.cc +++ b/cartographer/cloud/map_builder_server_main.cc @@ -50,7 +50,7 @@ void Run(const std::string& configuration_directory, proto::MapBuilderServerOptions map_builder_server_options = LoadMapBuilderServerOptions(configuration_directory, configuration_basename); - auto map_builder = absl::make_unique( + auto map_builder = mapping::CreateMapBuilder( map_builder_server_options.map_builder_options()); std::unique_ptr map_builder_server = CreateMapBuilderServer(map_builder_server_options, diff --git a/cartographer/cloud/map_builder_server_options.cc b/cartographer/cloud/map_builder_server_options.cc index 25fcc87722..84042f3224 100644 --- a/cartographer/cloud/map_builder_server_options.cc +++ b/cartographer/cloud/map_builder_server_options.cc @@ -18,7 +18,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" namespace cartographer { namespace cloud { diff --git a/cartographer/common/configuration_file_resolver.cc b/cartographer/common/configuration_file_resolver.cc index 9774caf104..3c2fd10c0e 100644 --- a/cartographer/common/configuration_file_resolver.cc +++ b/cartographer/common/configuration_file_resolver.cc @@ -47,6 +47,7 @@ std::string ConfigurationFileResolver::GetFullPathOrDie( std::string ConfigurationFileResolver::GetFileContentOrDie( const std::string& basename) { + CHECK(!basename.empty()) << "File basename cannot be empty." << basename; const std::string filename = GetFullPathOrDie(basename); std::ifstream stream(filename.c_str()); return std::string((std::istreambuf_iterator(stream)), diff --git a/cartographer/common/configuration_files_test.cc b/cartographer/common/configuration_files_test.cc index 62db6d8f48..d3c451abb0 100644 --- a/cartographer/common/configuration_files_test.cc +++ b/cartographer/common/configuration_files_test.cc @@ -20,7 +20,7 @@ #include "cartographer/common/config.h" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/map_builder.h" +#include "cartographer/mapping/map_builder_interface.h" #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/common/histogram.cc b/cartographer/common/histogram.cc index e887208ea7..d54a9b5dab 100644 --- a/cartographer/common/histogram.cc +++ b/cartographer/common/histogram.cc @@ -21,6 +21,7 @@ #include #include "absl/strings/str_cat.h" +#include "absl/strings/str_format.h" #include "cartographer/common/port.h" #include "glog/logging.h" @@ -59,8 +60,8 @@ std::string Histogram::ToString(const int buckets) const { } } total_count += count; - absl::StrAppend(&result, "\n[", lower_bound, ", ", upper_bound, - i + 1 == buckets ? "]" : ")"); + absl::StrAppendFormat(&result, "\n[%f, %f%c", lower_bound, upper_bound, + i + 1 == buckets ? ']' : ')'); constexpr int kMaxBarChars = 20; const int bar = (count * kMaxBarChars + values_.size() / 2) / values_.size(); diff --git a/cartographer/common/blocking_queue.h b/cartographer/common/internal/blocking_queue.h similarity index 100% rename from cartographer/common/blocking_queue.h rename to cartographer/common/internal/blocking_queue.h diff --git a/cartographer/common/blocking_queue_test.cc b/cartographer/common/internal/blocking_queue_test.cc similarity index 98% rename from cartographer/common/blocking_queue_test.cc rename to cartographer/common/internal/blocking_queue_test.cc index b6f019f5c6..47f961e0e8 100644 --- a/cartographer/common/blocking_queue_test.cc +++ b/cartographer/common/internal/blocking_queue_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include #include diff --git a/cartographer/common/ceres_solver_options.cc b/cartographer/common/internal/ceres_solver_options.cc similarity index 96% rename from cartographer/common/ceres_solver_options.cc rename to cartographer/common/internal/ceres_solver_options.cc index 25cd55815a..fd13e02ac9 100644 --- a/cartographer/common/ceres_solver_options.cc +++ b/cartographer/common/internal/ceres_solver_options.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" namespace cartographer { namespace common { diff --git a/cartographer/common/ceres_solver_options.h b/cartographer/common/internal/ceres_solver_options.h similarity index 100% rename from cartographer/common/ceres_solver_options.h rename to cartographer/common/internal/ceres_solver_options.h diff --git a/cartographer/common/rate_timer.h b/cartographer/common/internal/rate_timer.h similarity index 100% rename from cartographer/common/rate_timer.h rename to cartographer/common/internal/rate_timer.h diff --git a/cartographer/common/rate_timer_test.cc b/cartographer/common/internal/rate_timer_test.cc similarity index 97% rename from cartographer/common/rate_timer_test.cc rename to cartographer/common/internal/rate_timer_test.cc index 9bf3c753ec..8c73bddd54 100644 --- a/cartographer/common/rate_timer_test.cc +++ b/cartographer/common/internal/rate_timer_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/common/rate_timer.h" +#include "cartographer/common/internal/rate_timer.h" #include "gtest/gtest.h" diff --git a/cartographer/common/lua_parameter_dictionary_test_helpers.h b/cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h similarity index 100% rename from cartographer/common/lua_parameter_dictionary_test_helpers.h rename to cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h diff --git a/cartographer/common/lockless_queue.h b/cartographer/common/lockless_queue.h deleted file mode 100644 index 25174396ef..0000000000 --- a/cartographer/common/lockless_queue.h +++ /dev/null @@ -1,174 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ -#define CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ - -#include -#include - -#include "absl/memory/memory.h" -#include "glog/logging.h" - -namespace cartographer { -namespace common { - -// Lock-less queue which is thread safe for concurrent data producers and a -// single data consumer. -// -// This lockless queue implementation is adapted from -// https://github.com/resonance-audio/resonance-audio/blob/master/resonance_audio/utils/lockless_task_queue.h -template -class LocklessQueue { - public: - LocklessQueue() { - free_list_head_ = nullptr; - incoming_data_list_head_ = nullptr; - data_list_head_ = nullptr; - data_list_tail_ = nullptr; - } - - ~LocklessQueue() { - FreeNodes(free_list_head_.exchange(nullptr)); - FreeNodes(incoming_data_list_head_.exchange(nullptr)); - FreeNodes(data_list_head_); - } - - // Pushes the data item into the queue. - void Push(std::unique_ptr t) { - Node* const free_node = PopNodeFromFreeList(); - CHECK(free_node); - free_node->data = std::move(t); - PushNodeToList(&incoming_data_list_head_, free_node); - } - - // Pops the oldest data item from the queue. If the queue is empty returns a - // nullptr. - std::unique_ptr Pop() { - SwapLists(); - if (data_list_head_ != nullptr) { - Node* node = data_list_head_; - data_list_head_ = data_list_head_->next; - std::unique_ptr data = std::move(node->data); - PushNodeToList(&free_list_head_, node); - return std::move(data); - } - return nullptr; - } - - private: - // Node to model a single-linked list. - struct Node { - Node() = default; - - // Dummy copy constructor to enable vector::resize allocation. - Node(const Node& node) : next() {} - - // User data. - std::unique_ptr data; - - // Pointer to next node. - std::atomic next; - }; - - // Deallocates all nodes of the list starting with 'head'. - void FreeNodes(Node* node) { - while (node != nullptr) { - Node* next_node_ptr = node->next; - delete node; - node = next_node_ptr; - } - } - - // Pushes a node to the front of a list. - void PushNodeToList(std::atomic* list_head, Node* node) { - DCHECK(list_head); - DCHECK(node); - Node* list_head_ptr; - do { - list_head_ptr = list_head->load(); - node->next = list_head_ptr; - } while (!std::atomic_compare_exchange_strong_explicit( - list_head, &list_head_ptr, node, std::memory_order_release, - std::memory_order_relaxed)); - } - - // Pops a node from the front of the free node list. If the list is empty - // constructs a new node instance. - Node* PopNodeFromFreeList() { - Node* list_head_ptr; - Node* list_head_next_ptr; - do { - list_head_ptr = free_list_head_.load(); - if (list_head_ptr == nullptr) { - return new Node; - } - list_head_next_ptr = list_head_ptr->next.load(); - } while (!std::atomic_compare_exchange_strong_explicit( - &free_list_head_, &list_head_ptr, list_head_next_ptr, - std::memory_order_relaxed, std::memory_order_relaxed)); - return list_head_ptr; - } - - // Swaps the incoming data list for an empty list and appends all items - // to 'data_list_tail_'. - void SwapLists() { - Node* node_itr = incoming_data_list_head_.exchange(nullptr); - if (node_itr == nullptr) { - // There is no data on the incoming list. - return; - } - // The first node of the incoming data list will become the tail of the - // data list. - Node* const data_list_tail = node_itr; - - // Reverses the list order. After this operation 'prev_node_itr' points to - // head of the new data list items. - Node* prev_node_itr = nullptr; - while (node_itr != nullptr) { - Node* const next_node_ptr = node_itr->next; - node_itr->next = prev_node_itr; - prev_node_itr = node_itr; - node_itr = next_node_ptr; - } - - // If the previous data list was empty, replace head rather than appending - // to the list. - if (data_list_tail_ == nullptr) { - data_list_head_ = prev_node_itr; - } else { - data_list_tail_->next = prev_node_itr; - } - data_list_tail_ = data_list_tail; - } - - // Pointer to head node of free list. - std::atomic free_list_head_; - - // Pointer to head node of incoming data list, which is in FILO order. - std::atomic incoming_data_list_head_; - - // Pointer to head node of data list. - Node* data_list_head_; - - // Pointer to tail node of data list. - Node* data_list_tail_; -}; - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_LOCKLESS_QUEUE_H_ diff --git a/cartographer/common/lockless_queue_test.cc b/cartographer/common/lockless_queue_test.cc deleted file mode 100644 index f1f247c4ff..0000000000 --- a/cartographer/common/lockless_queue_test.cc +++ /dev/null @@ -1,25 +0,0 @@ -#include "cartographer/common/lockless_queue.h" -#include "gtest/gtest.h" - -namespace cartographer { -namespace common { -namespace { - -TEST(LocklessQueueTest, PushAndPop) { - LocklessQueue queue; - queue.Push(absl::make_unique(1)); - queue.Push(absl::make_unique(2)); - EXPECT_EQ(*queue.Pop(), 1); - queue.Push(absl::make_unique(3)); - queue.Push(absl::make_unique(4)); - EXPECT_EQ(*queue.Pop(), 2); - queue.Push(absl::make_unique(5)); - EXPECT_EQ(*queue.Pop(), 3); - EXPECT_EQ(*queue.Pop(), 4); - EXPECT_EQ(*queue.Pop(), 5); - EXPECT_EQ(queue.Pop(), nullptr); -} - -} // namespace -} // namespace common -} // namespace cartographer \ No newline at end of file diff --git a/cartographer/common/lua_parameter_dictionary_test.cc b/cartographer/common/lua_parameter_dictionary_test.cc index d794e0200a..ada3b7b363 100644 --- a/cartographer/common/lua_parameter_dictionary_test.cc +++ b/cartographer/common/lua_parameter_dictionary_test.cc @@ -21,7 +21,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/common/port.h b/cartographer/common/port.h index 338861f318..eec84697b8 100644 --- a/cartographer/common/port.h +++ b/cartographer/common/port.h @@ -17,13 +17,12 @@ #ifndef CARTOGRAPHER_COMMON_PORT_H_ #define CARTOGRAPHER_COMMON_PORT_H_ -#include -#include -#include - #include #include #include +#include +#include +#include namespace cartographer { diff --git a/cartographer/common/thread_pool.cc b/cartographer/common/thread_pool.cc index c6295f895c..2457152980 100644 --- a/cartographer/common/thread_pool.cc +++ b/cartographer/common/thread_pool.cc @@ -37,6 +37,7 @@ void ThreadPoolInterface::SetThreadPool(Task* task) { } ThreadPool::ThreadPool(int num_threads) { + CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!"; absl::MutexLock locker(&mutex_); for (int i = 0; i != num_threads; ++i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); diff --git a/cartographer/common/time.cc b/cartographer/common/time.cc index a2c90d227c..e74eead417 100644 --- a/cartographer/common/time.cc +++ b/cartographer/common/time.cc @@ -17,6 +17,7 @@ #include "cartographer/common/time.h" #include + #include #include #include diff --git a/cartographer/common/utils.h b/cartographer/common/utils.h deleted file mode 100644 index 229ad694f1..0000000000 --- a/cartographer/common/utils.h +++ /dev/null @@ -1,34 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_COMMON_UTILS_H_ -#define CARTOGRAPHER_COMMON_UTILS_H_ - -namespace cartographer { -namespace common { - -template -ValueType* FindOrNull(MapType& map, const KeyType& key) { - auto it = map.find(key); - if (it == map.end()) return nullptr; - return &(it->second); -} - -} // namespace common -} // namespace cartographer - -#endif // CARTOGRAPHER_COMMON_UTILS_H_ diff --git a/cartographer/io/fake_file_writer_test.cc b/cartographer/io/fake_file_writer_test.cc index 6189b92135..711e9164c2 100644 --- a/cartographer/io/fake_file_writer_test.cc +++ b/cartographer/io/fake_file_writer_test.cc @@ -14,9 +14,10 @@ * limitations under the License. */ +#include "cartographer/io/fake_file_writer.h" + #include -#include "cartographer/io/fake_file_writer.h" #include "glog/logging.h" #include "gtest/gtest.h" diff --git a/cartographer/io/hybrid_grid_points_processor.cc b/cartographer/io/hybrid_grid_points_processor.cc index 32525fb473..c81c1d5043 100644 --- a/cartographer/io/hybrid_grid_points_processor.cc +++ b/cartographer/io/hybrid_grid_points_processor.cc @@ -39,8 +39,9 @@ HybridGridPointsProcessor::FromDictionary( } void HybridGridPointsProcessor::Process(std::unique_ptr batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &hybrid_grid_); + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); next_->Process(std::move(batch)); } diff --git a/cartographer/io/hybrid_grid_points_processor.h b/cartographer/io/hybrid_grid_points_processor.h index 0b598b92d8..8124e4087f 100644 --- a/cartographer/io/hybrid_grid_points_processor.h +++ b/cartographer/io/hybrid_grid_points_processor.h @@ -11,7 +11,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" -#include "cartographer/mapping/proto/3d/range_data_inserter_options_3d.pb.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" namespace cartographer { namespace io { diff --git a/cartographer/io/internal/in_memory_proto_stream_test.cc b/cartographer/io/internal/in_memory_proto_stream_test.cc index 9dd3a1e83c..e274472aaf 100644 --- a/cartographer/io/internal/in_memory_proto_stream_test.cc +++ b/cartographer/io/internal/in_memory_proto_stream_test.cc @@ -15,9 +15,9 @@ */ #include "cartographer/io/internal/in_memory_proto_stream.h" + #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" - #include "gtest/gtest.h" namespace cartographer { diff --git a/cartographer/io/internal/mapping_state_serialization.cc b/cartographer/io/internal/mapping_state_serialization.cc index a34c8678a9..8305e7ade3 100644 --- a/cartographer/io/internal/mapping_state_serialization.cc +++ b/cartographer/io/internal/mapping_state_serialization.cc @@ -15,6 +15,7 @@ */ #include "cartographer/io/internal/mapping_state_serialization.h" + #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/io/internal/pbstream_migrate.cc b/cartographer/io/internal/pbstream_migrate.cc index a84af06656..43d6b8a4c9 100644 --- a/cartographer/io/internal/pbstream_migrate.cc +++ b/cartographer/io/internal/pbstream_migrate.cc @@ -21,19 +21,18 @@ #include "gflags/gflags.h" #include "glog/logging.h" -DEFINE_bool(migrate_grid_format, false, - "Set if the submap data of the input pbstream uses the old " - "probability grid format."); +DEFINE_bool(include_unfinished_submaps, true, + "Whether to include unfinished submaps in the output."); namespace cartographer { namespace io { int pbstream_migrate(int argc, char** argv) { std::stringstream ss; - ss << "\n\nTool for migrating files that use the serialization output of " - "Cartographer 0.3, to the new serialization format, which includes a " - "header (Version 1). You may need to specify the '--migrate_grid_format" - " flag if the input file contains submaps with the legacy grid format." + ss << "\n\nTool for migrating files that use submaps without histograms " + "to the new submap format, which includes a histogram. You can " + "set --include_unfinished_submaps to false if you want to exclude " + "unfinished submaps in the output." << "\nUsage: " << argv[0] << " " << argv[1] << " [flags]"; google::SetUsageMessage(ss.str()); @@ -44,10 +43,10 @@ int pbstream_migrate(int argc, char** argv) { } cartographer::io::ProtoStreamReader input(argv[2]); cartographer::io::ProtoStreamWriter output(argv[3]); - LOG(INFO) << "Migrating old serialization format in \"" << argv[2] - << "\" to new serialization format in \"" << argv[3] << "\""; - cartographer::io::MigrateStreamFormatToVersion1(&input, &output, - FLAGS_migrate_grid_format); + LOG(INFO) << "Migrating serialization format 1 in \"" << argv[2] + << "\" to serialization format 2 in \"" << argv[3] << "\""; + cartographer::io::MigrateStreamVersion1ToVersion2( + &input, &output, FLAGS_include_unfinished_submaps); CHECK(output.Close()) << "Could not write migrated pbstream file to: " << argv[3]; diff --git a/cartographer/io/testing/test_helpers.cc b/cartographer/io/internal/testing/test_helpers.cc similarity index 96% rename from cartographer/io/testing/test_helpers.cc rename to cartographer/io/internal/testing/test_helpers.cc index 1e9fbab911..96a56d5bc7 100644 --- a/cartographer/io/testing/test_helpers.cc +++ b/cartographer/io/internal/testing/test_helpers.cc @@ -14,7 +14,8 @@ * limitations under the License. */ -#include "cartographer/io/testing/test_helpers.h" +#include "cartographer/io/internal/testing/test_helpers.h" + #include "cartographer/mapping/proto/serialization.pb.h" namespace cartographer { diff --git a/cartographer/io/testing/test_helpers.h b/cartographer/io/internal/testing/test_helpers.h similarity index 100% rename from cartographer/io/testing/test_helpers.h rename to cartographer/io/internal/testing/test_helpers.h diff --git a/cartographer/io/min_max_range_filtering_points_processor.cc b/cartographer/io/min_max_range_filtering_points_processor.cc index c487d6de9a..ba5fc79878 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.cc +++ b/cartographer/io/min_max_range_filtering_points_processor.cc @@ -23,25 +23,29 @@ namespace cartographer { namespace io { -std::unique_ptr -MinMaxRangeFiteringPointsProcessor::FromDictionary( +std::unique_ptr +MinMaxRangeFilteringPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { - return absl::make_unique( + return absl::make_unique( dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), next); } -MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( +MinMaxRangeFilteringPointsProcessor::MinMaxRangeFilteringPointsProcessor( const double min_range, const double max_range, PointsProcessor* next) - : min_range_(min_range), max_range_(max_range), next_(next) {} + : min_range_squared_(min_range * min_range), + max_range_squared_(max_range * max_range), + next_(next) {} -void MinMaxRangeFiteringPointsProcessor::Process( +void MinMaxRangeFilteringPointsProcessor::Process( std::unique_ptr batch) { absl::flat_hash_set to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { - const float range = (batch->points[i].position - batch->origin).norm(); - if (!(min_range_ <= range && range <= max_range_)) { + const float range_squared = + (batch->points[i].position - batch->origin).squaredNorm(); + if (!(min_range_squared_ <= range_squared && + range_squared <= max_range_squared_)) { to_remove.insert(i); } } @@ -49,7 +53,7 @@ void MinMaxRangeFiteringPointsProcessor::Process( next_->Process(std::move(batch)); } -PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() { +PointsProcessor::FlushResult MinMaxRangeFilteringPointsProcessor::Flush() { return next_->Flush(); } diff --git a/cartographer/io/min_max_range_filtering_points_processor.h b/cartographer/io/min_max_range_filtering_points_processor.h index 2219cb61ba..0e90b3eee4 100644 --- a/cartographer/io/min_max_range_filtering_points_processor.h +++ b/cartographer/io/min_max_range_filtering_points_processor.h @@ -27,28 +27,28 @@ namespace io { // Filters all points that are farther away from their 'origin' as 'max_range' // or closer than 'min_range'. -class MinMaxRangeFiteringPointsProcessor : public PointsProcessor { +class MinMaxRangeFilteringPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "min_max_range_filter"; - MinMaxRangeFiteringPointsProcessor(double min_range, double max_range, - PointsProcessor* next); - static std::unique_ptr FromDictionary( + MinMaxRangeFilteringPointsProcessor(double min_range, double max_range, + PointsProcessor* next); + static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); - ~MinMaxRangeFiteringPointsProcessor() override {} + ~MinMaxRangeFilteringPointsProcessor() override {} - MinMaxRangeFiteringPointsProcessor( - const MinMaxRangeFiteringPointsProcessor&) = delete; - MinMaxRangeFiteringPointsProcessor& operator=( - const MinMaxRangeFiteringPointsProcessor&) = delete; + MinMaxRangeFilteringPointsProcessor( + const MinMaxRangeFilteringPointsProcessor&) = delete; + MinMaxRangeFilteringPointsProcessor& operator=( + const MinMaxRangeFilteringPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: - const double min_range_; - const double max_range_; + const double min_range_squared_; + const double max_range_squared_; PointsProcessor* const next_; }; diff --git a/cartographer/io/pbstream_main.cc b/cartographer/io/pbstream_main.cc index 0a795ff9d7..4fffbd4052 100644 --- a/cartographer/io/pbstream_main.cc +++ b/cartographer/io/pbstream_main.cc @@ -30,12 +30,12 @@ int main(int argc, char** argv) { "Swiss Army knife for pbstreams.\n\n" "Currently supported subcommands are:\n" "\tinfo - Prints summary of pbstream.\n" - "\tmigrate - Migrates old pbstream (w/o header) to new pbstream format."; + "\tmigrate - Migrates pbstream to the new submap format."; google::ParseCommandLineFlags(&argc, &argv, true); if (argc < 2) { google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } else if (std::string(argv[1]) == "info") { return ::cartographer::io::pbstream_info(argc, argv); @@ -44,7 +44,7 @@ int main(int argc, char** argv) { } else { LOG(INFO) << "Unknown subtool: \"" << argv[1]; google::SetUsageMessage(usage_message); - google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_info_main"); + google::ShowUsageWithFlagsRestrict(argv[0], "pbstream_main"); return EXIT_FAILURE; } } diff --git a/cartographer/io/ply_writing_points_processor.cc b/cartographer/io/ply_writing_points_processor.cc index 6c13d23c47..8b0ecdbafb 100644 --- a/cartographer/io/ply_writing_points_processor.cc +++ b/cartographer/io/ply_writing_points_processor.cc @@ -33,6 +33,7 @@ namespace { // Writes the PLY header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities, + const std::vector& comments, const int64 num_points, FileWriter* const file_writer) { const std::string color_header = !has_color ? "" @@ -44,8 +45,11 @@ void WriteBinaryPlyHeader(const bool has_color, const bool has_intensities, std::ostringstream stream; stream << "ply\n" << "format binary_little_endian 1.0\n" - << "comment generated by Cartographer\n" - << "element vertex " << std::setw(15) << std::setfill('0') + << "comment generated by Cartographer\n"; + for (const std::string& comment : comments) { + stream << "comment " << comment << "\n"; + } + stream << "element vertex " << std::setw(15) << std::setfill('0') << num_points << "\n" << "property float x\n" << "property float y\n" @@ -86,18 +90,22 @@ PlyWritingPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return absl::make_unique( - file_writer_factory(dictionary->GetString("filename")), next); + file_writer_factory(dictionary->GetString("filename")), + std::vector(), next); } PlyWritingPointsProcessor::PlyWritingPointsProcessor( - std::unique_ptr file_writer, PointsProcessor* const next) + std::unique_ptr file_writer, + const std::vector& comments, PointsProcessor* const next) : next_(next), + comments_(comments), num_points_(0), has_colors_(false), file_(std::move(file_writer)) {} PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { - WriteBinaryPlyHeader(has_colors_, has_intensities_, num_points_, file_.get()); + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, num_points_, + file_.get()); CHECK(file_->Close()) << "Closing PLY file_writer failed."; switch (next_->Flush()) { @@ -120,7 +128,8 @@ void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); has_intensities_ = !batch->intensities.empty(); - WriteBinaryPlyHeader(has_colors_, has_intensities_, 0, file_.get()); + WriteBinaryPlyHeader(has_colors_, has_intensities_, comments_, 0, + file_.get()); } if (has_colors_) { CHECK_EQ(batch->points.size(), batch->colors.size()) diff --git a/cartographer/io/ply_writing_points_processor.h b/cartographer/io/ply_writing_points_processor.h index 8f07de9276..76295870b3 100644 --- a/cartographer/io/ply_writing_points_processor.h +++ b/cartographer/io/ply_writing_points_processor.h @@ -26,6 +26,7 @@ class PlyWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_ply"; PlyWritingPointsProcessor(std::unique_ptr file_writer, + const std::vector& comments, PointsProcessor* next); static std::unique_ptr FromDictionary( @@ -44,6 +45,7 @@ class PlyWritingPointsProcessor : public PointsProcessor { private: PointsProcessor* const next_; + std::vector comments_; int64 num_points_; bool has_colors_; bool has_intensities_; diff --git a/cartographer/io/points_batch.cc b/cartographer/io/points_batch.cc index ea0fe4ca16..cb4848c029 100644 --- a/cartographer/io/points_batch.cc +++ b/cartographer/io/points_batch.cc @@ -21,7 +21,7 @@ namespace io { void RemovePoints(absl::flat_hash_set to_remove, PointsBatch* batch) { const int new_num_points = batch->points.size() - to_remove.size(); - sensor::PointCloud points; + std::vector points; points.reserve(new_num_points); std::vector intensities; if (!batch->intensities.empty()) { diff --git a/cartographer/io/points_batch.h b/cartographer/io/points_batch.h index 752966ecfd..cc1d6bc42e 100644 --- a/cartographer/io/points_batch.h +++ b/cartographer/io/points_batch.h @@ -25,7 +25,7 @@ #include "absl/container/flat_hash_set.h" #include "cartographer/common/time.h" #include "cartographer/io/color.h" -#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/rangefinder_point.h" namespace cartographer { namespace io { @@ -53,7 +53,7 @@ struct PointsBatch { int trajectory_id; // Geometry of the points in the map frame. - sensor::PointCloud points; + std::vector points; // Intensities are optional and may be unspecified. The meaning of these // intensity values varies by device. For example, the VLP16 provides values diff --git a/cartographer/io/points_processor_pipeline_builder.cc b/cartographer/io/points_processor_pipeline_builder.cc index 23c9232e4f..6592245b1e 100644 --- a/cartographer/io/points_processor_pipeline_builder.cc +++ b/cartographer/io/points_processor_pipeline_builder.cc @@ -29,6 +29,7 @@ #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/probability_grid_points_processor.h" +#include "cartographer/io/vertical_range_filtering_points_processor.h" #include "cartographer/io/xray_points_processor.h" #include "cartographer/io/xyz_writing_points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -83,7 +84,8 @@ void RegisterBuiltInPointsProcessors( RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); - RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); + RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); diff --git a/cartographer/io/probability_grid_points_processor.cc b/cartographer/io/probability_grid_points_processor.cc index 1d746f9574..907b995f29 100644 --- a/cartographer/io/probability_grid_points_processor.cc +++ b/cartographer/io/probability_grid_points_processor.cc @@ -126,8 +126,9 @@ ProbabilityGridPointsProcessor::FromDictionary( void ProbabilityGridPointsProcessor::Process( std::unique_ptr batch) { - range_data_inserter_.Insert({batch->origin, batch->points, {}}, - &probability_grid_); + range_data_inserter_.Insert( + {batch->origin, sensor::PointCloud(batch->points), {}}, + &probability_grid_); next_->Process(std::move(batch)); } diff --git a/cartographer/io/probability_grid_points_processor.h b/cartographer/io/probability_grid_points_processor.h index fe7e74f77c..2114bf8d0c 100644 --- a/cartographer/io/probability_grid_points_processor.h +++ b/cartographer/io/probability_grid_points_processor.h @@ -25,7 +25,7 @@ #include "cartographer/io/points_processor.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping/value_conversion_tables.h" diff --git a/cartographer/io/probability_grid_points_processor_test.cc b/cartographer/io/probability_grid_points_processor_test.cc index b3fc73f2c2..1af0a25b94 100644 --- a/cartographer/io/probability_grid_points_processor_test.cc +++ b/cartographer/io/probability_grid_points_processor_test.cc @@ -17,8 +17,9 @@ #include "cartographer/io/probability_grid_points_processor.h" #include + +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" #include "cartographer/io/fake_file_writer.h" #include "cartographer/io/points_processor_pipeline_builder.h" @@ -84,8 +85,9 @@ std::vector CreateExpectedProbabilityGrid( mapping::ValueConversionTables conversion_tables; auto probability_grid = CreateProbabilityGrid( probability_grid_options->GetDouble("resolution"), &conversion_tables); - range_data_inserter.Insert({points_batch->origin, points_batch->points, {}}, - &probability_grid); + range_data_inserter.Insert( + {points_batch->origin, sensor::PointCloud(points_batch->points), {}}, + &probability_grid); std::vector probability_grid_proto( probability_grid.ToProto().ByteSize()); diff --git a/cartographer/io/proto_stream.cc b/cartographer/io/proto_stream.cc index f55a3a6ebc..60ea02191b 100644 --- a/cartographer/io/proto_stream.cc +++ b/cartographer/io/proto_stream.cc @@ -15,6 +15,7 @@ */ #include "cartographer/io/proto_stream.h" + #include "glog/logging.h" namespace cartographer { diff --git a/cartographer/io/proto_stream_deserializer_test.cc b/cartographer/io/proto_stream_deserializer_test.cc index 44e5a0b6e6..90797991c9 100644 --- a/cartographer/io/proto_stream_deserializer_test.cc +++ b/cartographer/io/proto_stream_deserializer_test.cc @@ -14,11 +14,12 @@ * limitations under the License. */ +#include "cartographer/io/proto_stream_deserializer.h" + #include #include "cartographer/io/internal/in_memory_proto_stream.h" -#include "cartographer/io/proto_stream_deserializer.h" -#include "cartographer/io/testing/test_helpers.h" +#include "cartographer/io/internal/testing/test_helpers.h" #include "glog/logging.h" #include "gmock/gmock.h" #include "google/protobuf/text_format.h" diff --git a/cartographer/io/serialization_format_migration.cc b/cartographer/io/serialization_format_migration.cc index 186fb92aea..7ed692e36f 100644 --- a/cartographer/io/serialization_format_migration.cc +++ b/cartographer/io/serialization_format_migration.cc @@ -18,284 +18,197 @@ #include -#include "absl/container/flat_hash_map.h" +#include "cartographer/common/config.h" +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/thread_pool.h" +#include "cartographer/io/internal/mapping_state_serialization.h" +#include "cartographer/io/proto_stream_deserializer.h" #include "cartographer/mapping/3d/submap_3d.h" +#include "cartographer/mapping/id.h" +#include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" +#include "cartographer/mapping/map_builder_interface.h" +#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" -#include "cartographer/mapping/proto/internal/legacy_submap.pb.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "glog/logging.h" namespace cartographer { namespace io { -namespace { using mapping::proto::SerializedData; -using ProtoMap = absl::flat_hash_map>; -bool ReadPoseGraph(cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - auto& pose_graph_vec = (*proto_map)[SerializedData::kPoseGraph]; - pose_graph_vec.emplace_back(); - return input->ReadProto(pose_graph_vec.back().mutable_pose_graph()); -} - -bool ReadBuilderOptions( +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - auto& options_vec = - (*proto_map)[SerializedData::kAllTrajectoryBuilderOptions]; - options_vec.emplace_back(); - return input->ReadProto( - options_vec.back().mutable_all_trajectory_builder_options()); -} - -mapping::proto::Submap MigrateLegacySubmap2d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap2D submap_2d; - - // Convert probability grid to generalized grid. - *submap_2d.mutable_grid()->mutable_limits() = - submap_in.submap_2d().probability_grid().limits(); - *submap_2d.mutable_grid()->mutable_cells() = - submap_in.submap_2d().probability_grid().cells(); - for (auto& cell : *submap_2d.mutable_grid()->mutable_cells()) { - cell = -1 * cell; - } - // CellBox can't be trivially copied because the protobuf - // serialization field number doesn't match. - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_x( - submap_in.submap_2d().probability_grid().known_cells_box().max_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_max_y( - submap_in.submap_2d().probability_grid().known_cells_box().max_y()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_x( - submap_in.submap_2d().probability_grid().known_cells_box().min_x()); - submap_2d.mutable_grid()->mutable_known_cells_box()->set_min_y( - submap_in.submap_2d().probability_grid().known_cells_box().min_y()); + cartographer::io::ProtoStreamWriterInterface* const output, + bool include_unfinished_submaps) { + auto file_resolver = ::absl::make_unique( + std::vector{std::string(common::kSourceDirectory) + + "/configuration_files"}); + const std::string kCode = R"text( + include "map_builder.lua" + MAP_BUILDER.use_trajectory_builder_3d = true + return MAP_BUILDER)text"; + cartographer::common::LuaParameterDictionary lua_parameter_dictionary( + kCode, std::move(file_resolver)); + const auto options = + mapping::CreateMapBuilderOptions(&lua_parameter_dictionary); - // Correspondence costs can be safely set to standard values. - // Otherwise, this would be done during the next deserialization, but with a - // warning, which we can avoid by setting it already here. - submap_2d.mutable_grid()->set_max_correspondence_cost( - mapping::kMaxCorrespondenceCost); - submap_2d.mutable_grid()->set_min_correspondence_cost( - mapping::kMinCorrespondenceCost); - submap_2d.mutable_grid()->mutable_probability_grid_2d(); - *submap_2d.mutable_local_pose() = submap_in.submap_2d().local_pose(); - submap_2d.set_num_range_data(submap_in.submap_2d().num_range_data()); - submap_2d.set_finished(submap_in.submap_2d().finished()); + common::ThreadPool thread_pool(1); + CHECK(!options.use_trajectory_builder_2d()); + // We always use 3D here. 2D submaps do not have histograms. + mapping::PoseGraph3D pose_graph( + options.pose_graph_options(), + absl::make_unique( + options.pose_graph_options().optimization_problem_options()), + &thread_pool); - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_2d() = submap_2d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} + ProtoStreamDeserializer deserializer(input); -mapping::proto::Submap MigrateLegacySubmap3d( - const mapping::proto::LegacySubmap& submap_in) { - mapping::proto::Submap3D submap_3d; - *submap_3d.mutable_local_pose() = submap_in.submap_3d().local_pose(); - submap_3d.set_num_range_data(submap_in.submap_3d().num_range_data()); - submap_3d.set_finished(submap_in.submap_3d().finished()); - *submap_3d.mutable_high_resolution_hybrid_grid() = - submap_in.submap_3d().high_resolution_hybrid_grid(); - *submap_3d.mutable_low_resolution_hybrid_grid() = - submap_in.submap_3d().low_resolution_hybrid_grid(); + // Create a copy of the pose_graph_proto, such that we can re-write the + // trajectory ids. + mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph(); + const auto& all_builder_options_proto = + deserializer.all_trajectory_builder_options(); - mapping::proto::Submap submap_out; - *submap_out.mutable_submap_3d() = submap_3d; - *submap_out.mutable_submap_id() = submap_in.submap_id(); - return submap_out; -} + std::vector + trajectory_builder_options; + for (int i = 0; i < pose_graph_proto.trajectory_size(); ++i) { + auto& trajectory_proto = *pose_graph_proto.mutable_trajectory(i); + const auto& options_with_sensor_ids_proto = + all_builder_options_proto.options_with_sensor_ids(i); + trajectory_builder_options.push_back(options_with_sensor_ids_proto); + CHECK_EQ(trajectory_proto.trajectory_id(), i); + } -bool DeserializeNext(cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - mapping::proto::LegacySerializedData legacy_data; - if (!input->ReadProto(&legacy_data)) return false; + // Apply the calculated remapping to constraints in the pose graph proto. + for (auto& constraint_proto : *pose_graph_proto.mutable_constraint()) { + constraint_proto.mutable_submap_id()->set_trajectory_id( + constraint_proto.submap_id().trajectory_id()); + constraint_proto.mutable_node_id()->set_trajectory_id( + constraint_proto.node_id().trajectory_id()); + } - if (legacy_data.has_submap()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().grid().has_probability_grid_2d() || - legacy_data.submap().submap_2d().grid().has_tsdf_2d()) - << "\nThe legacy data contains a 2D submap, but it's not using the " - "expected grid format. Try to migrate the grid format instead."; + mapping::MapById submap_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Submap& submap_proto : + trajectory_proto.submap()) { + submap_poses.Insert(mapping::SubmapId{trajectory_proto.trajectory_id(), + submap_proto.submap_index()}, + transform::ToRigid3(submap_proto.pose())); } - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_submap() = legacy_data.submap(); - } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_node() = legacy_data.node(); } - if (legacy_data.has_trajectory_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating trajectory data."; - auto& output_vector = - (*proto_map)[SerializedData::kTrajectoryDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_trajectory_data() = - legacy_data.trajectory_data(); - } - if (legacy_data.has_imu_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating IMU data."; - auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_imu_data() = legacy_data.imu_data(); - } - if (legacy_data.has_odometry_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating odometry data."; - auto& output_vector = (*proto_map)[SerializedData::kOdometryData]; - output_vector.emplace_back(); - *output_vector.back().mutable_odometry_data() = legacy_data.odometry_data(); - } - if (legacy_data.has_fixed_frame_pose_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating fixed frame pose data."; - auto& output_vector = - (*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_fixed_frame_pose_data() = - legacy_data.fixed_frame_pose_data(); - } - if (legacy_data.has_landmark_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating landmark data."; - auto& output_vector = - (*proto_map)[SerializedData::kLandmarkDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_landmark_data() = legacy_data.landmark_data(); - } - return true; -} -bool DeserializeNextAndMigrateGridFormat( - cartographer::io::ProtoStreamReaderInterface* const input, - ProtoMap* proto_map) { - mapping::proto::LegacySerializedDataLegacySubmap legacy_data; - if (!input->ReadProto(&legacy_data)) return false; - - if (legacy_data.has_submap()) { - LOG_FIRST_N(INFO, 1) << "Migrating submap data."; - auto& output_vector = (*proto_map)[SerializedData::kSubmapFieldNumber]; - output_vector.emplace_back(); - if (legacy_data.submap().has_submap_2d()) { - CHECK(legacy_data.submap().submap_2d().has_probability_grid()) - << "\nThe legacy data contains a 2D submap, but it has no legacy " - "probability grid that can be migrated to the new grid format."; - *output_vector.back().mutable_submap() = - MigrateLegacySubmap2d(legacy_data.submap()); - } else { - *output_vector.back().mutable_submap() = - MigrateLegacySubmap3d(legacy_data.submap()); + mapping::MapById node_poses; + for (const mapping::proto::Trajectory& trajectory_proto : + pose_graph_proto.trajectory()) { + for (const mapping::proto::Trajectory::Node& node_proto : + trajectory_proto.node()) { + node_poses.Insert(mapping::NodeId{trajectory_proto.trajectory_id(), + node_proto.node_index()}, + transform::ToRigid3(node_proto.pose())); } } - if (legacy_data.has_node()) { - LOG_FIRST_N(INFO, 1) << "Migrating node data."; - auto& output_vector = (*proto_map)[SerializedData::kNodeFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_node() = legacy_data.node(); - } - if (legacy_data.has_trajectory_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating trajectory data."; - auto& output_vector = - (*proto_map)[SerializedData::kTrajectoryDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_trajectory_data() = - legacy_data.trajectory_data(); - } - if (legacy_data.has_imu_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating IMU data."; - auto& output_vector = (*proto_map)[SerializedData::kImuDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_imu_data() = legacy_data.imu_data(); - } - if (legacy_data.has_odometry_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating odometry data."; - auto& output_vector = (*proto_map)[SerializedData::kOdometryData]; - output_vector.emplace_back(); - *output_vector.back().mutable_odometry_data() = legacy_data.odometry_data(); - } - if (legacy_data.has_fixed_frame_pose_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating fixed frame pose data."; - auto& output_vector = - (*proto_map)[SerializedData::kFixedFramePoseDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_fixed_frame_pose_data() = - legacy_data.fixed_frame_pose_data(); - } - if (legacy_data.has_landmark_data()) { - LOG_FIRST_N(INFO, 1) << "Migrating landmark data."; - auto& output_vector = - (*proto_map)[SerializedData::kLandmarkDataFieldNumber]; - output_vector.emplace_back(); - *output_vector.back().mutable_landmark_data() = legacy_data.landmark_data(); - } - return true; -} -ProtoMap ParseLegacyData( - cartographer::io::ProtoStreamReaderInterface* const input, - bool migrate_grid_format) { - ProtoMap proto_map; - CHECK(ReadPoseGraph(input, &proto_map)) - << "Input stream seems to differ from original stream format. Could " - "not " - "read PoseGraph as first message."; - CHECK(ReadBuilderOptions(input, &proto_map)) - << "Input stream seems to differ from original stream format. Could " - "not " - "read AllTrajectoryBuilderOptions as second message."; - if (migrate_grid_format) { - do { - } while (DeserializeNextAndMigrateGridFormat(input, &proto_map)); - } else { - do { - } while (DeserializeNext(input, &proto_map)); + // Set global poses of landmarks. + for (const auto& landmark : pose_graph_proto.landmark_poses()) { + pose_graph.SetLandmarkPose(landmark.landmark_id(), + transform::ToRigid3(landmark.global_pose()), + true); } - return proto_map; -} -mapping::proto::SerializationHeader CreateSerializationHeader() { - constexpr uint32_t kVersion1 = 1; - mapping::proto::SerializationHeader header; - header.set_format_version(kVersion1); - return header; -} + mapping::MapById + submap_id_to_submap; + mapping::MapById node_id_to_node; + SerializedData proto; + while (deserializer.ReadNextSerializedData(&proto)) { + switch (proto.data_case()) { + case SerializedData::kPoseGraph: + LOG(FATAL) << "Found multiple serialized `PoseGraph`. Serialized " + "stream likely corrupt!."; + case SerializedData::kAllTrajectoryBuilderOptions: + LOG(FATAL) << "Found multiple serialized " + "`AllTrajectoryBuilderOptions`. Serialized stream likely " + "corrupt!."; + case SerializedData::kSubmap: { + CHECK(proto.submap().has_submap_3d()) + << "Converting to the new submap format only makes sense for 3D."; + proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( + proto.submap().submap_id().trajectory_id()); + submap_id_to_submap.Insert( + mapping::SubmapId{proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()}, + proto.submap()); + break; + } + case SerializedData::kNode: { + proto.mutable_node()->mutable_node_id()->set_trajectory_id( + proto.node().node_id().trajectory_id()); + const mapping::NodeId node_id(proto.node().node_id().trajectory_id(), + proto.node().node_id().node_index()); + const transform::Rigid3d& node_pose = node_poses.at(node_id); + pose_graph.AddNodeFromProto(node_pose, proto.node()); + node_id_to_node.Insert(node_id, proto.node()); + break; + } + case SerializedData::kTrajectoryData: { + proto.mutable_trajectory_data()->set_trajectory_id( + proto.trajectory_data().trajectory_id()); + pose_graph.SetTrajectoryDataFromProto(proto.trajectory_data()); + break; + } + case SerializedData::kImuData: { + pose_graph.AddImuData(proto.imu_data().trajectory_id(), + sensor::FromProto(proto.imu_data().imu_data())); + break; + } + case SerializedData::kOdometryData: { + pose_graph.AddOdometryData( + proto.odometry_data().trajectory_id(), + sensor::FromProto(proto.odometry_data().odometry_data())); + break; + } + case SerializedData::kFixedFramePoseData: { + pose_graph.AddFixedFramePoseData( + proto.fixed_frame_pose_data().trajectory_id(), + sensor::FromProto( + proto.fixed_frame_pose_data().fixed_frame_pose_data())); + break; + } + case SerializedData::kLandmarkData: { + pose_graph.AddLandmarkData( + proto.landmark_data().trajectory_id(), + sensor::FromProto(proto.landmark_data().landmark_data())); + break; + } + default: + LOG(WARNING) << "Skipping unknown message type in stream: " + << proto.GetTypeName(); + } + } -void SerializeToVersion1Format( - const ProtoMap& deserialized_data, - cartographer::io::ProtoStreamWriterInterface* const output) { - const std::vector kFieldSerializationOrder = { - SerializedData::kPoseGraphFieldNumber, - SerializedData::kAllTrajectoryBuilderOptionsFieldNumber, - SerializedData::kSubmapFieldNumber, - SerializedData::kNodeFieldNumber, - SerializedData::kTrajectoryDataFieldNumber, - SerializedData::kImuDataFieldNumber, - SerializedData::kOdometryDataFieldNumber, - SerializedData::kFixedFramePoseDataFieldNumber, - SerializedData::kLandmarkDataFieldNumber}; + // TODO(schwoere): Remove backwards compatibility once the pbstream format + // version 2 is established. + if (deserializer.header().format_version() == + kFormatVersionWithoutSubmapHistograms) { + submap_id_to_submap = MigrateSubmapFormatVersion1ToVersion2( + submap_id_to_submap, node_id_to_node, pose_graph_proto); + } - LOG(INFO) << "Writing proto stream."; - output->WriteProto(CreateSerializationHeader()); - for (auto field_index : kFieldSerializationOrder) { - const auto proto_vector_it = deserialized_data.find(field_index); - if (proto_vector_it == deserialized_data.end()) continue; - for (const auto& proto : proto_vector_it->second) { - output->WriteProto(proto); - } + for (const auto& submap_id_submap : submap_id_to_submap) { + pose_graph.AddSubmapFromProto(submap_poses.at(submap_id_submap.id), + submap_id_submap.data); } -} -} // namespace -void MigrateStreamFormatToVersion1( - cartographer::io::ProtoStreamReaderInterface* const input, - cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format) { - SerializeToVersion1Format(ParseLegacyData(input, migrate_grid_format), - output); + pose_graph.AddSerializedConstraints( + mapping::FromProto(pose_graph_proto.constraint())); + CHECK(input->eof()); + + WritePbStream(pose_graph, trajectory_builder_options, output, + include_unfinished_submaps); } mapping::MapById diff --git a/cartographer/io/serialization_format_migration.h b/cartographer/io/serialization_format_migration.h index 3570a8ac0f..32e101dd51 100644 --- a/cartographer/io/serialization_format_migration.h +++ b/cartographer/io/serialization_format_migration.h @@ -24,14 +24,13 @@ namespace cartographer { namespace io { -// This helper function, migrates the input stream, which is supposed to match -// to the "old" stream format order (PoseGraph, AllTrajectoryBuilderOptions, -// SerializedData*) to the version 1 stream format (SerializationHeader, -// SerializedData*). -void MigrateStreamFormatToVersion1( +// This helper function migrates the input stream, which is supposed +// to contain submaps without histograms (stream format version 1) to +// an output stream containing submaps with histograms (version 2). +void MigrateStreamVersion1ToVersion2( cartographer::io::ProtoStreamReaderInterface* const input, cartographer::io::ProtoStreamWriterInterface* const output, - bool migrate_grid_format); + bool include_unfinished_submaps); mapping::MapById MigrateSubmapFormatVersion1ToVersion2( diff --git a/cartographer/io/serialization_format_migration_test.cc b/cartographer/io/serialization_format_migration_test.cc index 24904ca63a..19e3535754 100644 --- a/cartographer/io/serialization_format_migration_test.cc +++ b/cartographer/io/serialization_format_migration_test.cc @@ -21,7 +21,6 @@ #include "cartographer/io/internal/in_memory_proto_stream.h" #include "cartographer/mapping/internal/testing/test_helpers.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" #include "cartographer/mapping/proto/pose_graph.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" @@ -35,74 +34,6 @@ namespace cartographer { namespace io { namespace { -using ::google::protobuf::TextFormat; -using ::testing::Eq; -using ::testing::SizeIs; - -class MigrationTest : public ::testing::Test { - private: - template - void AddLegacyDataToReader(InMemoryProtoStreamReader& reader) { - mapping::proto::PoseGraph pose_graph; - mapping::proto::AllTrajectoryBuilderOptions all_options; - LegacySerializedDataType submap; - submap.mutable_submap(); - LegacySerializedDataType node; - node.mutable_node(); - LegacySerializedDataType imu_data; - imu_data.mutable_imu_data(); - LegacySerializedDataType odometry_data; - odometry_data.mutable_odometry_data(); - LegacySerializedDataType fixed_frame_pose; - fixed_frame_pose.mutable_fixed_frame_pose_data(); - LegacySerializedDataType trajectory_data; - trajectory_data.mutable_trajectory_data(); - LegacySerializedDataType landmark_data; - landmark_data.mutable_landmark_data(); - - reader.AddProto(pose_graph); - reader.AddProto(all_options); - reader.AddProto(submap); - reader.AddProto(node); - reader.AddProto(imu_data); - reader.AddProto(odometry_data); - reader.AddProto(fixed_frame_pose); - reader.AddProto(trajectory_data); - reader.AddProto(landmark_data); - } - - protected: - void SetUp() override { - AddLegacyDataToReader(reader_); - AddLegacyDataToReader( - reader_for_migrating_grid_); - - writer_.reset(new ForwardingProtoStreamWriter( - [this](const google::protobuf::Message* proto) -> bool { - std::string msg_string; - TextFormat::PrintToString(*proto, &msg_string); - this->output_messages_.push_back(msg_string); - return true; - })); - writer_for_migrating_grid_.reset(new ForwardingProtoStreamWriter( - [this](const google::protobuf::Message* proto) -> bool { - std::string msg_string; - TextFormat::PrintToString(*proto, &msg_string); - this->output_messages_after_migrating_grid_.push_back(msg_string); - return true; - })); - } - - InMemoryProtoStreamReader reader_; - InMemoryProtoStreamReader reader_for_migrating_grid_; - std::unique_ptr writer_; - std::unique_ptr writer_for_migrating_grid_; - std::vector output_messages_; - std::vector output_messages_after_migrating_grid_; - - static constexpr int kNumOriginalMessages = 9; -}; - class SubmapHistogramMigrationTest : public ::testing::Test { protected: void SetUp() override { @@ -141,81 +72,6 @@ class SubmapHistogramMigrationTest : public ::testing::Test { mapping::proto::Node node_; }; -TEST_F(MigrationTest, MigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - // We expect one message more than the original number of messages, because of - // the added header. - EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); - - mapping::proto::SerializationHeader header; - EXPECT_TRUE(TextFormat::ParseFromString(output_messages_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1)); -} - -TEST_F(MigrationTest, MigrationWithGridMigrationAddsHeaderAsFirstMessage) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - // We expect one message more than the original number of messages, because of - // the added header. - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - mapping::proto::SerializationHeader header; - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[0], &header)); - EXPECT_THAT(header.format_version(), Eq(1)); -} - -TEST_F(MigrationTest, SerializedDataOrderIsCorrect) { - MigrateStreamFormatToVersion1(&reader_, writer_.get(), - false /* migrate_grid_format */); - EXPECT_THAT(output_messages_, SizeIs(kNumOriginalMessages + 1)); - - std::vector serialized( - output_messages_.size() - 1); - for (size_t i = 1; i < output_messages_.size(); ++i) { - EXPECT_TRUE( - TextFormat::ParseFromString(output_messages_[i], &serialized[i - 1])); - } - - EXPECT_TRUE(serialized[0].has_pose_graph()); - EXPECT_TRUE(serialized[1].has_all_trajectory_builder_options()); - EXPECT_TRUE(serialized[2].has_submap()); - EXPECT_TRUE(serialized[3].has_node()); - EXPECT_TRUE(serialized[4].has_trajectory_data()); - EXPECT_TRUE(serialized[5].has_imu_data()); - EXPECT_TRUE(serialized[6].has_odometry_data()); - EXPECT_TRUE(serialized[7].has_fixed_frame_pose_data()); - EXPECT_TRUE(serialized[8].has_landmark_data()); -} - -TEST_F(MigrationTest, SerializedDataOrderAfterGridMigrationIsCorrect) { - MigrateStreamFormatToVersion1(&reader_for_migrating_grid_, - writer_for_migrating_grid_.get(), - true /* migrate_grid_format */); - EXPECT_THAT(output_messages_after_migrating_grid_, - SizeIs(kNumOriginalMessages + 1)); - - std::vector serialized( - output_messages_after_migrating_grid_.size() - 1); - for (size_t i = 1; i < output_messages_after_migrating_grid_.size(); ++i) { - EXPECT_TRUE(TextFormat::ParseFromString( - output_messages_after_migrating_grid_[i], &serialized[i - 1])); - } - - EXPECT_TRUE(serialized[0].has_pose_graph()); - EXPECT_TRUE(serialized[1].has_all_trajectory_builder_options()); - EXPECT_TRUE(serialized[2].has_submap()); - EXPECT_TRUE(serialized[3].has_node()); - EXPECT_TRUE(serialized[4].has_trajectory_data()); - EXPECT_TRUE(serialized[5].has_imu_data()); - EXPECT_TRUE(serialized[6].has_odometry_data()); - EXPECT_TRUE(serialized[7].has_fixed_frame_pose_data()); - EXPECT_TRUE(serialized[8].has_landmark_data()); -} - TEST_F(SubmapHistogramMigrationTest, SubmapHistogramGenerationFromTrajectoryNodes) { mapping::MapById diff --git a/cartographer/io/vertical_range_filtering_points_processor.cc b/cartographer/io/vertical_range_filtering_points_processor.cc new file mode 100644 index 0000000000..325d359cc6 --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.cc @@ -0,0 +1,59 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/io/vertical_range_filtering_points_processor.h" + +#include "absl/memory/memory.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_batch.h" + +namespace cartographer { +namespace io { + +std::unique_ptr +VerticalRangeFilteringPointsProcessor::FromDictionary( + common::LuaParameterDictionary* const dictionary, + PointsProcessor* const next) { + return absl::make_unique( + dictionary->GetDouble("min_z"), dictionary->GetDouble("max_z"), + next); +} + +VerticalRangeFilteringPointsProcessor::VerticalRangeFilteringPointsProcessor( + const double min_z, const double max_z, + PointsProcessor* next) + : min_z_(min_z), max_z_(max_z), + next_(next) {} + +void VerticalRangeFilteringPointsProcessor::Process( + std::unique_ptr batch) { + absl::flat_hash_set to_remove; + for (size_t i = 0; i < batch->points.size(); ++i) { + const float distance_z = batch->points[i].position.z() - batch->origin.z(); + if (!(min_z_ <= distance_z && distance_z <= max_z_) ) { + to_remove.insert(i); + } + } + RemovePoints(to_remove, batch.get()); + next_->Process(std::move(batch)); +} + +PointsProcessor::FlushResult VerticalRangeFilteringPointsProcessor::Flush() { + return next_->Flush(); +} + +} // namespace io +} // namespace cartographer diff --git a/cartographer/io/vertical_range_filtering_points_processor.h b/cartographer/io/vertical_range_filtering_points_processor.h new file mode 100644 index 0000000000..d23eb20bed --- /dev/null +++ b/cartographer/io/vertical_range_filtering_points_processor.h @@ -0,0 +1,58 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ +#define CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ + +#include + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/io/points_processor.h" + +namespace cartographer { +namespace io { + +// Filters all points which distance in the Z direction from their 'origin' +// exceeds 'max_z' or 'min_z'. +class VerticalRangeFilteringPointsProcessor : public PointsProcessor { + public: + constexpr static const char* kConfigurationFileActionName = + "vertical_range_filter"; + VerticalRangeFilteringPointsProcessor(double min_z, double max_z, + PointsProcessor* next); + static std::unique_ptr FromDictionary( + common::LuaParameterDictionary* dictionary, PointsProcessor* next); + + ~VerticalRangeFilteringPointsProcessor() override {} + + VerticalRangeFilteringPointsProcessor( + const VerticalRangeFilteringPointsProcessor&) = delete; + VerticalRangeFilteringPointsProcessor& operator=( + const VerticalRangeFilteringPointsProcessor&) = delete; + + void Process(std::unique_ptr batch) override; + FlushResult Flush() override; + + private: + const double min_z_; + const double max_z_; + PointsProcessor* const next_; +}; + +} // namespace io +} // namespace cartographer + +#endif // CARTOGRAPHER_IO_VERTICAL_RANGE_FILTERING_POINTS_PROCESSOR_H_ diff --git a/cartographer/io/xray_points_processor.cc b/cartographer/io/xray_points_processor.cc index 4612e6692b..282bdda9b8 100644 --- a/cartographer/io/xray_points_processor.cc +++ b/cartographer/io/xray_points_processor.cc @@ -67,7 +67,7 @@ float Mix(const float a, const float b, const float t) { } // Convert 'matrix' into a pleasing-to-look-at image. -Image IntoImage(const PixelDataMatrix& matrix) { +Image IntoImage(const PixelDataMatrix& matrix, double saturation_factor) { Image image(matrix.width(), matrix.height()); float max = std::numeric_limits::min(); for (int y = 0; y < matrix.height(); ++y) { @@ -92,7 +92,8 @@ Image IntoImage(const PixelDataMatrix& matrix) { // basic idea here was that walls (full height) are fully saturated, but // details like chairs and tables are still well visible. const float saturation = - std::log(cell.num_occupied_cells_in_column) / max; + std::min(1.0, std::log(cell.num_occupied_cells_in_column) / + max * saturation_factor); const FloatColor color = {{Mix(1.f, cell.mean_r, saturation), Mix(1.f, cell.mean_g, saturation), Mix(1.f, cell.mean_b, saturation)}}; @@ -115,7 +116,8 @@ bool ContainedIn(const common::Time& time, } // namespace XRayPointsProcessor::XRayPointsProcessor( - const double voxel_size, const transform::Rigid3f& transform, + const double voxel_size, const double saturation_factor, + const transform::Rigid3f& transform, const std::vector& floors, const DrawTrajectories& draw_trajectories, const std::string& output_filename, @@ -127,7 +129,8 @@ XRayPointsProcessor::XRayPointsProcessor( next_(next), floors_(floors), output_filename_(output_filename), - transform_(transform) { + transform_(transform), + saturation_factor_(saturation_factor) { for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { aggregations_.emplace_back( Aggregation{mapping::HybridGridBase(voxel_size), {}}); @@ -146,6 +149,10 @@ std::unique_ptr XRayPointsProcessor::FromDictionary( dictionary->GetBool("draw_trajectories")) ? DrawTrajectories::kYes : DrawTrajectories::kNo; + const double saturation_factor = + dictionary->HasKey("saturation_factor") + ? dictionary->GetDouble("saturation_factor") + : 1.; if (separate_floor) { CHECK_EQ(trajectories.size(), 1) << "Can only detect floors with a single trajectory."; @@ -153,7 +160,7 @@ std::unique_ptr XRayPointsProcessor::FromDictionary( } return absl::make_unique( - dictionary->GetDouble("voxel_size"), + dictionary->GetDouble("voxel_size"), saturation_factor, transform::FromDictionary(dictionary->GetDictionary("transform").get()) .cast(), floors, draw_trajectories, dictionary->GetString("filename"), @@ -193,7 +200,7 @@ void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, ++pixel_data.num_occupied_cells_in_column; } - Image image = IntoImage(pixel_data_matrix); + Image image = IntoImage(pixel_data_matrix, saturation_factor_); if (draw_trajectories_ == DrawTrajectories::kYes) { for (size_t i = 0; i < trajectories_.size(); ++i) { DrawTrajectory( diff --git a/cartographer/io/xray_points_processor.h b/cartographer/io/xray_points_processor.h index 1d269a89a7..8d0f7585b4 100644 --- a/cartographer/io/xray_points_processor.h +++ b/cartographer/io/xray_points_processor.h @@ -38,7 +38,8 @@ class XRayPointsProcessor : public PointsProcessor { "write_xray_image"; enum class DrawTrajectories { kNo, kYes }; XRayPointsProcessor( - double voxel_size, const transform::Rigid3f& transform, + double voxel_size, double saturation_factor, + const transform::Rigid3f& transform, const std::vector& floors, const DrawTrajectories& draw_trajectories, const std::string& output_filename, @@ -90,6 +91,10 @@ class XRayPointsProcessor : public PointsProcessor { // Bounding box containing all cells with data in all 'aggregations_'. Eigen::AlignedBox3i bounding_box_; + + // Scale the saturation of the point color. If saturation_factor_ > 1, the + // point has darker color, otherwise it has lighter color. + const double saturation_factor_; }; } // namespace io diff --git a/cartographer/mapping/2d/grid_2d.h b/cartographer/mapping/2d/grid_2d.h index 527c456487..dfea1e5fee 100644 --- a/cartographer/mapping/2d/grid_2d.h +++ b/cartographer/mapping/2d/grid_2d.h @@ -22,9 +22,9 @@ #include "cartographer/mapping/2d/map_limits.h" #include "cartographer/mapping/grid_interface.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/2d/grid_2d.pb.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" +#include "cartographer/mapping/proto/grid_2d.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/mapping/value_conversion_tables.h" namespace cartographer { diff --git a/cartographer/mapping/2d/map_limits.h b/cartographer/mapping/2d/map_limits.h index 241fc570a7..0ad1f3c88f 100644 --- a/cartographer/mapping/2d/map_limits.h +++ b/cartographer/mapping/2d/map_limits.h @@ -24,7 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/xy_index.h" -#include "cartographer/mapping/proto/2d/map_limits.pb.h" +#include "cartographer/mapping/proto/map_limits.pb.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h index 83a04b8971..6271094213 100644 --- a/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h +++ b/cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h @@ -24,7 +24,7 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/xy_index.h" -#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" diff --git a/cartographer/mapping/2d/range_data_inserter_2d_test.cc b/cartographer/mapping/2d/range_data_inserter_2d_test.cc index 587573dbe4..6b182aa57f 100644 --- a/cartographer/mapping/2d/range_data_inserter_2d_test.cc +++ b/cartographer/mapping/2d/range_data_inserter_2d_test.cc @@ -14,14 +14,13 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" - #include #include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" +#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/mapping/probability_values.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping/2d/submap_2d.cc b/cartographer/mapping/2d/submap_2d.cc index feb30fc22f..a84d48eadd 100644 --- a/cartographer/mapping/2d/submap_2d.cc +++ b/cartographer/mapping/2d/submap_2d.cc @@ -26,7 +26,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/port.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "glog/logging.h" diff --git a/cartographer/mapping/2d/submap_2d.h b/cartographer/mapping/2d/submap_2d.h index 8ea9097718..da30cbbe2d 100644 --- a/cartographer/mapping/2d/submap_2d.h +++ b/cartographer/mapping/2d/submap_2d.h @@ -24,9 +24,9 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" diff --git a/cartographer/mapping/2d/submap_2d_test.cc b/cartographer/mapping/2d/submap_2d_test.cc index 3b9d41aa37..d9b6e21d75 100644 --- a/cartographer/mapping/2d/submap_2d_test.cc +++ b/cartographer/mapping/2d/submap_2d_test.cc @@ -15,16 +15,16 @@ */ #include "cartographer/mapping/2d/submap_2d.h" -#include "cartographer/mapping/2d/probability_grid.h" #include #include #include #include +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" +#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/transform/transform.h" #include "gmock/gmock.h" diff --git a/cartographer/mapping/2d/xy_index.h b/cartographer/mapping/2d/xy_index.h index 64acf274c2..2899e2dacc 100644 --- a/cartographer/mapping/2d/xy_index.h +++ b/cartographer/mapping/2d/xy_index.h @@ -25,7 +25,7 @@ #include "Eigen/Core" #include "cartographer/common/math.h" #include "cartographer/common/port.h" -#include "cartographer/mapping/proto/2d/cell_limits.pb.h" +#include "cartographer/mapping/proto/cell_limits_2d.pb.h" #include "glog/logging.h" namespace cartographer { diff --git a/cartographer/mapping/3d/hybrid_grid.h b/cartographer/mapping/3d/hybrid_grid.h index 10d2a5969a..5b746b5eaf 100644 --- a/cartographer/mapping/3d/hybrid_grid.h +++ b/cartographer/mapping/3d/hybrid_grid.h @@ -28,7 +28,7 @@ #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/probability_values.h" -#include "cartographer/mapping/proto/3d/hybrid_grid.pb.h" +#include "cartographer/mapping/proto/hybrid_grid.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -544,6 +544,32 @@ class HybridGrid : public HybridGridBase { std::vector update_indices_; }; +struct AverageIntensityData { + float sum = 0.f; + int count = 0; +}; + +class IntensityHybridGrid : public HybridGridBase { + public: + explicit IntensityHybridGrid(const float resolution) + : HybridGridBase(resolution) {} + + void AddIntensity(const Eigen::Array3i& index, const float intensity) { + AverageIntensityData* const cell = mutable_value(index); + cell->count += 1; + cell->sum += intensity; + } + + float GetIntensity(const Eigen::Array3i& index) const { + const AverageIntensityData& cell = value(index); + if (cell.count == 0) { + return 0.f; + } else { + return cell.sum / cell.count; + } + } +}; + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/3d/hybrid_grid_test.cc b/cartographer/mapping/3d/hybrid_grid_test.cc index 62309de912..f600032e36 100644 --- a/cartographer/mapping/3d/hybrid_grid_test.cc +++ b/cartographer/mapping/3d/hybrid_grid_test.cc @@ -84,6 +84,23 @@ TEST(HybridGridTest, GetProbability) { } } +TEST(HybridGridTest, GetIntensity) { + IntensityHybridGrid hybrid_grid(1.f); + const Eigen::Array3i cell_index = + hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)); + const float intensity = 58.0f; + + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9); + hybrid_grid.AddIntensity(cell_index, intensity); + EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9); + for (const Eigen::Array3i& index : + {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), + hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { + EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9); + } +} + MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } TEST(HybridGridTest, GetCellIndex) { @@ -212,11 +229,15 @@ struct EigenComparator { TEST_F(RandomHybridGridTest, FromProto) { const HybridGrid constructed_grid(hybrid_grid_.ToProto()); - std::map member_map( - hybrid_grid_.begin(), hybrid_grid_.end()); + std::map member_map; + for (const auto& cell : hybrid_grid_) { + member_map.insert(cell); + } - std::map constructed_map( - constructed_grid.begin(), constructed_grid.end()); + std::map constructed_map; + for (const auto& cell : constructed_grid) { + constructed_map.insert(cell); + } EXPECT_EQ(member_map, constructed_map); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.cc b/cartographer/mapping/3d/range_data_inserter_3d.cc index 104f805dd9..555323755a 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d.cc @@ -51,6 +51,21 @@ void InsertMissesIntoGrid(const std::vector& miss_table, } } +void InsertIntensitiesIntoGrid(const sensor::PointCloud& returns, + IntensityHybridGrid* intensity_hybrid_grid, + const float intensity_threshold) { + if (returns.intensities().size() > 0) { + for (size_t i = 0; i < returns.size(); ++i) { + if (returns.intensities()[i] > intensity_threshold) { + continue; + } + const Eigen::Array3i hit_cell = + intensity_hybrid_grid->GetCellIndex(returns[i].position); + intensity_hybrid_grid->AddIntensity(hit_cell, returns.intensities()[i]); + } + } +} + } // namespace proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( @@ -62,6 +77,8 @@ proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D( parameter_dictionary->GetDouble("miss_probability")); options.set_num_free_space_voxels( parameter_dictionary->GetInt("num_free_space_voxels")); + options.set_intensity_threshold( + parameter_dictionary->GetDouble("intensity_threshold")); CHECK_GT(options.hit_probability(), 0.5); CHECK_LT(options.miss_probability(), 0.5); return options; @@ -75,9 +92,10 @@ RangeDataInserter3D::RangeDataInserter3D( miss_table_( ComputeLookupTableToApplyOdds(Odds(options_.miss_probability()))) {} -void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const { - CHECK(hybrid_grid != nullptr); +void RangeDataInserter3D::Insert( + const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const { + CHECK_NOTNULL(hybrid_grid); for (const sensor::RangefinderPoint& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit.position); @@ -88,6 +106,10 @@ void RangeDataInserter3D::Insert(const sensor::RangeData& range_data, // (i.e. no hits will be ignored because of a miss in the same cell). InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, hybrid_grid, options_.num_free_space_voxels()); + if (intensity_hybrid_grid != nullptr) { + InsertIntensitiesIntoGrid(range_data.returns, intensity_hybrid_grid, + options_.intensity_threshold()); + } hybrid_grid->FinishUpdate(); } diff --git a/cartographer/mapping/3d/range_data_inserter_3d.h b/cartographer/mapping/3d/range_data_inserter_3d.h index b6b0bcd0b5..0dd5a8e789 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d.h +++ b/cartographer/mapping/3d/range_data_inserter_3d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_3D_H_ #include "cartographer/mapping/3d/hybrid_grid.h" -#include "cartographer/mapping/proto/3d/range_data_inserter_options_3d.pb.h" +#include "cartographer/mapping/proto/range_data_inserter_options_3d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" @@ -36,9 +36,10 @@ class RangeDataInserter3D { RangeDataInserter3D(const RangeDataInserter3D&) = delete; RangeDataInserter3D& operator=(const RangeDataInserter3D&) = delete; - // Inserts 'range_data' into 'hybrid_grid'. - void Insert(const sensor::RangeData& range_data, - HybridGrid* hybrid_grid) const; + // Inserts 'range_data' into 'hybrid_grid' and optionally into + // 'intensity_hybrid_grid'. + void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid, + IntensityHybridGrid* intensity_hybrid_grid) const; private: const proto::RangeDataInserterOptions3D options_; diff --git a/cartographer/mapping/3d/range_data_inserter_3d_test.cc b/cartographer/mapping/3d/range_data_inserter_3d_test.cc index eb051f1899..a7f92b31c7 100644 --- a/cartographer/mapping/3d/range_data_inserter_3d_test.cc +++ b/cartographer/mapping/3d/range_data_inserter_3d_test.cc @@ -19,7 +19,7 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { @@ -28,12 +28,13 @@ namespace { class RangeDataInserter3DTest : public ::testing::Test { protected: - RangeDataInserter3DTest() : hybrid_grid_(1.f) { + RangeDataInserter3DTest() : hybrid_grid_(1.f), intensity_hybrid_grid_(1.f) { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 1000, " + "intensity_threshold = 100, " "}"); options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get()); range_data_inserter_.reset(new RangeDataInserter3D(options_)); @@ -41,12 +42,28 @@ class RangeDataInserter3DTest : public ::testing::Test { void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); - sensor::PointCloud returns = {{Eigen::Vector3f{-3.f, -1.f, 4.f}}, - {Eigen::Vector3f{-2.f, 0.f, 4.f}}, - {Eigen::Vector3f{-1.f, 1.f, 4.f}}, - {Eigen::Vector3f{0.f, 2.f, 4.f}}}; - range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, - &hybrid_grid_); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns), {}}, + &hybrid_grid_, + /*intensity_hybrid_grid=*/nullptr); + } + + void InsertPointCloudWithIntensities() { + const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); + const std::vector returns = { + {Eigen::Vector3f{-3.f, -1.f, 4.f}}, + {Eigen::Vector3f{-2.f, 0.f, 4.f}}, + {Eigen::Vector3f{-1.f, 1.f, 4.f}}, + {Eigen::Vector3f{0.f, 2.f, 4.f}}}; + const std::vector intensities{7.f, 8.f, 9.f, 10.f}; + range_data_inserter_->Insert( + sensor::RangeData{origin, sensor::PointCloud(returns, intensities), {}}, + &hybrid_grid_, &intensity_hybrid_grid_); } float GetProbability(float x, float y, float z) const { @@ -54,6 +71,11 @@ class RangeDataInserter3DTest : public ::testing::Test { hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } + float GetIntensity(float x, float y, float z) const { + return intensity_hybrid_grid_.GetIntensity( + intensity_hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); + } + float IsKnown(float x, float y, float z) const { return hybrid_grid_.IsKnown( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); @@ -63,6 +85,7 @@ class RangeDataInserter3DTest : public ::testing::Test { private: HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; std::unique_ptr range_data_inserter_; proto::RangeDataInserterOptions3D options_; }; @@ -87,6 +110,28 @@ TEST_F(RangeDataInserter3DTest, InsertPointCloud) { } } +TEST_F(RangeDataInserter3DTest, InsertPointCloudWithIntensities) { + InsertPointCloudWithIntensities(); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), + 1e-4); + EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), + 1e-4); + for (int x = -4; x <= 4; ++x) { + for (int y = -4; y <= 4; ++y) { + if (x < -3 || x > 0 || y != x + 2) { + EXPECT_FALSE(IsKnown(x, y, 4.f)); + EXPECT_NEAR(0.f, GetIntensity(x, y, 4.f), 1e-6); + } else { + EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), + 1e-4); + EXPECT_NEAR(10 + x, GetIntensity(x, y, 4.f), 1e-6); + } + } + } +} + TEST_F(RangeDataInserter3DTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), diff --git a/cartographer/mapping/3d/submap_3d.cc b/cartographer/mapping/3d/submap_3d.cc index 97c5d0fac8..9facbe4a5a 100644 --- a/cartographer/mapping/3d/submap_3d.cc +++ b/cartographer/mapping/3d/submap_3d.cc @@ -37,16 +37,14 @@ struct PixelData { }; // Filters 'range_data', retaining only the returns that have no more than -// 'max_range' distance from the origin. Removes misses and reflectivity -// information. +// 'max_range' distance from the origin. Removes misses. sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, const float max_range) { sensor::RangeData result{range_data.origin, {}, {}}; - for (const sensor::RangefinderPoint& hit : range_data.returns) { - if ((hit.position - range_data.origin).norm() <= max_range) { - result.returns.push_back(hit); - } - } + result.returns = + range_data.returns.copy_if([&](const sensor::RangefinderPoint& point) { + return (point.position - range_data.origin).norm() <= max_range; + }); return result; } @@ -204,6 +202,8 @@ Submap3D::Submap3D(const float high_resolution, const float low_resolution, absl::make_unique(high_resolution)), low_resolution_hybrid_grid_( absl::make_unique(low_resolution)), + high_resolution_intensity_hybrid_grid_( + absl::make_unique(high_resolution)), rotational_scan_matcher_histogram_(rotational_scan_matcher_histogram) {} Submap3D::Submap3D(const proto::Submap3D& proto) @@ -280,9 +280,11 @@ void Submap3D::InsertData(const sensor::RangeData& range_data_in_local, range_data_inserter.Insert( FilterRangeDataByMaxRange(transformed_range_data, high_resolution_max_range), - high_resolution_hybrid_grid_.get()); + high_resolution_hybrid_grid_.get(), + high_resolution_intensity_hybrid_grid_.get()); range_data_inserter.Insert(transformed_range_data, - low_resolution_hybrid_grid_.get()); + low_resolution_hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); set_num_range_data(num_range_data() + 1); const float yaw_in_submap_from_gravity = transform::GetYaw( local_pose().inverse().rotation() * local_from_gravity_aligned); @@ -334,6 +336,11 @@ void ActiveSubmaps3D::AddSubmap( // This will crop the finished Submap before inserting a new Submap to // reduce peak memory usage a bit. CHECK(submaps_.front()->insertion_finished()); + // We use `ForgetIntensityHybridGrid` to reduce memory usage. Since we use + // active submaps and their associated intensity hybrid grids for scan + // matching, we call `ForgetIntensityHybridGrid` once we remove the submap + // from active submaps and no longer need the intensity hybrid grid. + submaps_.front()->ForgetIntensityHybridGrid(); submaps_.erase(submaps_.begin()); } const Eigen::VectorXf initial_rotational_scan_matcher_histogram = diff --git a/cartographer/mapping/3d/submap_3d.h b/cartographer/mapping/3d/submap_3d.h index b89da0f495..e5d7e8b443 100644 --- a/cartographer/mapping/3d/submap_3d.h +++ b/cartographer/mapping/3d/submap_3d.h @@ -26,9 +26,9 @@ #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h" #include "cartographer/mapping/proto/serialization.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" @@ -60,6 +60,14 @@ class Submap3D : public Submap { const HybridGrid& low_resolution_hybrid_grid() const { return *low_resolution_hybrid_grid_; } + const IntensityHybridGrid& high_resolution_intensity_hybrid_grid() const { + CHECK(high_resolution_intensity_hybrid_grid_ != nullptr); + return *high_resolution_intensity_hybrid_grid_; + } + void ForgetIntensityHybridGrid() { + high_resolution_intensity_hybrid_grid_.reset(); + } + const Eigen::VectorXf& rotational_scan_matcher_histogram() const { return rotational_scan_matcher_histogram_; } @@ -79,6 +87,7 @@ class Submap3D : public Submap { std::unique_ptr high_resolution_hybrid_grid_; std::unique_ptr low_resolution_hybrid_grid_; + std::unique_ptr high_resolution_intensity_hybrid_grid_; Eigen::VectorXf rotational_scan_matcher_histogram_; }; diff --git a/cartographer/mapping/detect_floors.cc b/cartographer/mapping/detect_floors.cc index ad402ac846..8ec605dd80 100644 --- a/cartographer/mapping/detect_floors.cc +++ b/cartographer/mapping/detect_floors.cc @@ -188,8 +188,13 @@ std::vector FindFloors(const proto::Trajectory& trajectory, common::FromUniversal( trajectory.node(span.end_index - 1).timestamp())}); } - std::sort(z_values.begin(), z_values.end()); - floors.back().z = Median(z_values); + if (!z_values.empty()) { + std::sort(z_values.begin(), z_values.end()); + floors.back().z = Median(z_values); + } else { + LOG(ERROR) << "All spans in level are short"; + floors.pop_back(); + } } return floors; } diff --git a/cartographer/mapping/detect_floors.h b/cartographer/mapping/detect_floors.h index 189086f92c..b5acde8019 100644 --- a/cartographer/mapping/detect_floors.h +++ b/cartographer/mapping/detect_floors.h @@ -17,9 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ #define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ -#include "cartographer/mapping/proto/trajectory.pb.h" - #include "cartographer/common/time.h" +#include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 8e8cadc46e..c5abdf9af7 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -378,14 +378,23 @@ class MapById { const std::map& trajectory = trajectories_.at(trajectory_id).data_; + if (internal::GetTime(std::prev(trajectory.end())->second) < time) { return EndOfTrajectory(trajectory_id); } + auto left = trajectory.begin(); auto right = std::prev(trajectory.end()); while (left != right) { + // This is never 'right' which is important to guarantee progress. const int middle = left->first + (right->first - left->first) / 2; - const auto lower_bound_middle = trajectory.lower_bound(middle); + // This could be 'right' in the presence of gaps, so we need to use the + // previous element in this case. + auto lower_bound_middle = trajectory.lower_bound(middle); + if (lower_bound_middle->first > middle) { + CHECK(lower_bound_middle != left); + lower_bound_middle = std::prev(lower_bound_middle); + } if (internal::GetTime(lower_bound_middle->second) < time) { left = std::next(lower_bound_middle); } else { diff --git a/cartographer/mapping/id_test.cc b/cartographer/mapping/id_test.cc index 96a204b17c..ab877e0f52 100644 --- a/cartographer/mapping/id_test.cc +++ b/cartographer/mapping/id_test.cc @@ -254,6 +254,53 @@ TEST(IdTest, LowerBoundFuzz) { } } +TEST(IdTest, LowerBoundTrimmedTrajectory) { + constexpr int kTrajectoryId = 1; + + std::mt19937 rng; + std::uniform_int_distribution dt_dist(1, 20); + + const int N = 500; + int t = 0; + MapById map_by_id; + for (int j = 0; j < N; ++j) { + t = t + dt_dist(rng); + map_by_id.Append(kTrajectoryId, Data(t)); + } + + // Choose random length of a trim segment. + std::uniform_int_distribution dt_trim_segment_length( + 1, static_cast(N / 2)); + size_t trim_segment_length = dt_trim_segment_length(rng); + // Choose random start for a trim_segment. + std::uniform_int_distribution dt_trim_segment_start( + 2, N - trim_segment_length - 1); + size_t trim_segment_start_index = dt_trim_segment_start(rng); + + auto trim_segment_start = map_by_id.begin(); + std::advance(trim_segment_start, trim_segment_start_index); + + auto trim_segment_end = map_by_id.begin(); + std::advance(trim_segment_end, + trim_segment_start_index + trim_segment_length); + + for (auto it = trim_segment_start; it != trim_segment_end;) { + const auto this_it = it; + ++it; + map_by_id.Trim(this_it->id); + } + + auto it = map_by_id.lower_bound(kTrajectoryId, CreateTime(0)); + + auto ground_truth = + std::lower_bound(map_by_id.BeginOfTrajectory(kTrajectoryId), + map_by_id.EndOfTrajectory(kTrajectoryId), CreateTime(0), + [](MapById::IdDataReference a, + const common::Time& t) { return a.data.time() < t; }); + + EXPECT_EQ(ground_truth, it); +} + struct DataStruct { const common::Time time; }; diff --git a/cartographer/mapping/imu_tracker.cc b/cartographer/mapping/imu_tracker.cc index 39cac08fcb..8aed29cd0a 100644 --- a/cartographer/mapping/imu_tracker.cc +++ b/cartographer/mapping/imu_tracker.cc @@ -20,6 +20,7 @@ #include #include "cartographer/common/math.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -60,7 +61,7 @@ void ImuTracker::AddImuLinearAccelerationObservation( (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; // Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. - const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( + const Eigen::Quaterniond rotation = FromTwoVectors( gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized(); CHECK_GT((orientation_ * gravity_vector_).z(), 0.); diff --git a/cartographer/mapping/imu_tracker_test.cc b/cartographer/mapping/imu_tracker_test.cc index 8f3b452bf7..10992eda62 100644 --- a/cartographer/mapping/imu_tracker_test.cc +++ b/cartographer/mapping/imu_tracker_test.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/imu_tracker.h" #include "absl/memory/memory.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "gtest/gtest.h" namespace cartographer { @@ -82,9 +83,8 @@ TEST_F(ImuTrackerTest, IntegrateFullRotation) { TEST_F(ImuTrackerTest, LearnGravityVector) { linear_acceleration_ = Eigen::Vector3d(0.5, 0.3, 9.5); AdvanceImu(); - Eigen::Quaterniond expected_orientation; - expected_orientation.setFromTwoVectors(linear_acceleration_, - Eigen::Vector3d::UnitZ()); + const Eigen::Quaterniond expected_orientation = + FromTwoVectors(linear_acceleration_, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., imu_tracker_->orientation().angularDistance(expected_orientation), kPrecision); diff --git a/cartographer/mapping/internal/2d/local_slam_result_2d.cc b/cartographer/mapping/internal/2d/local_slam_result_2d.cc index 05b078c954..d068d01456 100644 --- a/cartographer/mapping/internal/2d/local_slam_result_2d.cc +++ b/cartographer/mapping/internal/2d/local_slam_result_2d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/2d/local_slam_result_2d.h" + #include "cartographer/mapping/internal/2d/pose_graph_2d.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/local_slam_result_2d.h b/cartographer/mapping/internal/2d/local_slam_result_2d.h index f55fdd25bb..71b4f181ef 100644 --- a/cartographer/mapping/internal/2d/local_slam_result_2d.h +++ b/cartographer/mapping/internal/2d/local_slam_result_2d.h @@ -17,8 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_SLAM_RESULT_2D_H_ +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" namespace cartographer { diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc index 08823e01cd..5bda7938a5 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc @@ -58,8 +58,8 @@ LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.returns), - sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses)}; + sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), + sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())}; } std::unique_ptr LocalTrajectoryBuilder2D::ScanMatch( @@ -226,8 +226,8 @@ LocalTrajectoryBuilder2D::AddAccumulatedRangeData( non_gravity_aligned_pose_prediction * gravity_alignment.inverse()); const sensor::PointCloud& filtered_gravity_aligned_point_cloud = - sensor::AdaptiveVoxelFilter(options_.adaptive_voxel_filter_options()) - .Filter(gravity_aligned_range_data.returns); + sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns, + options_.adaptive_voxel_filter_options()); if (filtered_gravity_aligned_point_cloud.empty()) { return nullptr; } @@ -319,14 +319,15 @@ void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) { if (extrapolator_ != nullptr) { return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; + CHECK(!options_.pose_extrapolator_options().use_imu_based()); // TODO(gaschler): Consider using InitializeWithImu as 3D does. extrapolator_ = absl::make_unique( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant()); + ::cartographer::common::FromSeconds(options_.pose_extrapolator_options() + .constant_velocity() + .pose_queue_duration()), + options_.pose_extrapolator_options() + .constant_velocity() + .imu_gravity_time_constant()); extrapolator_->AddPose(time, transform::Rigid3d::Identity()); } diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h index 1a9346378d..faed1df105 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_2d.h @@ -27,7 +27,7 @@ #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" #include "cartographer/mapping/pose_extrapolator.h" -#include "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/internal/voxel_filter.h" diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc index 552e0d5893..434da3d0f3 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" namespace cartographer { @@ -58,6 +59,8 @@ proto::LocalTrajectoryBuilderOptions2D CreateLocalTrajectoryBuilderOptions2D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); *options.mutable_submaps_options() = CreateSubmapsOptions2D( diff --git a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h index 32fa74228e..768492060a 100644 --- a/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h +++ b/cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_2D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_2d.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.h b/cartographer/mapping/internal/2d/normal_estimation_2d.h index f3d937de72..9a344261fb 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d.h +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.h @@ -18,7 +18,8 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ #include -#include "cartographer/mapping/proto/2d/normal_estimation_options_2d.pb.h" + +#include "cartographer/mapping/proto/normal_estimation_options_2d.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc index e32dcb49e8..336d31dde6 100644 --- a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/internal/2d/normal_estimation_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/math.h" #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -45,9 +45,9 @@ TEST(NormalEstimation2DTest, SinglePoint) { const double angle = static_cast(angle_idx) / static_cast(num_angles) * 2. * M_PI - M_PI; - range_data.returns.clear(); - range_data.returns.push_back( - {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast()}); + range_data.returns = sensor::PointCloud( + {{Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.} + .cast()}}); std::vector normals; normals = EstimateNormals(range_data, options); EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), @@ -75,30 +75,27 @@ TEST(NormalEstimation2DTest, StraightLineGeometry) { EXPECT_NEAR(normal, -M_PI_2, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, 1.f, 0.f}}, + {Eigen::Vector3f{1.f, 0.f, 0.f}}, + {Eigen::Vector3f{1.f, -1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(std::abs(normal), M_PI, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{1.f, -1.f, 0.f}}, + {Eigen::Vector3f{0.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, -1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, M_PI_2, 1e-4); } normals.clear(); - range_data.returns.clear(); - range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}}); - range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}}); + range_data.returns = sensor::PointCloud({{Eigen::Vector3f{-1.f, -1.f, 0.f}}, + {Eigen::Vector3f{-1.f, 0.f, 0.f}}, + {Eigen::Vector3f{-1.f, 1.f, 0.f}}}); normals = EstimateNormals(range_data, options); for (const float normal : normals) { EXPECT_NEAR(normal, 0, 1e-4); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 4c22be3e1b..060277eb56 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -42,6 +42,7 @@ namespace cartographer { namespace mapping { static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); @@ -181,6 +182,7 @@ void PoseGraph2D::AddWorkItem( } const auto now = std::chrono::steady_clock::now(); work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); kWorkQueueDelayMetric->Set( std::chrono::duration_cast>( now - work_queue_->front().time) @@ -229,7 +231,14 @@ void PoseGraph2D::AddOdometryData(const int trajectory_id, void PoseGraph2D::AddFixedFramePoseData( const int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data) { - LOG(FATAL) << "Not yet implemented for 2D."; + AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddFixedFramePoseData(trajectory_id, + fixed_frame_pose_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); } void PoseGraph2D::AddLandmarkData(int trajectory_id, @@ -522,6 +531,7 @@ void PoseGraph2D::DrainWorkQueue() { work_item = work_queue_->front().task; work_queue_->pop_front(); work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); } process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; } @@ -649,6 +659,22 @@ void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; }); @@ -737,7 +763,24 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph2D::SetTrajectoryDataFromProto( const proto::TrajectoryData& data) { - LOG(ERROR) << "not implemented"; + TrajectoryData trajectory_data; + // gravity_constant and imu_calibration are omitted as its not used in 2d + + if (data.has_fixed_frame_origin_in_map()) { + trajectory_data.fixed_frame_origin_in_map = + transform::ToRigid3(data.fixed_frame_origin_in_map()); + + const int trajectory_id = data.trajectory_id(); + AddWorkItem([this, trajectory_id, trajectory_data]() + LOCKS_EXCLUDED(mutex_) { + absl::MutexLock locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->SetTrajectoryData( + trajectory_id, trajectory_data); + } + return WorkItem::Result::kDoNotRunOptimization; + }); + } } void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, @@ -968,15 +1011,14 @@ PoseGraph2D::GetLandmarkNodes() const { std::map PoseGraph2D::GetTrajectoryData() const { - // The 2D optimization problem does not have any 'TrajectoryData'. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->trajectory_data(); } sensor::MapByTime PoseGraph2D::GetFixedFramePoseData() const { - // FixedFramePoseData is not yet implemented for 2D. We need to return empty - // so serialization works. - return {}; + absl::MutexLock locker(&mutex_); + return optimization_problem_->fixed_frame_pose_data(); } std::vector PoseGraph2D::constraints() const { @@ -1168,42 +1210,69 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished); - // Compile all nodes that are still INTRA_SUBMAP constrained once the submap - // with 'submap_id' is gone. + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. std::set nodes_to_retain; - for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - constraint.submap_id != submap_id) { - nodes_to_retain.insert(constraint.node_id); + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); } } - // Remove all 'data_.constraints' related to 'submap_id'. + + // Remove all nodes that are exlusively associated to 'submap_id'. std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. { std::vector constraints; for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.submap_id == submap_id) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - nodes_to_retain.count(constraint.node_id) == 0) { - // This node will no longer be INTRA_SUBMAP contrained and has to be - // removed. - nodes_to_remove.insert(constraint.node_id); - } - } else { + if (constraint.submap_id != submap_id) { constraints.push_back(constraint); } } parent_->data_.constraints = std::move(constraints); } + // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data. @@ -1249,6 +1318,10 @@ void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory* family_factory) { "mapping_2d_pose_graph_work_queue_delay", "Age of the oldest entry in the work queue in seconds"); kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); auto* constraints = family_factory->NewGaugeFamily( "mapping_2d_pose_graph_constraints", "Current number of constraints in the pose graph"); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index bf20f42381..373342525f 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -35,10 +35,10 @@ #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#include "cartographer/mapping/internal/pose_graph_data.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/internal/work_queue.h" #include "cartographer/mapping/pose_graph.h" -#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/value_conversion_tables.h" #include "cartographer/metrics/family_factory.h" diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index 965ff0a28d..2ecdfd12fe 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -21,7 +21,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" @@ -142,6 +142,9 @@ class PoseGraph2DTest : public ::testing::Test { odometry_rotation_weight = 0., fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = true, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc index 3590d31d8d..2fe2219896 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d.cc @@ -20,7 +20,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc index 88ec321c7d..74036adfde 100644 --- a/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/ceres_scan_matcher_2d_test.cc @@ -19,8 +19,8 @@ #include #include "absl/memory/memory.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc index 29a076bc42..ce513a9c61 100644 --- a/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_test.cc @@ -15,7 +15,6 @@ */ #include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h" - #include "cartographer/sensor/point_cloud.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc index c50252f0ca..2de83935f5 100644 --- a/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d_test.cc @@ -22,7 +22,7 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h index 1784d07c8b..5b014fd45c 100644 --- a/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h @@ -19,7 +19,7 @@ #include -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" namespace cartographer { namespace mapping { @@ -133,4 +133,4 @@ class InterpolatedTSDF2D { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ \ No newline at end of file +#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_INTERPOLATED_TSDF_2D_H_ diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc index 3cf182aa21..0c4422ee30 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d.h" + #include "cartographer/mapping/probability_values.h" #include "ceres/cubic_interpolation.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc index 240e1c85c6..c9ef1828a5 100644 --- a/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/occupied_space_cost_function_2d_test.cc @@ -18,7 +18,6 @@ #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/probability_values.h" - #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -34,7 +33,7 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { ValueConversionTables conversion_tables; ProbabilityGrid grid(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), &conversion_tables); - sensor::PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; + sensor::PointCloud point_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); ceres::Problem problem; std::unique_ptr cost_function( CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid)); @@ -55,4 +54,4 @@ TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) { } // namespace } // namespace scan_matching } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc index 1bc75bd056..60b00fe4c3 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.cc @@ -25,7 +25,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/mapping/2d/probability_grid.h" -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc index f8eef22fd9..71fb0a9604 100644 --- a/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d_test.cc @@ -21,11 +21,11 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h index d66ef48668..8a1ce6dae2 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h @@ -17,7 +17,7 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" diff --git a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc index c25840fb50..8c9cf04813 100644 --- a/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc @@ -16,11 +16,10 @@ #include "cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" -#include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" - +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "gmock/gmock.h" #include "gtest/gtest.h" @@ -73,7 +72,7 @@ class TSDFSpaceCostFunction2DTest : public ::testing::Test { }; TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { - const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}}; + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 0.f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -89,7 +88,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) { TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { InsertPointcloud(); - const sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + const sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); const std::array pose_estimate{{0., 0., 0.}}; @@ -109,7 +108,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.1, 0.}}; @@ -140,7 +139,7 @@ TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) { TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) { InsertPointcloud(); - sensor::PointCloud matching_cloud = {{Eigen::Vector3f{0.f, 1.0f, 0.f}}}; + sensor::PointCloud matching_cloud({{Eigen::Vector3f{0.f, 1.0f, 0.f}}}); std::unique_ptr cost_function( CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_)); std::array pose_estimate{{0., 0.4, 0.}}; diff --git a/cartographer/mapping/2d/tsd_value_converter.cc b/cartographer/mapping/internal/2d/tsd_value_converter.cc similarity index 95% rename from cartographer/mapping/2d/tsd_value_converter.cc rename to cartographer/mapping/internal/2d/tsd_value_converter.cc index dde8ddb4a7..5feddad786 100644 --- a/cartographer/mapping/2d/tsd_value_converter.cc +++ b/cartographer/mapping/internal/2d/tsd_value_converter.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsd_value_converter.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/2d/tsd_value_converter.h b/cartographer/mapping/internal/2d/tsd_value_converter.h similarity index 100% rename from cartographer/mapping/2d/tsd_value_converter.h rename to cartographer/mapping/internal/2d/tsd_value_converter.h diff --git a/cartographer/mapping/2d/tsd_value_converter_test.cc b/cartographer/mapping/internal/2d/tsd_value_converter_test.cc similarity index 98% rename from cartographer/mapping/2d/tsd_value_converter_test.cc rename to cartographer/mapping/internal/2d/tsd_value_converter_test.cc index 0c37d2dd53..1fa496e040 100644 --- a/cartographer/mapping/2d/tsd_value_converter_test.cc +++ b/cartographer/mapping/internal/2d/tsd_value_converter_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsd_value_converter.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" #include "gtest/gtest.h" diff --git a/cartographer/mapping/2d/tsdf_2d.cc b/cartographer/mapping/internal/2d/tsdf_2d.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_2d.cc rename to cartographer/mapping/internal/2d/tsdf_2d.cc index 32bdb59632..fa6fc9b756 100644 --- a/cartographer/mapping/2d/tsdf_2d.cc +++ b/cartographer/mapping/internal/2d/tsdf_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include "absl/memory/memory.h" diff --git a/cartographer/mapping/2d/tsdf_2d.h b/cartographer/mapping/internal/2d/tsdf_2d.h similarity index 97% rename from cartographer/mapping/2d/tsdf_2d.h rename to cartographer/mapping/internal/2d/tsdf_2d.h index 250f38f08e..16134f637e 100644 --- a/cartographer/mapping/2d/tsdf_2d.h +++ b/cartographer/mapping/internal/2d/tsdf_2d.h @@ -22,8 +22,9 @@ #include "cartographer/common/port.h" #include "cartographer/mapping/2d/grid_2d.h" #include "cartographer/mapping/2d/map_limits.h" -#include "cartographer/mapping/2d/tsd_value_converter.h" #include "cartographer/mapping/2d/xy_index.h" +#include "cartographer/mapping/internal/2d/tsd_value_converter.h" + namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/2d/tsdf_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_2d_test.cc similarity index 99% rename from cartographer/mapping/2d/tsdf_2d_test.cc rename to cartographer/mapping/internal/2d/tsdf_2d_test.cc index 5e50343ac1..2ea899ec26 100644 --- a/cartographer/mapping/2d/tsdf_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" #include diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc similarity index 96% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc index 491be9c438..7e6bd3f1a2 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" #include "cartographer/mapping/internal/2d/normal_estimation_2d.h" #include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h" @@ -73,7 +73,7 @@ struct RangeDataSorter { const Eigen::Vector2f delta_lhs = (lhs.position.head<2>() - origin_).normalized(); const Eigen::Vector2f delta_rhs = - (lhs.position.head<2>() - origin_).normalized(); + (rhs.position.head<2>() - origin_).normalized(); if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) { return delta_lhs[1] < 0.f; } else if (delta_lhs[1] < 0.f) { @@ -142,9 +142,11 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data, std::vector normals; if (options_.project_sdf_distance_to_scan_normal() || scale_update_weight_angle_scan_normal_to_ray) { - std::sort(sorted_range_data.returns.begin(), - sorted_range_data.returns.end(), + std::vector returns = + sorted_range_data.returns.points(); + std::sort(returns.begin(), returns.end(), RangeDataSorter(sorted_range_data.origin)); + sorted_range_data.returns = sensor::PointCloud(std::move(returns)); normals = EstimateNormals(sorted_range_data, options_.normal_estimation_options()); } diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h similarity index 94% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d.h rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h index 61a77a2f58..bdc810ac5c 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d.h +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h @@ -18,8 +18,8 @@ #define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/2d/tsdf_2d.h" -#include "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.pb.h" +#include "cartographer/mapping/internal/2d/tsdf_2d.h" +#include "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.pb.h" #include "cartographer/mapping/range_data_inserter_interface.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" @@ -57,4 +57,4 @@ class TSDFRangeDataInserter2D : public RangeDataInserterInterface { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ \ No newline at end of file +#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_ diff --git a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc similarity index 98% rename from cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc rename to cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc index 4db939a016..0850e6ecf7 100644 --- a/cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc +++ b/cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d_test.cc @@ -14,10 +14,10 @@ * limitations under the License. */ -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { @@ -27,7 +27,7 @@ namespace { class RangeDataInserterTest2DTSDF : public ::testing::Test { protected: RangeDataInserterTest2DTSDF() - : tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0, + : tsdf_(MapLimits(1., Eigen::Vector2d(1., 7.), CellLimits(8, 1)), 2.0, 10.0, &conversion_tables_) { auto parameter_dictionary = common::MakeDictionary( "return { " @@ -346,4 +346,4 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) { } // namespace } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer diff --git a/cartographer/mapping/internal/3d/imu_integration.h b/cartographer/mapping/internal/3d/imu_integration.h index 88ee221c6c..b89875c1de 100644 --- a/cartographer/mapping/internal/3d/imu_integration.h +++ b/cartographer/mapping/internal/3d/imu_integration.h @@ -24,6 +24,7 @@ #include "Eigen/Geometry" #include "cartographer/common/time.h" #include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -33,6 +34,7 @@ namespace mapping { template struct IntegrateImuResult { Eigen::Matrix delta_velocity; + Eigen::Matrix delta_translation; Eigen::Quaternion delta_rotation; }; @@ -54,6 +56,7 @@ IntegrateImuResult IntegrateImu( common::Time current_time = start_time; IntegrateImuResult result = {Eigen::Matrix::Zero(), + Eigen::Matrix::Zero(), Eigen::Quaterniond::Identity().cast()}; while (current_time < end_time) { common::Time next_imu_data = common::Time::max(); @@ -73,6 +76,7 @@ IntegrateImuResult IntegrateImu( ((linear_acceleration_calibration * (*it)->linear_acceleration.template cast()) * delta_t); + result.delta_translation += delta_t * result.delta_velocity; current_time = next_time; if (current_time == next_imu_data) { ++(*it); @@ -92,6 +96,67 @@ IntegrateImuResult IntegrateImu(const RangeType& imu_data, start_time, end_time, it); } +template +struct ExtrapolatePoseResult { + transform::Rigid3 pose; + Eigen::Matrix velocity; +}; + +// Returns pose and linear velocity at 'time' which is equal to +// 'prev_from_tracking' extrapolated using IMU data. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const Eigen::Matrix& prev_velocity_in_tracking, + const common::Time prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + const IntegrateImuResult result = + IntegrateImu(imu_data, Eigen::Transform::Identity(), + Eigen::Transform::Identity(), prev_time, + time, imu_it); + + const T delta_t = static_cast(common::ToSeconds(time - prev_time)); + const Eigen::Matrix translation = + prev_from_tracking.translation() + + prev_from_tracking.rotation() * + (delta_t * prev_velocity_in_tracking + result.delta_translation) - + static_cast(.5) * delta_t * delta_t * gravity; + const Eigen::Quaternion rotation = + prev_from_tracking.rotation() * result.delta_rotation; + + const Eigen::Matrix velocity = + prev_from_tracking.rotation() * + (prev_velocity_in_tracking + result.delta_velocity) - + delta_t * gravity; + return ExtrapolatePoseResult{transform::Rigid3(translation, rotation), + velocity}; +} + +// Same as above but given the last two poses. +template +ExtrapolatePoseResult ExtrapolatePoseWithImu( + const transform::Rigid3& prev_from_tracking, + const common::Time prev_time, + const transform::Rigid3& prev_prev_from_tracking, + const common::Time prev_prev_time, const Eigen::Matrix& gravity, + const common::Time time, const RangeType& imu_data, + IteratorType* const imu_it) { + // TODO(danielsievers): Really we should integrate velocities starting from + // the midpoint in between two poses, since this is how we fit them to poses + // in the optimization. + const T prev_delta_t = + static_cast(common::ToSeconds(prev_time - prev_prev_time)); + const Eigen::Matrix prev_velocity_in_tracking = + prev_from_tracking.inverse().rotation() * + (prev_from_tracking.translation() - + prev_prev_from_tracking.translation()) / + prev_delta_t; + + return ExtrapolatePoseWithImu(prev_from_tracking, prev_velocity_in_tracking, + prev_time, gravity, time, imu_data, imu_it); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/local_slam_result_3d.cc b/cartographer/mapping/internal/3d/local_slam_result_3d.cc index 614300852f..6f8e703def 100644 --- a/cartographer/mapping/internal/3d/local_slam_result_3d.cc +++ b/cartographer/mapping/internal/3d/local_slam_result_3d.cc @@ -15,6 +15,7 @@ */ #include "cartographer/mapping/internal/3d/local_slam_result_3d.h" + #include "cartographer/mapping/internal/3d/pose_graph_3d.h" namespace cartographer { diff --git a/cartographer/mapping/internal/3d/local_slam_result_3d.h b/cartographer/mapping/internal/3d/local_slam_result_3d.h index 3efba2ed2e..88e94ffa57 100644 --- a/cartographer/mapping/internal/3d/local_slam_result_3d.h +++ b/cartographer/mapping/internal/3d/local_slam_result_3d.h @@ -17,8 +17,8 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_SLAM_RESULT_3D_H_ +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/internal/submap_controller.h" -#include "cartographer/mapping/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" namespace cartographer { diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc index 5cd26ab11d..b2a4f86bc1 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc @@ -21,10 +21,11 @@ #include "absl/memory/memory.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" -#include "cartographer/mapping/proto/3d/submaps_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h" +#include "cartographer/mapping/proto/submaps_options_3d.pb.h" +#include "cartographer/transform/timestamped_transform.h" #include "glog/logging.h" namespace cartographer { @@ -55,7 +56,6 @@ LocalTrajectoryBuilder3D::LocalTrajectoryBuilder3D( options_.real_time_correlative_scan_matcher_options())), ceres_scan_matcher_(absl::make_unique( options_.ceres_scan_matcher_options())), - accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}}, range_data_collator_(expected_range_sensor_ids) {} LocalTrajectoryBuilder3D::~LocalTrajectoryBuilder3D() {} @@ -82,13 +82,18 @@ std::unique_ptr LocalTrajectoryBuilder3D::ScanMatch( transform::Rigid3d pose_observation_in_submap; ceres::Solver::Summary summary; + const auto* high_resolution_intensity_hybrid_grid = + options_.use_intensities() + ? &matching_submap->high_resolution_intensity_hybrid_grid() + : nullptr; ceres_scan_matcher_->Match( (matching_submap->local_pose().inverse() * pose_prediction).translation(), - initial_ceres_pose, - {{&high_resolution_point_cloud_in_tracking, - &matching_submap->high_resolution_hybrid_grid()}, - {&low_resolution_point_cloud_in_tracking, - &matching_submap->low_resolution_hybrid_grid()}}, + initial_ceres_pose, {{&high_resolution_point_cloud_in_tracking, + &matching_submap->high_resolution_hybrid_grid(), + high_resolution_intensity_hybrid_grid}, + {&low_resolution_point_cloud_in_tracking, + &matching_submap->low_resolution_hybrid_grid(), + /*intensity_hybrid_grid=*/nullptr}}, &pose_observation_in_submap, &summary); kCeresScanMatcherCostMetric->Observe(summary.final_cost); const double residual_distance = (pose_observation_in_submap.translation() - @@ -108,27 +113,36 @@ void LocalTrajectoryBuilder3D::AddImuData(const sensor::ImuData& imu_data) { extrapolator_->AddImuData(imu_data); return; } - // We derive velocities from poses which are at least 1 ms apart for numerical - // stability. Usually poses known to the extrapolator will be further apart - // in time and thus the last two are used. - constexpr double kExtrapolationEstimationTimeSec = 0.001; - extrapolator_ = mapping::PoseExtrapolator::InitializeWithImu( - ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), - options_.imu_gravity_time_constant(), imu_data); + std::vector initial_poses; + for (const auto& pose_proto : options_.initial_poses()) { + initial_poses.push_back(transform::FromProto(pose_proto)); + } + std::vector initial_imu_data; + for (const auto& imu : options_.initial_imu_data()) { + initial_imu_data.push_back(sensor::FromProto(imu)); + } + initial_imu_data.push_back(imu_data); + extrapolator_ = mapping::PoseExtrapolatorInterface::CreateWithImuData( + options_.pose_extrapolator_options(), initial_imu_data, initial_poses); } std::unique_ptr LocalTrajectoryBuilder3D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { - const auto synchronized_data = + if (options_.use_intensities()) { + CHECK_EQ(unsynchronized_data.ranges.size(), + unsynchronized_data.intensities.size()) + << "Passed point cloud has inconsistent number of intensities and " + "ranges."; + } + auto synchronized_data = range_data_collator_.AddRangeData(sensor_id, unsynchronized_data); if (synchronized_data.ranges.empty()) { LOG(INFO) << "Range data collator filling buffer."; return nullptr; } - const common::Time& current_sensor_time = synchronized_data.time; if (extrapolator_ == nullptr) { // Until we've initialized the extrapolator with our first IMU message, we // cannot compute the orientation of the rangefinder. @@ -139,127 +153,159 @@ LocalTrajectoryBuilder3D::AddRangeData( CHECK(!synchronized_data.ranges.empty()); CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f); const common::Time time_first_point = - current_sensor_time + + synchronized_data.time + common::FromSeconds(synchronized_data.ranges.front().point_time.time); if (time_first_point < extrapolator_->GetLastPoseTime()) { LOG(INFO) << "Extrapolator is still initializing."; return nullptr; } - std::vector hits = - sensor::VoxelFilter(0.5f * options_.voxel_filter_size()) - .Filter(synchronized_data.ranges); + if (num_accumulated_ == 0) { + accumulated_point_cloud_origin_data_.clear(); + } - std::vector hits_poses; - hits_poses.reserve(hits.size()); - bool warned = false; + synchronized_data.ranges = sensor::VoxelFilter( + synchronized_data.ranges, 0.5f * options_.voxel_filter_size()); + accumulated_point_cloud_origin_data_.emplace_back( + std::move(synchronized_data)); + ++num_accumulated_; - for (const auto& hit : hits) { - common::Time time_point = - current_sensor_time + common::FromSeconds(hit.point_time.time); - if (time_point < extrapolator_->GetLastExtrapolatedTime()) { - if (!warned) { - LOG(ERROR) - << "Timestamp of individual range data point jumps backwards from " - << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point; - warned = true; + if (num_accumulated_ < options_.num_accumulated_range_data()) { + return nullptr; + } + num_accumulated_ = 0; + + bool warned = false; + std::vector hit_times; + common::Time prev_time_point = extrapolator_->GetLastExtrapolatedTime(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + common::Time time_point = point_cloud_origin_data.time + + common::FromSeconds(hit.point_time.time); + if (time_point < prev_time_point) { + if (!warned) { + LOG(ERROR) << "Timestamp of individual range data point jumps " + "backwards from " + << prev_time_point << " to " << time_point; + warned = true; + } + time_point = prev_time_point; } - time_point = extrapolator_->GetLastExtrapolatedTime(); + + hit_times.push_back(time_point); + prev_time_point = time_point; } - hits_poses.push_back( - extrapolator_->ExtrapolatePose(time_point).cast()); } - - if (num_accumulated_ == 0) { - // 'accumulated_range_data_.origin' is not used. - accumulated_range_data_ = sensor::RangeData{{}, {}, {}}; + hit_times.push_back(accumulated_point_cloud_origin_data_.back().time); + + const PoseExtrapolatorInterface::ExtrapolationResult extrapolation_result = + extrapolator_->ExtrapolatePosesWithGravity(hit_times); + std::vector hits_poses( + std::move(extrapolation_result.previous_poses)); + hits_poses.push_back(extrapolation_result.current_pose.cast()); + CHECK_EQ(hits_poses.size(), hit_times.size()); + + const size_t max_possible_number_of_accumulated_points = hit_times.size(); + std::vector accumulated_points; + std::vector accumulated_intensities; + accumulated_points.reserve(max_possible_number_of_accumulated_points); + if (options_.use_intensities()) { + accumulated_intensities.reserve(max_possible_number_of_accumulated_points); } - - for (size_t i = 0; i < hits.size(); ++i) { - sensor::RangefinderPoint hit_in_local = - hits_poses[i] * sensor::ToRangefinderPoint(hits[i].point_time); - const Eigen::Vector3f origin_in_local = - hits_poses[i] * synchronized_data.origins.at(hits[i].origin_index); - const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; - const float range = delta.norm(); - if (range >= options_.min_range()) { - if (range <= options_.max_range()) { - accumulated_range_data_.returns.push_back(hit_in_local); - } else { - // We insert a ray cropped to 'max_range' as a miss for hits beyond the - // maximum range. This way the free space up to the maximum range will - // be updated. - hit_in_local.position = - origin_in_local + options_.max_range() / range * delta; - accumulated_range_data_.misses.push_back(hit_in_local); + sensor::PointCloud misses; + std::vector::const_iterator hits_poses_it = + hits_poses.begin(); + for (const auto& point_cloud_origin_data : + accumulated_point_cloud_origin_data_) { + for (const auto& hit : point_cloud_origin_data.ranges) { + const Eigen::Vector3f hit_in_local = + *hits_poses_it * hit.point_time.position; + const Eigen::Vector3f origin_in_local = + *hits_poses_it * point_cloud_origin_data.origins.at(hit.origin_index); + const Eigen::Vector3f delta = hit_in_local - origin_in_local; + const float range = delta.norm(); + if (range >= options_.min_range()) { + if (range <= options_.max_range()) { + accumulated_points.push_back(sensor::RangefinderPoint{hit_in_local}); + if (options_.use_intensities()) { + accumulated_intensities.push_back(hit.intensity); + } + } else { + // We insert a ray cropped to 'max_range' as a miss for hits beyond + // the maximum range. This way the free space up to the maximum range + // will be updated. + // TODO(wohe): since `misses` are not used anywhere in 3D, consider + // removing `misses` from `range_data` and/or everywhere in 3D. + misses.push_back(sensor::RangefinderPoint{ + origin_in_local + options_.max_range() / range * delta}); + } } + ++hits_poses_it; } } - ++num_accumulated_; + CHECK(std::next(hits_poses_it) == hits_poses.end()); + const sensor::PointCloud returns(std::move(accumulated_points), + std::move(accumulated_intensities)); + + const common::Time current_sensor_time = synchronized_data.time; + absl::optional sensor_duration; + if (last_sensor_time_.has_value()) { + sensor_duration = current_sensor_time - last_sensor_time_.value(); + } + last_sensor_time_ = current_sensor_time; - if (num_accumulated_ >= options_.num_accumulated_range_data()) { - absl::optional sensor_duration; - if (last_sensor_time_.has_value()) { - sensor_duration = current_sensor_time - last_sensor_time_.value(); - } - last_sensor_time_ = current_sensor_time; - num_accumulated_ = 0; - - transform::Rigid3f current_pose = - extrapolator_->ExtrapolatePose(current_sensor_time).cast(); - - const auto voxel_filter_start = std::chrono::steady_clock::now(); - const sensor::RangeData filtered_range_data = { - current_pose.translation(), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data_.returns), - sensor::VoxelFilter(options_.voxel_filter_size()) - .Filter(accumulated_range_data_.misses)}; - const auto voxel_filter_stop = std::chrono::steady_clock::now(); - const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; + const common::Time current_time = hit_times.back(); + const auto voxel_filter_start = std::chrono::steady_clock::now(); + const sensor::RangeData filtered_range_data = { + extrapolation_result.current_pose.translation().cast(), + sensor::VoxelFilter(returns, options_.voxel_filter_size()), + sensor::VoxelFilter(misses, options_.voxel_filter_size())}; + const auto voxel_filter_stop = std::chrono::steady_clock::now(); + const auto voxel_filter_duration = voxel_filter_stop - voxel_filter_start; - if (sensor_duration.has_value()) { - const double voxel_filter_fraction = - common::ToSeconds(voxel_filter_duration) / - common::ToSeconds(sensor_duration.value()); - kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); - } - - return AddAccumulatedRangeData( - current_sensor_time, - sensor::TransformRangeData(filtered_range_data, current_pose.inverse()), - sensor_duration); + if (sensor_duration.has_value()) { + const double voxel_filter_fraction = + common::ToSeconds(voxel_filter_duration) / + common::ToSeconds(sensor_duration.value()); + kLocalSlamVoxelFilterFraction->Set(voxel_filter_fraction); } - return nullptr; + + return AddAccumulatedRangeData( + current_time, + sensor::TransformRangeData( + filtered_range_data, + extrapolation_result.current_pose.inverse().cast()), + sensor_duration, extrapolation_result.current_pose, + extrapolation_result.gravity_from_tracking); } std::unique_ptr LocalTrajectoryBuilder3D::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& filtered_range_data_in_tracking, - const absl::optional& sensor_duration) { + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment) { if (filtered_range_data_in_tracking.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; } - const transform::Rigid3d pose_prediction = - extrapolator_->ExtrapolatePose(time); const auto scan_matcher_start = std::chrono::steady_clock::now(); - sensor::AdaptiveVoxelFilter adaptive_voxel_filter( - options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud high_resolution_point_cloud_in_tracking = - adaptive_voxel_filter.Filter(filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.high_resolution_adaptive_voxel_filter_options()); if (high_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty high resolution point cloud data."; return nullptr; } - sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( - options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = - low_resolution_adaptive_voxel_filter.Filter( - filtered_range_data_in_tracking.returns); + sensor::AdaptiveVoxelFilter( + filtered_range_data_in_tracking.returns, + options_.low_resolution_adaptive_voxel_filter_options()); if (low_resolution_point_cloud_in_tracking.empty()) { LOG(WARNING) << "Dropped empty low resolution point cloud data."; return nullptr; @@ -283,8 +329,6 @@ LocalTrajectoryBuilder3D::AddAccumulatedRangeData( kLocalSlamScanMatcherFraction->Set(scan_matcher_fraction); } - const Eigen::Quaterniond gravity_alignment = - extrapolator_->EstimateGravityOrientation(time); sensor::RangeData filtered_range_data_in_local = sensor::TransformRangeData( filtered_range_data_in_tracking, pose_estimate->cast()); diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h index 2abeee857e..f242c385af 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d.h @@ -26,13 +26,14 @@ #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/range_data_collator.h" -#include "cartographer/mapping/pose_extrapolator.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/internal/voxel_filter.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/sensor/range_data.h" +#include "cartographer/sensor/timed_point_cloud_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { @@ -78,7 +79,9 @@ class LocalTrajectoryBuilder3D { std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& filtered_range_data_in_tracking, - const absl::optional& sensor_duration); + const absl::optional& sensor_duration, + const transform::Rigid3d& pose_prediction, + const Eigen::Quaterniond& gravity_alignment); std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& filtered_range_data_in_local, @@ -103,10 +106,11 @@ class LocalTrajectoryBuilder3D { real_time_correlative_scan_matcher_; std::unique_ptr ceres_scan_matcher_; - std::unique_ptr extrapolator_; + std::unique_ptr extrapolator_; int num_accumulated_ = 0; - sensor::RangeData accumulated_range_data_; + std::vector + accumulated_point_cloud_origin_data_; absl::optional last_wall_time_; absl::optional last_thread_cpu_time_seconds_; diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc index 423ee91207..e8f12691b6 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_3d_test.cc @@ -20,7 +20,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" @@ -96,7 +96,30 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { imu_gravity_time_constant = 1., rotational_histogram_size = 120, - + + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, + submaps = { high_resolution = 0.2, high_resolution_max_range = 50., @@ -106,8 +129,11 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { hit_probability = 0.7, miss_probability = 0.4, num_free_space_voxels = 0, + intensity_threshold = 100.0, }, }, + + use_intensities = false, } )text"); return mapping::CreateLocalTrajectoryBuilderOptions3D( @@ -174,7 +200,8 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { return first * (to - from) + from; } - sensor::TimedRangeData GenerateRangeData(const transform::Rigid3d& pose) { + sensor::TimedPointCloudData GeneratePointCloudData( + const transform::Rigid3d& pose, const common::Time time) { // 360 degree rays at 16 angles. sensor::TimedPointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { @@ -214,10 +241,9 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { 0.}; returns_in_world_frame.push_back(return_point); } - return {Eigen::Vector3f::Zero(), + return {time, Eigen::Vector3f::Zero(), sensor::TransformTimedPointCloud(returns_in_world_frame, - pose.inverse().cast()), - {}}; + pose.inverse().cast())}; } void AddLinearOnlyImuObservation(const common::Time time, @@ -255,11 +281,10 @@ class LocalTrajectoryBuilderTest : public ::testing::Test { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); - const auto range_data = GenerateRangeData(node.pose); + const auto point_cloud = GeneratePointCloudData(node.pose, node.time); const std::unique_ptr - matching_result = local_trajectory_builder_->AddRangeData( - kSensorId, sensor::TimedPointCloudData{ - node.time, range_data.origin, range_data.returns}); + matching_result = + local_trajectory_builder_->AddRangeData(kSensorId, point_cloud); if (matching_result != nullptr) { EXPECT_THAT(matching_result->local_pose, transform::IsNearly(node.pose, 1e-1)); diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc index 92c9ed1cc7..860b7d674f 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.cc @@ -20,6 +20,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h" #include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/internal/voxel_filter.h" #include "glog/logging.h" @@ -57,12 +58,15 @@ proto::LocalTrajectoryBuilderOptions3D CreateLocalTrajectoryBuilderOptions3D( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); + *options.mutable_pose_extrapolator_options() = CreatePoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("pose_extrapolator").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_rotational_histogram_size( parameter_dictionary->GetInt("rotational_histogram_size")); *options.mutable_submaps_options() = CreateSubmapsOptions3D( parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_intensities(parameter_dictionary->GetBool("use_intensities")); return options; } diff --git a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h index b01b298b5b..e26a6ec0e4 100644 --- a/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h +++ b/cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h @@ -18,7 +18,7 @@ #define CARTOGRAPHER_MAPPING_INTERNAL_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_3D_H_ #include "cartographer/common/lua_parameter_dictionary.h" -#include "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.pb.h" +#include "cartographer/mapping/proto/local_trajectory_builder_options_3d.pb.h" namespace cartographer { namespace mapping { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index a7e533e2be..8a91e592ec 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -40,6 +40,7 @@ namespace cartographer { namespace mapping { static auto* kWorkQueueDelayMetric = metrics::Gauge::Null(); +static auto* kWorkQueueSizeMetric = metrics::Gauge::Null(); static auto* kConstraintsSameTrajectoryMetric = metrics::Gauge::Null(); static auto* kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null(); static auto* kActiveSubmapsMetric = metrics::Gauge::Null(); @@ -169,6 +170,7 @@ void PoseGraph3D::AddWorkItem( } const auto now = std::chrono::steady_clock::now(); work_queue_->push_back({now, work_item}); + kWorkQueueSizeMetric->Set(work_queue_->size()); kWorkQueueDelayMetric->Set( std::chrono::duration_cast>( now - work_queue_->front().time) @@ -516,6 +518,7 @@ void PoseGraph3D::DrainWorkQueue() { work_item = work_queue_->front().task; work_queue_->pop_front(); work_queue_size = work_queue_->size(); + kWorkQueueSizeMetric->Set(work_queue_size); } process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; } @@ -643,6 +646,22 @@ void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) { absl::MutexLock locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); + // Connect multiple frozen trajectories among each other. + // This is required for localization against multiple frozen trajectories + // because we lose inter-trajectory constraints when freezing. + for (const auto& entry : data_.trajectories_state) { + const int other_trajectory_id = entry.first; + if (!IsTrajectoryFrozen(other_trajectory_id)) { + continue; + } + if (data_.trajectory_connectivity_state.TransitivelyConnected( + trajectory_id, other_trajectory_id)) { + // Already connected, nothing to do. + continue; + } + data_.trajectory_connectivity_state.Connect( + trajectory_id, other_trajectory_id, common::FromUniversal(0)); + } data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; }); @@ -1171,42 +1190,69 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished); - // Compile all nodes that are still INTRA_SUBMAP constrained once the submap - // with 'submap_id' is gone. + // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps + // once the submap with 'submap_id' is gone. + // We need to use node_ids instead of constraints here to be also compatible + // with frozen trajectories that don't have intra-constraints. std::set nodes_to_retain; - for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - constraint.submap_id != submap_id) { - nodes_to_retain.insert(constraint.node_id); + for (const auto& submap_data : parent_->data_.submap_data) { + if (submap_data.id != submap_id) { + nodes_to_retain.insert(submap_data.data.node_ids.begin(), + submap_data.data.node_ids.end()); } } - // Remove all 'data_.constraints' related to 'submap_id'. + + // Remove all nodes that are exlusively associated to 'submap_id'. std::set nodes_to_remove; + std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(), + parent_->data_.submap_data.at(submap_id).node_ids.end(), + nodes_to_retain.begin(), nodes_to_retain.end(), + std::inserter(nodes_to_remove, nodes_to_remove.begin())); + + // Remove all 'data_.constraints' related to 'submap_id'. { std::vector constraints; for (const Constraint& constraint : parent_->data_.constraints) { - if (constraint.submap_id == submap_id) { - if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && - nodes_to_retain.count(constraint.node_id) == 0) { - // This node will no longer be INTRA_SUBMAP contrained and has to be - // removed. - nodes_to_remove.insert(constraint.node_id); - } - } else { + if (constraint.submap_id != submap_id) { constraints.push_back(constraint); } } parent_->data_.constraints = std::move(constraints); } + // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data. @@ -1252,9 +1298,13 @@ void PoseGraph3D::RegisterMetrics(metrics::FamilyFactory* family_factory) { "mapping_3d_pose_graph_work_queue_delay", "Age of the oldest entry in the work queue in seconds"); kWorkQueueDelayMetric = latency->Add({}); + auto* queue_size = + family_factory->NewGaugeFamily("mapping_3d_pose_graph_work_queue_size", + "Number of items in the work queue"); + kWorkQueueSizeMetric = queue_size->Add({}); auto* constraints = family_factory->NewGaugeFamily( "mapping_3d_pose_graph_constraints", - "Current number constraints in the pose graph"); + "Current number of constraints in the pose graph"); kConstraintsDifferentTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}}); kConstraintsSameTrajectoryMetric = diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index f8467aacdd..12a3469a72 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -36,9 +36,9 @@ #include "cartographer/mapping/internal/constraints/constraint_builder_3d.h" #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" +#include "cartographer/mapping/internal/pose_graph_data.h" #include "cartographer/mapping/internal/work_queue.h" #include "cartographer/mapping/pose_graph.h" -#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/fixed_frame_pose_data.h" diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index e40433b31d..0ecc220970 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -323,6 +323,59 @@ TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) { } } +TEST_F(PoseGraph3DTest, EvenSubmapTrimmerOnFrozenTrajectory) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_keep = 10; + const int num_submaps_to_create = 2 * num_submaps_to_keep; + const int num_nodes_per_submap = 3; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = 42 + i; + auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * i + j; + auto node = testing::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node); + // This step is normally done by a MapBuilder when loading frozen state. + pose_graph_->AddNodeToSubmap(NodeId{trajectory_id, node_index}, + SubmapId{trajectory_id, submap_index}); + } + } + pose_graph_->FreezeTrajectory(trajectory_id); + pose_graph_->AddTrimmer(absl::make_unique(trajectory_id)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), 0); + } +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc index 7eb8fb5629..5aab1fd327 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -21,8 +21,9 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" @@ -48,6 +49,24 @@ proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( options.add_occupied_space_weight( parameter_dictionary->GetDouble(lua_identifier)); } + for (int i = 0;; ++i) { + const std::string lua_identifier = + "intensity_cost_function_options_" + std::to_string(i); + if (!parameter_dictionary->HasKey(lua_identifier)) { + break; + } + const auto intensity_cost_function_options_dictionary = + parameter_dictionary->GetDictionary(lua_identifier); + auto* intensity_cost_function_options = + options.add_intensity_cost_function_options(); + intensity_cost_function_options->set_weight( + intensity_cost_function_options_dictionary->GetDouble("weight")); + intensity_cost_function_options->set_huber_scale( + intensity_cost_function_options_dictionary->GetDouble("huber_scale")); + intensity_cost_function_options->set_intensity_threshold( + intensity_cost_function_options_dictionary->GetDouble( + "intensity_threshold")); + } options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( @@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D( void CeresScanMatcher3D::Match( const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* const pose_estimate, - ceres::Solver::Summary* const summary) { + ceres::Solver::Summary* const summary) const { ceres::Problem problem; optimization::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, @@ -91,8 +110,9 @@ void CeresScanMatcher3D::Match( for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = - *point_clouds_and_hybrid_grids[i].first; - const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; + *point_clouds_and_hybrid_grids[i].point_cloud; + const HybridGrid& hybrid_grid = + *point_clouds_and_hybrid_grids[i].hybrid_grid; problem.AddResidualBlock( OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction( options_.occupied_space_weight(i) / @@ -100,7 +120,25 @@ void CeresScanMatcher3D::Match( point_cloud, hybrid_grid), nullptr /* loss function */, ceres_pose.translation(), ceres_pose.rotation()); + if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) { + CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.); + CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.); + CHECK_GT( + options_.intensity_cost_function_options(i).intensity_threshold(), 0); + const IntensityHybridGrid& intensity_hybrid_grid = + *point_clouds_and_hybrid_grids[i].intensity_hybrid_grid; + problem.AddResidualBlock( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + options_.intensity_cost_function_options(i).weight() / + std::sqrt(static_cast(point_cloud.size())), + options_.intensity_cost_function_options(i).intensity_threshold(), + point_cloud, intensity_hybrid_grid), + new ceres::HuberLoss( + options_.intensity_cost_function_options(i).huber_scale()), + ceres_pose.translation(), ceres_pose.rotation()); + } } + CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction( diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h index ffbaa0cdcc..17c6066b4b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h @@ -34,8 +34,11 @@ namespace scan_matching { proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D( common::LuaParameterDictionary* parameter_dictionary); -using PointCloudAndHybridGridPointers = - std::pair; +struct PointCloudAndHybridGridsPointers { + const sensor::PointCloud* point_cloud; + const HybridGrid* hybrid_grid; + const IntensityHybridGrid* intensity_hybrid_grid; // optional +}; // This scan matcher uses Ceres to align scans with an existing map. class CeresScanMatcher3D { @@ -50,10 +53,10 @@ class CeresScanMatcher3D { // 'summary'. void Match(const Eigen::Vector3d& target_translation, const transform::Rigid3d& initial_pose_estimate, - const std::vector& + const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* pose_estimate, - ceres::Solver::Summary* summary); + ceres::Solver::Summary* summary) const; private: const proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc index 52d40258ab..bc0489fbbf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" @@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test { protected: CeresScanMatcher3DTest() : hybrid_grid_(1.f), + intensity_hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { + std::vector points; + std::vector intensities; for (const Eigen::Vector3f& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { - point_cloud_.push_back({point}); + points.push_back({point}); + intensities.push_back(50); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); + intensity_hybrid_grid_.AddIntensity( + intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast() * + point), + 50); } + point_cloud_ = sensor::PointCloud(points, intensities); auto parameter_dictionary = common::MakeDictionary(R"text( return { occupied_space_weight_0 = 1., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 55, + intensity_threshold = 100, + }, translation_weight = 0.01, rotation_weight = 0.1, only_optimize_yaw = false, @@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test { transform::Rigid3d pose; ceres::Solver::Summary summary; - ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose, - {{&point_cloud_, &hybrid_grid_}}, &pose, - &summary); + + IntensityHybridGrid* intensity_hybrid_grid_ptr = + point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_; + + ceres_scan_matcher_->Match( + initial_pose.translation(), initial_pose, + {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose, + &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } HybridGrid hybrid_grid_; + IntensityHybridGrid intensity_hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; proto::CeresScanMatcherOptions3D options_; diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc index da4bb91e77..0a0a895382 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.cc @@ -25,7 +25,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/mapping/internal/3d/scan_matching/low_resolution_matcher.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc index 100a72fd83..df610e3978 100644 --- a/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d_test.cc @@ -22,7 +22,7 @@ #include #include "absl/memory/memory.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/range_data_inserter_3d.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" @@ -40,13 +40,18 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {} void SetUp() override { - point_cloud_ = { - {Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)}, - {Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)}, - {Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)}, - {Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)}, - {Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)}, - {Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}}; + point_cloud_ = sensor::PointCloud({{Eigen::Vector3f(4.f, 0.f, 0.f)}, + {Eigen::Vector3f(4.5f, 0.f, 0.f)}, + {Eigen::Vector3f(5.f, 0.f, 0.f)}, + {Eigen::Vector3f(5.5f, 0.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.f, 0.f)}, + {Eigen::Vector3f(0.f, 4.5f, 0.f)}, + {Eigen::Vector3f(0.f, 5.f, 0.f)}, + {Eigen::Vector3f(0.f, 5.5f, 0.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.f)}, + {Eigen::Vector3f(0.f, 0.f, 4.5f)}, + {Eigen::Vector3f(0.f, 0.f, 5.f)}, + {Eigen::Vector3f(0.f, 0.f, 5.5f)}}); } transform::Rigid3f GetRandomPose() { @@ -89,6 +94,7 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " + "intensity_threshold = 100.0, " "}"); return CreateRangeDataInserterOptions3D(parameter_dictionary.get()); } @@ -101,7 +107,8 @@ class FastCorrelativeScanMatcher3DTest : public ::testing::Test { sensor::RangeData{pose.translation(), sensor::TransformPointCloud(point_cloud_, pose), {}}, - hybrid_grid_.get()); + hybrid_grid_.get(), + /*intensity_hybrid_grid=*/nullptr); hybrid_grid_->FinishUpdate(); return absl::make_unique( @@ -159,7 +166,8 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->Match( transform::Rigid3d::Identity(), transform::Rigid3d::Identity(), - CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; @@ -188,7 +196,9 @@ TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) { const std::unique_ptr low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap( Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(), - CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore); + CreateConstantData( + sensor::PointCloud({{Eigen::Vector3f(42.f, 42.f, 42.f)}})), + kMinScore); EXPECT_THAT(low_resolution_result, testing::IsNull()) << low_resolution_result->low_resolution_score; } diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc new file mode 100644 index 0000000000..fe16ff5dac --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.cc @@ -0,0 +1,25 @@ +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// This method is defined here instead of the header file as it was observed +// that defining it in the header file has a negative impact on the runtime +// performance. +ceres::CostFunction* IntensityCostFunction3D::CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) { + CHECK(!point_cloud.intensities().empty()); + return new ceres::AutoDiffCostFunction< + IntensityCostFunction3D, ceres::DYNAMIC /* residuals */, + 3 /* translation variables */, 4 /* rotation variables */>( + new IntensityCostFunction3D(scaling_factor, intensity_threshold, + point_cloud, hybrid_grid), + point_cloud.size()); +} + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h new file mode 100644 index 0000000000..59f1c29694 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h @@ -0,0 +1,98 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ + +#include "Eigen/Core" +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/ceres.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { + +// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a +// 'translation' and 'rotation'. The cost increases when points fall into space +// for which different intensity has been observed, i.e. at voxels with different +// values. Only points up to a certain threshold are evaluated which is intended +// to ignore data from retroreflections. +class IntensityCostFunction3D { + public: + static ceres::CostFunction* CreateAutoDiffCostFunction( + const double scaling_factor, const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid); + + template + bool operator()(const T* const translation, const T* const rotation, + T* const residual) const { + const transform::Rigid3 transform( + Eigen::Map>(translation), + Eigen::Quaternion(rotation[0], rotation[1], rotation[2], + rotation[3])); + return Evaluate(transform, residual); + } + + private: + IntensityCostFunction3D(const double scaling_factor, + const float intensity_threshold, + const sensor::PointCloud& point_cloud, + const IntensityHybridGrid& hybrid_grid) + : scaling_factor_(scaling_factor), + intensity_threshold_(intensity_threshold), + point_cloud_(point_cloud), + interpolated_grid_(hybrid_grid) {} + + IntensityCostFunction3D(const IntensityCostFunction3D&) = delete; + IntensityCostFunction3D& operator=(const IntensityCostFunction3D&) = delete; + + template + bool Evaluate(const transform::Rigid3& transform, + T* const residual) const { + for (size_t i = 0; i < point_cloud_.size(); ++i) { + if (point_cloud_.intensities()[i] > intensity_threshold_) { + residual[i] = T(0.f); + } else { + const Eigen::Matrix point = point_cloud_[i].position.cast(); + const T intensity = T(point_cloud_.intensities()[i]); + + const Eigen::Matrix world = transform * point; + const T interpolated_intensity = + interpolated_grid_.GetInterpolatedValue(world[0], world[1], + world[2]); + residual[i] = scaling_factor_ * (interpolated_intensity - intensity); + } + } + return true; + } + + const double scaling_factor_; + // We will ignore returns with intensity above this threshold. + const float intensity_threshold_; + const sensor::PointCloud& point_cloud_; + const InterpolatedIntensityGrid interpolated_grid_; +}; + +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc new file mode 100644 index 0000000000..90fc8c4b65 --- /dev/null +++ b/cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d_test.cc @@ -0,0 +1,66 @@ +/* + * Copyright 2019 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h" + +#include +#include + +#include "cartographer/mapping/3d/hybrid_grid.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace scan_matching { +namespace { + +using ::testing::DoubleNear; +using ::testing::ElementsAre; + +TEST(IntensityCostFunction3DTest, SmokeTest) { + const sensor::PointCloud point_cloud( + {{{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}, + {50.f, 100.f, 150.f}); + IntensityHybridGrid hybrid_grid(0.3f); + hybrid_grid.AddIntensity(hybrid_grid.GetCellIndex({0.f, 0.f, 0.f}), 50.f); + + std::unique_ptr cost_function( + IntensityCostFunction3D::CreateAutoDiffCostFunction( + /*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud, + hybrid_grid)); + + const std::array translation{{0., 0., 0.}}; + const std::array rotation{{1., 0., 0., 0.}}; + + const std::array parameter_blocks{ + {translation.data(), rotation.data()}}; + std::array residuals; + + cost_function->Evaluate(parameter_blocks.data(), residuals.data(), + /*jacobians=*/nullptr); + EXPECT_THAT(residuals, + ElementsAre(DoubleNear(0., 1e-9), DoubleNear(-100., 1e-9), + DoubleNear(0., 1e-9))); +} + +} // namespace +} // namespace scan_matching +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h index d2b30d70fe..b38fdbe963 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h @@ -25,22 +25,23 @@ namespace cartographer { namespace mapping { namespace scan_matching { -// Interpolates between HybridGrid probability voxels. We use the tricubic +// Interpolates between HybridGrid voxels. We use the tricubic // interpolation which interpolates the values and has vanishing derivative at // these points. // // This class is templated to work with the autodiff that Ceres provides. // For this reason, it is also important that the interpolation scheme be // continuously differentiable. +template class InterpolatedGrid { public: - explicit InterpolatedGrid(const HybridGrid& hybrid_grid) + explicit InterpolatedGrid(const HybridGridType& hybrid_grid) : hybrid_grid_(hybrid_grid) {} - InterpolatedGrid(const InterpolatedGrid&) = delete; - InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; + InterpolatedGrid(const InterpolatedGrid&) = delete; + InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; - // Returns the interpolated probability at (x, y, z) of the HybridGrid + // Returns the interpolated value at (x, y, z) of the HybridGrid // used to perform the interpolation. // // This is a piecewise, continuously differentiable function. We use the @@ -48,27 +49,27 @@ class InterpolatedGrid { // tensor product volume of piecewise cubic polynomials that interpolate // the values, and have vanishing derivative at the interval boundaries. template - T GetProbability(const T& x, const T& y, const T& z) const { + T GetInterpolatedValue(const T& x, const T& y, const T& z) const { double x1, y1, z1, x2, y2, z2; ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); const Eigen::Array3i index1 = hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); - const double q111 = hybrid_grid_.GetProbability(index1); + const double q111 = GetValue(hybrid_grid_, index1); const double q112 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1)); const double q121 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0)); const double q122 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1)); const double q211 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0)); const double q212 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1)); const double q221 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0)); const double q222 = - hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1)); + GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1)); const T normalized_x = (x - x1) / (x2 - x1); const T normalized_y = (y - y1) / (y2 - y1); @@ -145,9 +146,22 @@ class InterpolatedGrid { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } - const HybridGrid& hybrid_grid_; + static float GetValue(const HybridGrid& probability_grid, + const Eigen::Array3i& index) { + return probability_grid.GetProbability(index); + } + + static float GetValue(const IntensityHybridGrid& intensity_grid, + const Eigen::Array3i& index) { + return intensity_grid.GetIntensity(index); + } + + const HybridGridType& hybrid_grid_; }; +using InterpolatedIntensityGrid = InterpolatedGrid; +using InterpolatedProbabilityGrid = InterpolatedGrid; + } // namespace scan_matching } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc index f11126da42..503b2ecf11 100644 --- a/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/interpolated_grid_test.cc @@ -44,7 +44,7 @@ class InterpolatedGridTest : public ::testing::Test { } HybridGrid hybrid_grid_; - InterpolatedGrid interpolated_grid_; + InterpolatedProbabilityGrid interpolated_grid_; }; TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { @@ -52,7 +52,7 @@ TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { EXPECT_NEAR(GetHybridGridProbability(x, y, z), - interpolated_grid_.GetProbability(x, y, z), 1e-6); + interpolated_grid_.GetInterpolatedValue(x, y, z), 1e-6); } } } @@ -73,10 +73,11 @@ TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { for (double sample = kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample += kSampleStep) { - EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( - x + sample + kSampleStep, y, z) - - interpolated_grid_.GetProbability( - x + sample, y, z))); + EXPECT_LT(0., + grid_difference * (interpolated_grid_.GetInterpolatedValue( + x + sample + kSampleStep, y, z) - + interpolated_grid_.GetInterpolatedValue( + x + sample, y, z))); } } } diff --git a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h index 5e0cb807e2..ff5b65373b 100644 --- a/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h @@ -73,7 +73,7 @@ class OccupiedSpaceCostFunction3D { const Eigen::Matrix world = transform * point_cloud_[i].position.cast(); const T probability = - interpolated_grid_.GetProbability(world[0], world[1], world[2]); + interpolated_grid_.GetInterpolatedValue(world[0], world[1], world[2]); residual[i] = scaling_factor_ * (1. - probability); } return true; @@ -81,7 +81,7 @@ class OccupiedSpaceCostFunction3D { const double scaling_factor_; const sensor::PointCloud& point_cloud_; - const InterpolatedGrid interpolated_grid_; + const InterpolatedProbabilityGrid interpolated_grid_; }; } // namespace scan_matching diff --git a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc index 1d841a9047..da3223d9ea 100644 --- a/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc +++ b/cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping/3d/hybrid_grid.h" #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" diff --git a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc index 712cf897fa..a584ff45cf 100644 --- a/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc +++ b/cartographer/mapping/internal/3d/scan_matching/rotational_scan_matcher.cc @@ -68,7 +68,7 @@ void AddPointCloudSliceToHistogram(const sensor::PointCloud& slice, // will add the angle between points to the histogram with the maximum weight. // This is to reject, e.g., the angles observed on the ceiling and floor. const Eigen::Vector3f centroid = ComputeCentroid(slice); - Eigen::Vector3f last_point_position = slice.front().position; + Eigen::Vector3f last_point_position = slice.points().front().position; for (const sensor::RangefinderPoint& point : slice) { const Eigen::Vector2f delta = (point.position - last_point_position).head<2>(); @@ -140,9 +140,13 @@ RotationalScanMatcher::RotationalScanMatcher(const Eigen::VectorXf* histogram) // rotations of a fractional bucket which is handled by linearly interpolating. Eigen::VectorXf RotationalScanMatcher::RotateHistogram( const Eigen::VectorXf& histogram, const float angle) { + if (histogram.size() == 0) { + return histogram; + } const float rotate_by_buckets = -angle * histogram.size() / M_PI; int full_buckets = common::RoundToInt(rotate_by_buckets - 0.5f); const float fraction = rotate_by_buckets - full_buckets; + CHECK_GT(histogram.size(), 0); while (full_buckets < 0) { full_buckets += histogram.size(); } diff --git a/cartographer/mapping/internal/collated_trajectory_builder.h b/cartographer/mapping/internal/collated_trajectory_builder.h index c999edfaf0..c00a979ad7 100644 --- a/cartographer/mapping/internal/collated_trajectory_builder.h +++ b/cartographer/mapping/internal/collated_trajectory_builder.h @@ -23,9 +23,9 @@ #include #include +#include "cartographer/common/internal/rate_timer.h" #include "cartographer/common/port.h" -#include "cartographer/common/rate_timer.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/sensor/collator_interface.h" diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc index 58e830c1f1..ad3dfb7ec6 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.cc @@ -24,13 +24,15 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_2d.pb.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_2d.pb.h" #include "cartographer/metrics/counter.h" #include "cartographer/metrics/gauge.h" #include "cartographer/metrics/histogram.h" @@ -48,6 +50,7 @@ static auto* kGlobalConstraintsFoundMetric = metrics::Counter::Null(); static auto* kQueueLengthMetric = metrics::Gauge::Null(); static auto* kConstraintScoresMetric = metrics::Histogram::Null(); static auto* kGlobalConstraintScoresMetric = metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) { return transform::Project2D(submap.local_pose()); @@ -60,7 +63,6 @@ ConstraintBuilder2D::ConstraintBuilder2D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder2D::~ConstraintBuilder2D() { @@ -80,7 +82,12 @@ void ConstraintBuilder2D::MaybeAddConstraint( options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -163,6 +170,7 @@ ConstraintBuilder2D::DispatchScanMatcherConstruction(const SubmapId& submap_id, return &submap_scan_matchers_.at(submap_id); } auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); submap_scan_matcher.grid = grid; auto& scan_matcher_options = options_.fast_correlative_scan_matcher_options(); auto scan_matcher_task = absl::make_unique(); @@ -303,6 +311,8 @@ void ConstraintBuilder2D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { @@ -326,6 +336,10 @@ void ConstraintBuilder2D::RegisterMetrics(metrics::FamilyFactory* factory) { "Constraint scores built", boundaries); kConstraintScoresMetric = scores->Add({{"search_region", "local"}}); kGlobalConstraintScoresMetric = scores->Add({{"search_region", "global"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_2d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); } } // namespace constraints diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d.h b/cartographer/mapping/internal/constraints/constraint_builder_2d.h index 272f7dcfce..6667fdb57b 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -160,8 +161,8 @@ class ConstraintBuilder2D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher2D ceres_scan_matcher_; // Histogram of scan matcher scores. diff --git a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc index 066bb8c65e..f80fad9858 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_2d_test.cc @@ -15,11 +15,11 @@ */ #include "cartographer/mapping/internal/constraints/constraint_builder_2d.h" -#include "cartographer/mapping/2d/probability_grid.h" #include #include "cartographer/common/internal/testing/thread_pool_for_testing.h" +#include "cartographer/mapping/2d/probability_grid.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/constraints/constraint_builder.h" #include "cartographer/mapping/internal/testing/test_helpers.h" diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc index ef6dbd4edc..87b3742b53 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.cc +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.cc @@ -24,13 +24,15 @@ #include #include #include +#include +#include #include "Eigen/Eigenvalues" #include "absl/memory/memory.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/proto/scan_matching//ceres_scan_matcher_options_3d.pb.h" -#include "cartographer/mapping/proto/scan_matching//fast_correlative_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.pb.h" +#include "cartographer/mapping/proto/scan_matching/fast_correlative_scan_matcher_options_3d.pb.h" #include "cartographer/metrics/counter.h" #include "cartographer/metrics/gauge.h" #include "cartographer/metrics/histogram.h" @@ -54,6 +56,7 @@ static auto* kGlobalConstraintRotationalScoresMetric = metrics::Histogram::Null(); static auto* kGlobalConstraintLowResolutionScoresMetric = metrics::Histogram::Null(); +static auto* kNumSubmapScanMatchersMetric = metrics::Gauge::Null(); ConstraintBuilder3D::ConstraintBuilder3D( const proto::ConstraintBuilderOptions& options, @@ -62,7 +65,6 @@ ConstraintBuilder3D::ConstraintBuilder3D( thread_pool_(thread_pool), finish_node_task_(absl::make_unique()), when_done_task_(absl::make_unique()), - sampler_(options.sampling_ratio()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder3D::~ConstraintBuilder3D() { @@ -83,7 +85,12 @@ void ConstraintBuilder3D::MaybeAddConstraint( .norm() > options_.max_constraint_distance()) { return; } - if (!sampler_.Pulse()) return; + if (!per_submap_sampler_ + .emplace(std::piecewise_construct, std::forward_as_tuple(submap_id), + std::forward_as_tuple(options_.sampling_ratio())) + .first->second.Pulse()) { + return; + } absl::MutexLock locker(&mutex_); if (when_done_) { @@ -167,6 +174,7 @@ ConstraintBuilder3D::DispatchScanMatcherConstruction(const SubmapId& submap_id, return &submap_scan_matchers_.at(submap_id); } auto& submap_scan_matcher = submap_scan_matchers_[submap_id]; + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); submap_scan_matcher.high_resolution_hybrid_grid = &submap->high_resolution_hybrid_grid(); submap_scan_matcher.low_resolution_hybrid_grid = @@ -259,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint( ceres_scan_matcher_.Match(match_result->pose_estimate.translation(), match_result->pose_estimate, {{&constant_data->high_resolution_point_cloud, - submap_scan_matcher.high_resolution_hybrid_grid}, + submap_scan_matcher.high_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}, {&constant_data->low_resolution_point_cloud, - submap_scan_matcher.low_resolution_hybrid_grid}}, + submap_scan_matcher.low_resolution_hybrid_grid, + /*intensity_hybrid_grid=*/nullptr}}, &constraint_transform, &unused_summary); constraint->reset(new Constraint{ @@ -334,6 +344,8 @@ void ConstraintBuilder3D::DeleteScanMatcher(const SubmapId& submap_id) { << "DeleteScanMatcher was called while WhenDone was scheduled."; } submap_scan_matchers_.erase(submap_id); + per_submap_sampler_.erase(submap_id); + kNumSubmapScanMatchersMetric->Set(submap_scan_matchers_.size()); } void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { @@ -367,6 +379,10 @@ void ConstraintBuilder3D::RegisterMetrics(metrics::FamilyFactory* factory) { scores->Add({{"search_region", "global"}, {"kind", "rotational_score"}}); kGlobalConstraintLowResolutionScoresMetric = scores->Add( {{"search_region", "global"}, {"kind", "low_resolution_score"}}); + auto* num_matchers = factory->NewGaugeFamily( + "mapping_constraints_constraint_builder_3d_num_submap_scan_matchers", + "Current number of constructed submap scan matchers"); + kNumSubmapScanMatchersMetric = num_matchers->Add({}); } } // namespace constraints diff --git a/cartographer/mapping/internal/constraints/constraint_builder_3d.h b/cartographer/mapping/internal/constraints/constraint_builder_3d.h index fb0edc8b05..247a9da62c 100644 --- a/cartographer/mapping/internal/constraints/constraint_builder_3d.h +++ b/cartographer/mapping/internal/constraints/constraint_builder_3d.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "Eigen/Core" @@ -169,8 +170,8 @@ class ConstraintBuilder3D { // Map of dispatched or constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); + std::map per_submap_sampler_; - common::FixedRatioSampler sampler_; scan_matching::CeresScanMatcher3D ceres_scan_matcher_; // Histograms of scan matcher scores. diff --git a/cartographer/pose_graph/node/node.cc b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc similarity index 69% rename from cartographer/pose_graph/node/node.cc rename to cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc index b9293f1bd5..e0edcb72f6 100644 --- a/cartographer/pose_graph/node/node.cc +++ b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.cc @@ -14,18 +14,15 @@ * limitations under the License. */ -#include "cartographer/pose_graph/node/node.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" namespace cartographer { -namespace pose_graph { +namespace mapping { -proto::Node Node::ToProto() const { - proto::Node node; - node.set_constant(constant_); - *node.mutable_id() = node_id_.ToProto(); - *node.mutable_parameters() = ToParametersProto(); - return node; +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b) { + return Eigen::Quaterniond::FromTwoVectors(a, b); } -} // namespace pose_graph +} // namespace mapping } // namespace cartographer diff --git a/cartographer/pose_graph/solver/ceres_solver.h b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h similarity index 52% rename from cartographer/pose_graph/solver/ceres_solver.h rename to cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h index fcaa36544e..e8d04b370d 100644 --- a/cartographer/pose_graph/solver/ceres_solver.h +++ b/cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h @@ -14,28 +14,21 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ -#define CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ +#ifndef CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ +#define CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ -#include "cartographer/pose_graph/solver/solver.h" -#include "ceres/problem.h" -#include "ceres/solver.h" +#include "Eigen/Geometry" namespace cartographer { -namespace pose_graph { +namespace mapping { -class CeresSolver : public Solver { - public: - explicit CeresSolver(const ceres::Solver::Options& options); +// Calls Eigen::Quaterniond::FromTwoVectors(). This is in its own compilation +// unit since it can take more than 10 s to build while using more than 1 GB of +// memory causing slow build times and high peak memory usage. +Eigen::Quaterniond FromTwoVectors(const Eigen::Vector3d& a, + const Eigen::Vector3d& b); - SolverStatus Solve(PoseGraphData* data) const final; - - private: - const ceres::Problem::Options problem_options_; - const ceres::Solver::Options solver_options_; -}; - -} // namespace pose_graph +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_POSE_GRAPH_OPTIMIZER_CERES_SOLVER_H_ +#endif // CARTOGRAPHER_MAPPING_EIGEN_QUATERNIOND_FROM_TWO_VECTORS_H_ diff --git a/cartographer/mapping/internal/global_trajectory_builder.cc b/cartographer/mapping/internal/global_trajectory_builder.cc index 3260f05bfe..a60e88a742 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.cc +++ b/cartographer/mapping/internal/global_trajectory_builder.cc @@ -19,8 +19,10 @@ #include #include "absl/memory/memory.h" +#include "absl/types/optional.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" +#include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/metrics/family_factory.h" #include "glog/logging.h" @@ -39,11 +41,13 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { GlobalTrajectoryBuilder( std::unique_ptr local_trajectory_builder, const int trajectory_id, PoseGraph* const pose_graph, - const LocalSlamResultCallback& local_slam_result_callback) + const LocalSlamResultCallback& local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) : trajectory_id_(trajectory_id), pose_graph_(pose_graph), local_trajectory_builder_(std::move(local_trajectory_builder)), - local_slam_result_callback_(local_slam_result_callback) {} + local_slam_result_callback_(local_slam_result_callback), + pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter) {} ~GlobalTrajectoryBuilder() override {} GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; @@ -97,6 +101,14 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { if (local_trajectory_builder_) { local_trajectory_builder_->AddOdometryData(odometry_data); } + // TODO(MichaelGrupp): Instead of having an optional filter on this level, + // odometry could be marginalized between nodes in the pose graph. + // Related issue: cartographer-project/cartographer/#1768 + if (pose_graph_odometry_motion_filter_.has_value() && + pose_graph_odometry_motion_filter_.value().IsSimilar( + odometry_data.time, odometry_data.pose)) { + return; + } pose_graph_->AddOdometryData(trajectory_id_, odometry_data); } @@ -127,6 +139,7 @@ class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface { PoseGraph* const pose_graph_; std::unique_ptr local_trajectory_builder_; LocalSlamResultCallback local_slam_result_callback_; + absl::optional pose_graph_odometry_motion_filter_; }; } // namespace @@ -135,22 +148,24 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback) { + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, - local_slam_result_callback); + local_slam_result_callback, pose_graph_odometry_motion_filter); } std::unique_ptr CreateGlobalTrajectoryBuilder3D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback) { + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter) { return absl::make_unique< GlobalTrajectoryBuilder>( std::move(local_trajectory_builder), trajectory_id, pose_graph, - local_slam_result_callback); + local_slam_result_callback, pose_graph_odometry_motion_filter); } void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory* factory) { diff --git a/cartographer/mapping/internal/global_trajectory_builder.h b/cartographer/mapping/internal/global_trajectory_builder.h index 754912d59b..c0980f5848 100644 --- a/cartographer/mapping/internal/global_trajectory_builder.h +++ b/cartographer/mapping/internal/global_trajectory_builder.h @@ -23,7 +23,7 @@ #include "cartographer/mapping/internal/2d/pose_graph_2d.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_3d.h" #include "cartographer/mapping/internal/3d/pose_graph_3d.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "cartographer/mapping/trajectory_builder_interface.h" #include "cartographer/metrics/family_factory.h" @@ -34,13 +34,15 @@ std::unique_ptr CreateGlobalTrajectoryBuilder2D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph2D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback); + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); std::unique_ptr CreateGlobalTrajectoryBuilder3D( std::unique_ptr local_trajectory_builder, const int trajectory_id, mapping::PoseGraph3D* const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback& - local_slam_result_callback); + local_slam_result_callback, + const absl::optional& pose_graph_odometry_motion_filter); void GlobalTrajectoryBuilderRegisterMetrics( metrics::FamilyFactory* family_factory); diff --git a/cartographer/mapping/internal/imu_based_pose_extrapolator.cc b/cartographer/mapping/internal/imu_based_pose_extrapolator.cc new file mode 100644 index 0000000000..ddb88ea902 --- /dev/null +++ b/cartographer/mapping/internal/imu_based_pose_extrapolator.cc @@ -0,0 +1,439 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" + +#include + +#include "absl/memory/memory.h" +#include "cartographer/mapping/internal/3d/imu_integration.h" +#include "cartographer/mapping/internal/3d/rotation_parameterization.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h" +#include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/transform/transform.h" +#include "glog/logging.h" + +namespace cartographer { +namespace mapping { + +using ::cartographer::transform::TimestampedTransform; + +ImuBasedPoseExtrapolator::ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options) + : options_(options), + solver_options_( + common::CreateCeresSolverOptions(options_.solver_options())) {} + +ImuBasedPoseExtrapolator::~ImuBasedPoseExtrapolator() { + LOG(INFO) << "Number of iterations for pose extrapolation:"; + LOG(INFO) << num_iterations_hist_.ToString(10); +} + +std::unique_ptr +ImuBasedPoseExtrapolator::InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses) { + CHECK(!imu_data.empty()); + LOG(INFO) << options.DebugString(); + auto extrapolator = absl::make_unique(options); + std::copy(imu_data.begin(), imu_data.end(), + std::back_inserter(extrapolator->imu_data_)); + if (!initial_poses.empty()) { + for (const auto& pose : initial_poses) { + if (pose.time > imu_data.front().time) { + extrapolator->AddPose(pose.time, pose.transform); + } + } + } else { + extrapolator->AddPose( + imu_data.back().time, + transform::Rigid3d::Rotation(FromTwoVectors( + imu_data.back().linear_acceleration, Eigen::Vector3d::UnitZ()))); + } + return extrapolator; +} + +common::Time ImuBasedPoseExtrapolator::GetLastPoseTime() const { + if (timed_pose_queue_.empty()) { + return common::Time::min(); + } + return timed_pose_queue_.back().time; +} + +common::Time ImuBasedPoseExtrapolator::GetLastExtrapolatedTime() const { + return last_extrapolated_time_; +} + +void ImuBasedPoseExtrapolator::AddPose(const common::Time time, + const transform::Rigid3d& pose) { + timed_pose_queue_.push_back(TimestampedTransform{time, pose}); + while (timed_pose_queue_.size() > 3 && + timed_pose_queue_[1].time <= + time - common::FromSeconds(options_.pose_queue_duration())) { + if (!previous_solution_.empty()) { + CHECK_EQ(timed_pose_queue_.front().time, previous_solution_.front().time); + previous_solution_.pop_front(); + } + timed_pose_queue_.pop_front(); + } + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) { + CHECK(timed_pose_queue_.empty() || + imu_data.time >= timed_pose_queue_.back().time); + imu_data_.push_back(imu_data); + TrimImuData(); +} + +void ImuBasedPoseExtrapolator::AddOdometryData( + const sensor::OdometryData& odometry_data) { + CHECK(timed_pose_queue_.empty() || + odometry_data.time >= timed_pose_queue_.back().time); + odometry_data_.push_back(odometry_data); + TrimOdometryData(); +} + +ImuBasedPoseExtrapolator::ExtrapolationResult +ImuBasedPoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + const auto& time = times.back(); + const auto& newest_timed_pose = timed_pose_queue_.back(); + CHECK_GE(time, newest_timed_pose.time); + CHECK_GE(times.size(), 1); + last_extrapolated_time_ = time; + + if (timed_pose_queue_.size() < 3 || + common::ToSeconds(time - newest_timed_pose.time) < 1e-6) { + return ExtrapolationResult{ + std::vector( + times.size() - 1, timed_pose_queue_.back().transform.cast()), + timed_pose_queue_.back().transform, Eigen::Vector3d::Zero(), + timed_pose_queue_.back().transform.rotation()}; + } + + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + + // Track gravity alignment over time and use this as a frame here so that + // we can estimate the gravity alignment of the current pose. + optimization::CeresPose gravity_from_local( + gravity_from_local_, nullptr, + absl::make_unique(), &problem); + // Use deque so addresses stay constant during problem formulation. + std::deque nodes; + std::vector node_times; + + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const bool is_last = (i == (timed_pose_queue_.size() - 1)); + const auto& timed_pose = timed_pose_queue_[i]; + node_times.push_back(timed_pose.time); + + transform::Rigid3d gravity_from_node; + // Use the last scan match result (timed_pose_queue_.back()) for + // initialization here instead of the last result from the optimization. + // This keeps poses from slowly drifting apart due to lack of feedback + // from the scan matching here. + if (!previous_solution_.empty() && !is_last) { + CHECK_GT(previous_solution_.size(), i); + CHECK_EQ(timed_pose.time, previous_solution_[i].time); + gravity_from_node = previous_solution_[i].transform; + } else { + gravity_from_node = gravity_from_local_ * timed_pose.transform; + } + + if (is_last) { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique>(), + &problem); + problem.SetParameterBlockConstant(nodes.back().translation()); + } else { + nodes.emplace_back(gravity_from_node, nullptr, + absl::make_unique(), + &problem); + } + } + + double gravity_constant = 9.8; + bool fix_gravity = false; + if (options_.gravity_constant() > 0) { + fix_gravity = true; + gravity_constant = options_.gravity_constant(); + } + + auto imu_it_prev_prev = imu_data_.begin(); + while (imu_it_prev_prev != std::prev(imu_data_.end()) && + std::next(imu_it_prev_prev)->time <= timed_pose_queue_.back().time) { + ++imu_it_prev_prev; + } + + const TimestampedTransform prev_gravity_from_tracking = + TimestampedTransform{node_times.back(), nodes.back().ToRigid()}; + const TimestampedTransform prev_prev_gravity_from_tracking = + TimestampedTransform{node_times.at(node_times.size() - 2), + nodes.at(nodes.size() - 2).ToRigid()}; + const transform::Rigid3d initial_estimate = + ExtrapolatePoseWithImu( + prev_gravity_from_tracking.transform, prev_gravity_from_tracking.time, + prev_prev_gravity_from_tracking.transform, + prev_prev_gravity_from_tracking.time, + gravity_constant * Eigen::Vector3d::UnitZ(), time, imu_data_, + &imu_it_prev_prev) + .pose; + nodes.emplace_back(initial_estimate, nullptr, + absl::make_unique(), + &problem); + node_times.push_back(time); + + // Add cost functions for node constraints. + for (size_t i = 0; i < timed_pose_queue_.size(); i++) { + const auto& timed_pose = timed_pose_queue_[i]; + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + timed_pose.transform, options_.pose_translation_weight(), + options_.pose_rotation_weight()}), + nullptr /* loss function */, gravity_from_local.rotation(), + gravity_from_local.translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + } + + CHECK(!imu_data_.empty()); + CHECK_LE(imu_data_.front().time, timed_pose_queue_.front().time); + + std::array imu_calibration{{1., 0., 0., 0.}}; + + problem.AddParameterBlock(imu_calibration.data(), 4, + new ceres::QuaternionParameterization()); + problem.SetParameterBlockConstant(imu_calibration.data()); + + auto imu_it = imu_data_.begin(); + CHECK(imu_data_.size() == 1 || + std::next(imu_it)->time > timed_pose_queue_.front().time); + + transform::Rigid3d last_node_odometry; + common::Time last_node_odometry_time; + + for (size_t i = 1; i < nodes.size(); i++) { + const common::Time first_time = node_times[i - 1]; + const common::Time second_time = node_times[i]; + + auto imu_it2 = imu_it; + const IntegrateImuResult result = + IntegrateImu(imu_data_, first_time, second_time, &imu_it); + if ((i + 1) < nodes.size()) { + const common::Time third_time = node_times[i + 1]; + const common::Duration first_duration = second_time - first_time; + const common::Duration second_duration = third_time - second_time; + const common::Time first_center = first_time + first_duration / 2; + const common::Time second_center = second_time + second_duration / 2; + const IntegrateImuResult result_to_first_center = + IntegrateImu(imu_data_, first_time, first_center, &imu_it2); + const IntegrateImuResult result_center_to_center = + IntegrateImu(imu_data_, first_center, second_center, &imu_it2); + // 'delta_velocity' is the change in velocity from the point in time + // halfway between the first and second poses to halfway between + // second and third pose. It is computed from IMU data and still + // contains a delta due to gravity. The orientation of this vector is + // in the IMU frame at the second pose. + const Eigen::Vector3d delta_velocity = + (result.delta_rotation.inverse() * + result_to_first_center.delta_rotation) * + result_center_to_center.delta_velocity; + problem.AddResidualBlock( + AccelerationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_acceleration_weight(), delta_velocity, + common::ToSeconds(first_duration), + common::ToSeconds(second_duration)), + nullptr /* loss function */, nodes.at(i).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).translation(), + nodes.at(i + 1).translation(), &gravity_constant, + imu_calibration.data()); + // TODO(danielsievers): Fix gravity in CostFunction. + if (fix_gravity) { + problem.SetParameterBlockConstant(&gravity_constant); + } else { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&gravity_constant, 0, 0.0); + } + } + problem.AddResidualBlock( + RotationCostFunction3D::CreateAutoDiffCostFunction( + options_.imu_rotation_weight(), result.delta_rotation), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i).rotation(), imu_calibration.data()); + + // Add a relative pose constraint based on the odometry (if available). + if (HasOdometryDataForTime(first_time) && + HasOdometryDataForTime(second_time)) { + // Here keep track of last node odometry to avoid double computation. + // Do this if first loop, or if there were some missing odometry nodes + // then recalculate. + if (i == 1 || last_node_odometry_time != first_time) { + last_node_odometry = InterpolateOdometry(first_time); + last_node_odometry_time = first_time; + } + const transform::Rigid3d current_node_odometry = + InterpolateOdometry(second_time); + const transform::Rigid3d relative_odometry = + CalculateOdometryBetweenNodes(last_node_odometry, + current_node_odometry); + + problem.AddResidualBlock( + optimization::SpaCostFunction3D::CreateAutoDiffCostFunction( + PoseGraphInterface::Constraint::Pose{ + relative_odometry, options_.odometry_translation_weight(), + options_.odometry_rotation_weight()}), + nullptr /* loss function */, nodes.at(i - 1).rotation(), + nodes.at(i - 1).translation(), nodes.at(i).rotation(), + nodes.at(i).translation()); + // Use the current node odometry in the next iteration + last_node_odometry = current_node_odometry; + last_node_odometry_time = second_time; + } + } + + // Solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options_, &problem, &summary); + LOG_IF_EVERY_N(INFO, !fix_gravity, 20) << "Gravity was: " << gravity_constant; + + const auto gravity_estimate = nodes.back().ToRigid().rotation(); + + const auto& last_pose = timed_pose_queue_.back(); + const auto extrapolated_pose = TimestampedTransform{ + time, last_pose.transform * + nodes.at(nodes.size() - 2).ToRigid().inverse() * + nodes.back().ToRigid()}; + + num_iterations_hist_.Add(summary.iterations.size()); + + gravity_from_local_ = gravity_from_local.ToRigid(); + + previous_solution_.clear(); + for (size_t i = 0; i < nodes.size(); ++i) { + previous_solution_.push_back( + TimestampedTransform{node_times.at(i), nodes.at(i).ToRigid()}); + } + + const Eigen::Vector3d current_velocity = + (extrapolated_pose.transform.translation() - + last_pose.transform.translation()) / + common::ToSeconds(time - last_pose.time); + + return ExtrapolationResult{ + InterpolatePoses(last_pose, extrapolated_pose, times.begin(), + std::prev(times.end())), + extrapolated_pose.transform, current_velocity, gravity_estimate}; +} + +std::vector ImuBasedPoseExtrapolator::InterpolatePoses( + const TimestampedTransform& start, const TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end) { + std::vector poses; + poses.reserve(std::distance(times_begin, times_end)); + const float duration_scale = 1. / common::ToSeconds(end.time - start.time); + + const Eigen::Quaternionf start_rotation = + Eigen::Quaternionf(start.transform.rotation()); + const Eigen::Quaternionf end_rotation = + Eigen::Quaternionf(end.transform.rotation()); + const Eigen::Vector3f start_translation = + start.transform.translation().cast(); + const Eigen::Vector3f end_translation = + end.transform.translation().cast(); + + for (auto it = times_begin; it != times_end; ++it) { + const float factor = common::ToSeconds(*it - start.time) * duration_scale; + const Eigen::Vector3f origin = + start_translation + (end_translation - start_translation) * factor; + const Eigen::Quaternionf rotation = + start_rotation.slerp(factor, end_rotation); + poses.emplace_back(origin, rotation); + } + return poses; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::ExtrapolatePose( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .current_pose; +} + +Eigen::Quaterniond ImuBasedPoseExtrapolator::EstimateGravityOrientation( + const common::Time time) { + return ExtrapolatePosesWithGravity(std::vector{time}) + .gravity_from_tracking; +} + +template +void ImuBasedPoseExtrapolator::TrimDequeData(std::deque* data) { + while (data->size() > 1 && !timed_pose_queue_.empty() && + data->at(1).time <= timed_pose_queue_.front().time) { + data->pop_front(); + } +} + +void ImuBasedPoseExtrapolator::TrimImuData() { + TrimDequeData(&imu_data_); +} + +void ImuBasedPoseExtrapolator::TrimOdometryData() { + TrimDequeData(&odometry_data_); +} + +// Odometry methods +bool ImuBasedPoseExtrapolator::HasOdometryData() const { + return odometry_data_.size() >= 2; +} + +bool ImuBasedPoseExtrapolator::HasOdometryDataForTime( + const common::Time& time) const { + return HasOdometryData() && odometry_data_.front().time < time && + time < odometry_data_.back().time; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::InterpolateOdometry( + const common::Time& time) const { + // Only interpolate if time is within odometry data range. + CHECK(HasOdometryDataForTime(time)) + << "Odometry data range does not include time " << time; + std::deque::const_iterator data = std::upper_bound( + odometry_data_.begin(), odometry_data_.end(), time, + [](const common::Time& time, const sensor::OdometryData& odometry_data) { + return time < odometry_data.time; + }); + const TimestampedTransform first{std::prev(data)->time, + std::prev(data)->pose}; + const TimestampedTransform second{data->time, data->pose}; + return Interpolate(first, second, time).transform; +} + +transform::Rigid3d ImuBasedPoseExtrapolator::CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const { + return first_node_odometry.inverse() * second_node_odometry; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/imu_based_pose_extrapolator.h b/cartographer/mapping/internal/imu_based_pose_extrapolator.h new file mode 100644 index 0000000000..6adc9b78e4 --- /dev/null +++ b/cartographer/mapping/internal/imu_based_pose_extrapolator.h @@ -0,0 +1,102 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ +#define CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ + +#include +#include +#include + +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/histogram.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +// Uses the linear acceleration and rotational velocities to estimate a pose. +class ImuBasedPoseExtrapolator : public PoseExtrapolatorInterface { + public: + explicit ImuBasedPoseExtrapolator( + const proto::ImuBasedPoseExtrapolatorOptions& options); + ~ImuBasedPoseExtrapolator() override; + + static std::unique_ptr InitializeWithImu( + const proto::ImuBasedPoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; + + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; + + // Gravity alignment estimate. + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; + + private: + template + void TrimDequeData(std::deque* data); + + void TrimImuData(); + void TrimOdometryData(); + + // Odometry methods. + bool HasOdometryData() const; + bool HasOdometryDataForTime(const common::Time& first_time) const; + transform::Rigid3d InterpolateOdometry(const common::Time& first_time) const; + transform::Rigid3d CalculateOdometryBetweenNodes( + const transform::Rigid3d& first_node_odometry, + const transform::Rigid3d& second_node_odometry) const; + + std::vector InterpolatePoses( + const ::cartographer::transform::TimestampedTransform& start, + const ::cartographer::transform::TimestampedTransform& end, + const std::vector::const_iterator times_begin, + const std::vector::const_iterator times_end); + + std::deque<::cartographer::transform::TimestampedTransform> timed_pose_queue_; + std::deque<::cartographer::transform::TimestampedTransform> + previous_solution_; + + std::deque imu_data_; + std::deque odometry_data_; + common::Time last_extrapolated_time_ = common::Time::min(); + + transform::Rigid3d gravity_from_local_ = transform::Rigid3d::Identity(); + + const proto::ImuBasedPoseExtrapolatorOptions options_; + const ceres::Solver::Options solver_options_; + + common::Histogram num_iterations_hist_; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_IMU_BASED_POSE_EXTRAPOLATOR_H_ diff --git a/cartographer/mapping/local_slam_result_data.h b/cartographer/mapping/internal/local_slam_result_data.h similarity index 100% rename from cartographer/mapping/local_slam_result_data.h rename to cartographer/mapping/internal/local_slam_result_data.h diff --git a/cartographer/mapping/internal/motion_filter_test.cc b/cartographer/mapping/internal/motion_filter_test.cc index 7ecc2d8a01..c5002fafe4 100644 --- a/cartographer/mapping/internal/motion_filter_test.cc +++ b/cartographer/mapping/internal/motion_filter_test.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/internal/motion_filter.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index cd0eafc99b..3faf2556f4 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -24,7 +24,7 @@ #include #include -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" #include "cartographer/mapping/internal/optimization/ceres_pose.h" @@ -42,6 +42,35 @@ namespace { using ::cartographer::mapping::optimization::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; +using TrajectoryData = + ::cartographer::mapping::PoseGraphInterface::TrajectoryData; + +// For fixed frame pose. +std::unique_ptr Interpolate( + const sensor::MapByTime& map_by_time, + const int trajectory_id, const common::Time time) { + const auto it = map_by_time.lower_bound(trajectory_id, time); + if (it == map_by_time.EndOfTrajectory(trajectory_id) || + !it->pose.has_value()) { + return nullptr; + } + if (it == map_by_time.BeginOfTrajectory(trajectory_id)) { + if (it->time == time) { + return absl::make_unique(it->pose.value()); + } + return nullptr; + } + const auto prev_it = std::prev(it); + if (prev_it->pose.has_value()) { + return absl::make_unique( + Interpolate(transform::TimestampedTransform{prev_it->time, + prev_it->pose.value()}, + transform::TimestampedTransform{it->time, it->pose.value()}, + time) + .transform); + } + return nullptr; +} // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the @@ -146,7 +175,8 @@ OptimizationProblem2D::~OptimizationProblem2D() {} void OptimizationProblem2D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - imu_data_.Append(trajectory_id, imu_data); + // IMU data is not used in 2D optimization, so we ignore this part of the + // interface. } void OptimizationProblem2D::AddOdometryData( @@ -154,20 +184,37 @@ void OptimizationProblem2D::AddOdometryData( odometry_data_.Append(trajectory_id, odometry_data); } +void OptimizationProblem2D::AddFixedFramePoseData( + const int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data) { + fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); +} + void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, const NodeSpec2D& node_data) { node_data_.Append(trajectory_id, node_data); + trajectory_data_[trajectory_id]; +} + +void OptimizationProblem2D::SetTrajectoryData( + int trajectory_id, const TrajectoryData& trajectory_data) { + trajectory_data_[trajectory_id] = trajectory_data; } void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, const NodeSpec2D& node_data) { node_data_.Insert(node_id, node_data); + trajectory_data_[node_id.trajectory_id]; } void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { - imu_data_.Trim(node_data_, node_id); + empty_imu_data_.Trim(node_data_, node_id); odometry_data_.Trim(node_data_, node_id); + fixed_frame_pose_data_.Trim(node_data_, node_id); node_data_.Trim(node_id); + if (node_data_.SizeOfTrajectoryOrZero(node_id.trajectory_id) == 0) { + trajectory_data_.erase(node_id.trajectory_id); + } } void OptimizationProblem2D::AddSubmap( @@ -301,6 +348,58 @@ void OptimizationProblem2D::Solve( } } + std::map> C_fixed_frames; + for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { + const int trajectory_id = node_it->id.trajectory_id; + const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) { + node_it = trajectory_end; + continue; + } + + const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id); + bool fixed_frame_pose_initialized = false; + for (; node_it != trajectory_end; ++node_it) { + const NodeId node_id = node_it->id; + const NodeSpec2D& node_data = node_it->data; + + const std::unique_ptr fixed_frame_pose = + Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time); + if (fixed_frame_pose == nullptr) { + continue; + } + + const Constraint::Pose constraint_pose{ + *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), + options_.fixed_frame_pose_rotation_weight()}; + + if (!fixed_frame_pose_initialized) { + transform::Rigid2d fixed_frame_pose_in_map; + if (trajectory_data.fixed_frame_origin_in_map.has_value()) { + fixed_frame_pose_in_map = transform::Project2D( + trajectory_data.fixed_frame_origin_in_map.value()); + } else { + fixed_frame_pose_in_map = + node_data.global_pose_2d * + transform::Project2D(constraint_pose.zbar_ij).inverse(); + } + + C_fixed_frames.emplace(trajectory_id, + FromPose(fixed_frame_pose_in_map)); + fixed_frame_pose_initialized = true; + } + + problem.AddResidualBlock( + CreateAutoDiffSpaCostFunction(constraint_pose), + options_.fixed_frame_pose_use_tolerant_loss() + ? new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) + : nullptr, + C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data()); + } + } + // Solve. ceres::Solver::Summary summary; ceres::Solve( @@ -319,6 +418,10 @@ void OptimizationProblem2D::Solve( node_data_.at(C_node_id_data.id).global_pose_2d = ToPose(C_node_id_data.data); } + for (const auto& C_fixed_frame : C_fixed_frames) { + trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map = + transform::Embed3D(ToPose(C_fixed_frame.second)); + } for (const auto& C_landmark : C_landmarks) { landmark_data_[C_landmark.first] = C_landmark.second.ToRigid(); } diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h index 496f2d9a9a..0a1e39dcd2 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -94,13 +94,28 @@ class OptimizationProblem2D return landmark_data_; } const sensor::MapByTime& imu_data() const override { - return imu_data_; + return empty_imu_data_; } const sensor::MapByTime& odometry_data() const override { return odometry_data_; } + void AddFixedFramePoseData( + int trajectory_id, + const sensor::FixedFramePoseData& fixed_frame_pose_data); + void SetTrajectoryData( + int trajectory_id, + const PoseGraphInterface::TrajectoryData& trajectory_data); + const sensor::MapByTime& fixed_frame_pose_data() + const { + return fixed_frame_pose_data_; + } + const std::map& trajectory_data() + const { + return trajectory_data_; + } + private: std::unique_ptr InterpolateOdometry( int trajectory_id, common::Time time) const; @@ -113,8 +128,10 @@ class OptimizationProblem2D MapById node_data_; MapById submap_data_; std::map landmark_data_; - sensor::MapByTime imu_data_; + sensor::MapByTime empty_imu_data_; sensor::MapByTime odometry_data_; + sensor::MapByTime fixed_frame_pose_data_; + std::map trajectory_data_; }; } // namespace optimization diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc index 1b8f63229a..246707b26d 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -27,7 +27,7 @@ #include "Eigen/Core" #include "absl/memory/memory.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/3d/imu_integration.h" @@ -374,6 +374,8 @@ void OptimizationProblem3D::Solve( auto imu_it = imu_data.begin(); auto prev_node_it = node_it; + + bool gravity_block_added = false; for (++node_it; node_it != trajectory_end; ++node_it) { const NodeId first_node_id = prev_node_it->id; const NodeSpec3D& first_node_data = prev_node_it->data; @@ -395,14 +397,14 @@ void OptimizationProblem3D::Solve( const IntegrateImuResult result = IntegrateImu( imu_data, first_node_data.time, second_node_data.time, &imu_it); const auto next_node_it = std::next(node_it); + const common::Time first_time = first_node_data.time; + const common::Time second_time = second_node_data.time; + const common::Duration first_duration = second_time - first_time; if (next_node_it != trajectory_end && next_node_it->id.node_index == second_node_id.node_index + 1) { const NodeId third_node_id = next_node_it->id; const NodeSpec3D& third_node_data = next_node_it->data; - const common::Time first_time = first_node_data.time; - const common::Time second_time = second_node_data.time; const common::Time third_time = third_node_data.time; - const common::Duration first_duration = second_time - first_time; const common::Duration second_duration = third_time - second_time; const common::Time first_center = first_time + first_duration / 2; const common::Time second_center = second_time + second_duration / 2; @@ -421,8 +423,9 @@ void OptimizationProblem3D::Solve( result_center_to_center.delta_velocity; problem.AddResidualBlock( AccelerationCostFunction3D::CreateAutoDiffCostFunction( - options_.acceleration_weight(), delta_velocity, - common::ToSeconds(first_duration), + options_.acceleration_weight() / + common::ToSeconds(first_duration + second_duration), + delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration)), nullptr /* loss function */, C_nodes.at(second_node_id).rotation(), @@ -431,14 +434,22 @@ void OptimizationProblem3D::Solve( C_nodes.at(third_node_id).translation(), &trajectory_data.gravity_constant, trajectory_data.imu_calibration.data()); + gravity_block_added = true; } problem.AddResidualBlock( RotationCostFunction3D::CreateAutoDiffCostFunction( - options_.rotation_weight(), result.delta_rotation), + options_.rotation_weight() / common::ToSeconds(first_duration), + result.delta_rotation), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), C_nodes.at(second_node_id).rotation(), trajectory_data.imu_calibration.data()); } + + if (gravity_block_added) { + // Force gravity constant to be positive. + problem.SetParameterLowerBound(&trajectory_data.gravity_constant, 0, + 0.0); + } } } @@ -548,7 +559,10 @@ void OptimizationProblem3D::Solve( problem.AddResidualBlock( SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose), - nullptr /* loss function */, + options_.fixed_frame_pose_use_tolerant_loss() ? + new ceres::TolerantLoss( + options_.fixed_frame_pose_tolerant_loss_param_a(), + options_.fixed_frame_pose_tolerant_loss_param_b()) : nullptr, C_fixed_frames.at(trajectory_id).rotation(), C_fixed_frames.at(trajectory_id).translation(), C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation()); diff --git a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 4e3a1293b3..26f9685a42 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -19,7 +19,7 @@ #include #include "Eigen/Core" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping/internal/optimization/optimization_problem_options.h" #include "cartographer/transform/transform.h" @@ -39,8 +39,8 @@ class OptimizationProblem3DTest : public ::testing::Test { optimization::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { - acceleration_weight = 1e-4, - rotation_weight = 1e-2, + acceleration_weight = 2e-5, + rotation_weight = 1e-3, huber_scale = 1., local_slam_pose_translation_weight = 1e-2, local_slam_pose_rotation_weight = 1e-2, @@ -48,6 +48,9 @@ class OptimizationProblem3DTest : public ::testing::Test { odometry_rotation_weight = 1e-2, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = true, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, @@ -131,7 +134,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { Eigen::Vector3d::Zero()}); optimization_problem_.AddTrajectoryNode(kTrajectoryId, NodeSpec3D{now, pose, pose}); - now += common::FromSeconds(0.01); + now += common::FromSeconds(0.1); } std::vector constraints; diff --git a/cartographer/mapping/internal/optimization/optimization_problem_options.cc b/cartographer/mapping/internal/optimization/optimization_problem_options.cc index f32921727a..39a0a561fe 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_options.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_options.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/internal/optimization/optimization_problem_options.h" -#include "cartographer/common/ceres_solver_options.h" +#include "cartographer/common/internal/ceres_solver_options.h" namespace cartographer { namespace mapping { @@ -42,6 +42,12 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); options.set_fixed_frame_pose_rotation_weight( parameter_dictionary->GetDouble("fixed_frame_pose_rotation_weight")); + options.set_fixed_frame_pose_use_tolerant_loss( + parameter_dictionary->GetBool("fixed_frame_pose_use_tolerant_loss")); + options.set_fixed_frame_pose_tolerant_loss_param_a( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_a")); + options.set_fixed_frame_pose_tolerant_loss_param_b( + parameter_dictionary->GetDouble("fixed_frame_pose_tolerant_loss_param_b")); options.set_log_solver_summary( parameter_dictionary->GetBool("log_solver_summary")); options.set_use_online_imu_extrinsics_in_3d( diff --git a/cartographer/mapping/pose_graph_data.h b/cartographer/mapping/internal/pose_graph_data.h similarity index 100% rename from cartographer/mapping/pose_graph_data.h rename to cartographer/mapping/internal/pose_graph_data.h diff --git a/cartographer/mapping/internal/range_data_collator.cc b/cartographer/mapping/internal/range_data_collator.cc index 702565afcf..3ce9e2145d 100644 --- a/cartographer/mapping/internal/range_data_collator.cc +++ b/cartographer/mapping/internal/range_data_collator.cc @@ -19,16 +19,20 @@ #include #include "absl/memory/memory.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" #include "glog/logging.h" namespace cartographer { namespace mapping { +constexpr float RangeDataCollator::kDefaultIntensityValue; + sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data) { + sensor::TimedPointCloudData timed_point_cloud_data) { CHECK_NE(expected_sensor_ids_.count(sensor_id), 0); + timed_point_cloud_data.intensities.resize( + timed_point_cloud_data.ranges.size(), kDefaultIntensityValue); // TODO(gaschler): These two cases can probably be one. if (id_to_pending_data_.count(sensor_id) != 0) { current_start_ = current_end_; @@ -36,10 +40,10 @@ sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData( // the two (do not send out current). current_end_ = id_to_pending_data_.at(sensor_id).time; auto result = CropAndMerge(); - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); return result; } - id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data); + id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data)); if (expected_sensor_ids_.size() != id_to_pending_data_.size()) { return {}; } @@ -59,7 +63,8 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { for (auto it = id_to_pending_data_.begin(); it != id_to_pending_data_.end();) { sensor::TimedPointCloudData& data = it->second; - sensor::TimedPointCloud& ranges = it->second.ranges; + const sensor::TimedPointCloud& ranges = it->second.ranges; + const std::vector& intensities = it->second.intensities; auto overlap_begin = ranges.begin(); while (overlap_begin < ranges.end() && @@ -85,10 +90,14 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { result.origins.push_back(data.origin); const float time_correction = static_cast(common::ToSeconds(data.time - current_end_)); + auto intensities_overlap_it = + intensities.begin() + (overlap_begin - ranges.begin()); + result.ranges.reserve(result.ranges.size() + + std::distance(overlap_begin, overlap_end)); for (auto overlap_it = overlap_begin; overlap_it != overlap_end; - ++overlap_it) { - sensor::TimedPointCloudOriginData::RangeMeasurement point{*overlap_it, - origin_index}; + ++overlap_it, ++intensities_overlap_it) { + sensor::TimedPointCloudOriginData::RangeMeasurement point{ + *overlap_it, *intensities_overlap_it, origin_index}; // current_end_ + point_time[3]_after == in_timestamp + // point_time[3]_before point.point_time.time += time_correction; @@ -102,9 +111,12 @@ sensor::TimedPointCloudOriginData RangeDataCollator::CropAndMerge() { } else if (overlap_end == ranges.begin()) { ++it; } else { + const auto intensities_overlap_end = + intensities.begin() + (overlap_end - ranges.begin()); data = sensor::TimedPointCloudData{ data.time, data.origin, - sensor::TimedPointCloud(overlap_end, ranges.end())}; + sensor::TimedPointCloud(overlap_end, ranges.end()), + std::vector(intensities_overlap_end, intensities.end())}; ++it; } } diff --git a/cartographer/mapping/internal/range_data_collator.h b/cartographer/mapping/internal/range_data_collator.h index 8328828df4..29cf1381ea 100644 --- a/cartographer/mapping/internal/range_data_collator.h +++ b/cartographer/mapping/internal/range_data_collator.h @@ -37,9 +37,11 @@ class RangeDataCollator { : expected_sensor_ids_(expected_range_sensor_ids.begin(), expected_range_sensor_ids.end()) {} + // If timed_point_cloud_data has incomplete intensity data, we will fill the + // missing intensities with kDefaultIntensityValue. sensor::TimedPointCloudOriginData AddRangeData( const std::string& sensor_id, - const sensor::TimedPointCloudData& timed_point_cloud_data); + sensor::TimedPointCloudData timed_point_cloud_data); private: sensor::TimedPointCloudOriginData CropAndMerge(); @@ -49,6 +51,8 @@ class RangeDataCollator { std::map id_to_pending_data_; common::Time current_start_ = common::Time::min(); common::Time current_end_ = common::Time::min(); + + constexpr static float kDefaultIntensityValue = 0.f; }; } // namespace mapping diff --git a/cartographer/mapping/internal/range_data_collator_test.cc b/cartographer/mapping/internal/range_data_collator_test.cc index 8799c19c7a..43ceb00a9d 100644 --- a/cartographer/mapping/internal/range_data_collator_test.cc +++ b/cartographer/mapping/internal/range_data_collator_test.cc @@ -26,16 +26,21 @@ namespace { const int kNumSamples = 10; -sensor::TimedPointCloudData CreateFakeRangeData(int from, int to) { +sensor::TimedPointCloudData CreateFakeRangeData(int from, int to, + bool fake_intensities) { double duration = common::ToSeconds(common::FromUniversal(to) - common::FromUniversal(from)); sensor::TimedPointCloudData result{ - common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}}; + common::FromUniversal(to), Eigen::Vector3f(0., 1., 2.), {}, {}}; result.ranges.reserve(kNumSamples); for (int i = 0; i < kNumSamples; ++i) { double fraction = static_cast(i) / (kNumSamples - 1); - float relative_time = (1.f - fraction) * -duration; - result.ranges.push_back({Eigen::Vector3f{1., 2., 3.}, relative_time}); + float relative_time = (1. - fraction) * -duration; + result.ranges.push_back( + {Eigen::Vector3f{1., 2., static_cast(fraction)}, relative_time}); + if (fake_intensities) { + result.intensities.push_back(result.ranges.back().position.z()); + } } return result; } @@ -49,17 +54,23 @@ bool ArePointTimestampsSorted(const sensor::TimedPointCloudOriginData& data) { return std::is_sorted(timestamps.begin(), timestamps.end()); } +void IntensitiesAreConsistent(const sensor::TimedPointCloudOriginData& data) { + for (const auto& range : data.ranges) { + EXPECT_NEAR(range.point_time.position.z(), range.intensity, 1e-6); + } +} + TEST(RangeDataCollatorTest, SingleSensor) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); auto output_0 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(common::ToUniversal(output_0.time), 300); EXPECT_EQ(output_0.origins.size(), 1); EXPECT_EQ(output_0.ranges.size(), kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_0)); auto output_1 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -69,7 +80,7 @@ TEST(RangeDataCollatorTest, SingleSensor) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -80,13 +91,14 @@ TEST(RangeDataCollatorTest, SingleSensor) { TEST(RangeDataCollatorTest, SingleSensorEmptyData) { const std::string sensor_id = "single_sensor"; RangeDataCollator collator({sensor_id}); - sensor::TimedPointCloudData empty_data{common::FromUniversal(300)}; + sensor::TimedPointCloudData empty_data{ + common::FromUniversal(300), {}, {}, {}}; auto output_0 = collator.AddRangeData(sensor_id, empty_data); EXPECT_EQ(output_0.time, empty_data.time); EXPECT_EQ(output_0.ranges.size(), empty_data.ranges.size()); EXPECT_TRUE(ArePointTimestampsSorted(output_0)); auto output_1 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(common::ToUniversal(output_1.time), 500); EXPECT_EQ(output_1.origins.size(), 1); ASSERT_EQ(output_1.ranges.size(), kNumSamples); @@ -96,7 +108,7 @@ TEST(RangeDataCollatorTest, SingleSensorEmptyData) { common::FromSeconds(output_1.ranges[0].point_time.time)), 300, 2); auto output_2 = - collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510)); + collator.AddRangeData(sensor_id, CreateFakeRangeData(-1000, 510, false)); EXPECT_EQ(common::ToUniversal(output_2.time), 510); EXPECT_EQ(output_2.origins.size(), 1); EXPECT_EQ(output_2.ranges.size(), 1); @@ -109,10 +121,10 @@ TEST(RangeDataCollatorTest, TwoSensors) { const std::string sensor_1 = "sensor_1"; RangeDataCollator collator({sensor_0, sensor_1}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(200, 300, false)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(-1000, 310, false)); EXPECT_EQ(output_1.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_1.time), 300); ASSERT_EQ(output_1.ranges.size(), 2 * kNumSamples - 1); @@ -123,7 +135,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_EQ(output_1.ranges.back().point_time.time, 0.f); EXPECT_TRUE(ArePointTimestampsSorted(output_1)); auto output_2 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(300, 500, false)); EXPECT_EQ(output_2.origins.size(), 2); EXPECT_EQ(common::ToUniversal(output_2.time), 310); ASSERT_EQ(output_2.ranges.size(), 2); @@ -135,7 +147,7 @@ TEST(RangeDataCollatorTest, TwoSensors) { EXPECT_TRUE(ArePointTimestampsSorted(output_2)); // Sending the same sensor will flush everything before. auto output_3 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(600, 700, false)); EXPECT_EQ(common::ToUniversal(output_3.time), 500); EXPECT_EQ( output_1.ranges.size() + output_2.ranges.size() + output_3.ranges.size(), @@ -150,19 +162,42 @@ TEST(RangeDataCollatorTest, ThreeSensors) { const std::string sensor_2 = "sensor_2"; RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); auto output_0 = - collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200)); + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, false)); + EXPECT_EQ(output_0.ranges.size(), 0); + auto output_1 = + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, false)); + EXPECT_EQ(output_1.ranges.size(), 0); + auto output_2 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, false)); + EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); + EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + auto output_3 = + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, false)); + EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); + EXPECT_TRUE(ArePointTimestampsSorted(output_3)); +} + +TEST(RangeDataCollatorTest, ThreeSensorsWithIntensities) { + const std::string sensor_0 = "sensor_0"; + const std::string sensor_1 = "sensor_1"; + const std::string sensor_2 = "sensor_2"; + RangeDataCollator collator({sensor_0, sensor_1, sensor_2}); + auto output_0 = + collator.AddRangeData(sensor_0, CreateFakeRangeData(100, 200, true)); EXPECT_EQ(output_0.ranges.size(), 0); auto output_1 = - collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250)); + collator.AddRangeData(sensor_1, CreateFakeRangeData(199, 250, true)); EXPECT_EQ(output_1.ranges.size(), 0); auto output_2 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(210, 300, true)); EXPECT_EQ(output_2.ranges.size(), kNumSamples + 1); EXPECT_TRUE(ArePointTimestampsSorted(output_2)); + IntensitiesAreConsistent(output_2); auto output_3 = - collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500)); + collator.AddRangeData(sensor_2, CreateFakeRangeData(400, 500, true)); EXPECT_EQ(output_2.ranges.size() + output_3.ranges.size(), 3 * kNumSamples); EXPECT_TRUE(ArePointTimestampsSorted(output_3)); + IntensitiesAreConsistent(output_3); } } // namespace diff --git a/cartographer/mapping/internal/testing/mock_pose_graph.h b/cartographer/mapping/internal/testing/mock_pose_graph.h index 074ac09735..dbddfd1f79 100644 --- a/cartographer/mapping/internal/testing/mock_pose_graph.h +++ b/cartographer/mapping/internal/testing/mock_pose_graph.h @@ -34,6 +34,7 @@ class MockPoseGraph : public mapping::PoseGraphInterface { MOCK_METHOD0(RunFinalOptimization, void()); MOCK_CONST_METHOD0(GetAllSubmapData, mapping::MapById()); + MOCK_CONST_METHOD1(GetSubmapData, SubmapData(const SubmapId&)); MOCK_CONST_METHOD0(GetAllSubmapPoses, mapping::MapById()); MOCK_CONST_METHOD1(GetLocalToGlobalTransform, transform::Rigid3d(int)); diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index f8401dd4f5..53fcd3dc4c 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/cartographer/mapping/internal/testing/test_helpers.cc @@ -19,7 +19,7 @@ #include "absl/memory/memory.h" #include "cartographer/common/config.h" #include "cartographer/common/configuration_file_resolver.h" -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h" #include "cartographer/sensor/timed_point_cloud_data.h" #include "cartographer/transform/transform.h" diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 331ee6ee58..ba8a8b7565 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -17,6 +17,7 @@ #include "cartographer/mapping/map_builder.h" #include "absl/memory/memory.h" +#include "absl/types/optional.h" #include "cartographer/common/time.h" #include "cartographer/io/internal/mapping_state_serialization.h" #include "cartographer/io/proto_stream.h" @@ -28,7 +29,7 @@ #include "cartographer/mapping/internal/3d/pose_graph_3d.h" #include "cartographer/mapping/internal/collated_trajectory_builder.h" #include "cartographer/mapping/internal/global_trajectory_builder.h" -#include "cartographer/mapping/proto/internal/legacy_serialized_data.pb.h" +#include "cartographer/mapping/internal/motion_filter.h" #include "cartographer/sensor/internal/collator.h" #include "cartographer/sensor/internal/trajectory_collator.h" #include "cartographer/sensor/internal/voxel_filter.h" @@ -73,24 +74,6 @@ void MaybeAddPureLocalizationTrimmer( } // namespace -proto::MapBuilderOptions CreateMapBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::MapBuilderOptions options; - options.set_use_trajectory_builder_2d( - parameter_dictionary->GetBool("use_trajectory_builder_2d")); - options.set_use_trajectory_builder_3d( - parameter_dictionary->GetBool("use_trajectory_builder_3d")); - options.set_num_background_threads( - parameter_dictionary->GetNonNegativeInt("num_background_threads")); - options.set_collate_by_trajectory( - parameter_dictionary->GetBool("collate_by_trajectory")); - *options.mutable_pose_graph_options() = CreatePoseGraphOptions( - parameter_dictionary->GetDictionary("pose_graph").get()); - CHECK_NE(options.use_trajectory_builder_2d(), - options.use_trajectory_builder_3d()); - return options; -} - MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { CHECK(options.use_trajectory_builder_2d() ^ @@ -121,6 +104,14 @@ int MapBuilder::AddTrajectoryBuilder( const proto::TrajectoryBuilderOptions& trajectory_options, LocalSlamResultCallback local_slam_result_callback) { const int trajectory_id = trajectory_builders_.size(); + + absl::optional pose_graph_odometry_motion_filter; + if (trajectory_options.has_pose_graph_odometry_motion_filter()) { + LOG(INFO) << "Using a motion filter for adding odometry to the pose graph."; + pose_graph_odometry_motion_filter.emplace( + MotionFilter(trajectory_options.pose_graph_odometry_motion_filter())); + } + if (options_.use_trajectory_builder_3d()) { std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_3d_options()) { @@ -135,7 +126,7 @@ int MapBuilder::AddTrajectoryBuilder( CreateGlobalTrajectoryBuilder3D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } else { std::unique_ptr local_trajectory_builder; if (trajectory_options.has_trajectory_builder_2d_options()) { @@ -150,7 +141,7 @@ int MapBuilder::AddTrajectoryBuilder( CreateGlobalTrajectoryBuilder2D( std::move(local_trajectory_builder), trajectory_id, static_cast(pose_graph_.get()), - local_slam_result_callback))); + local_slam_result_callback, pose_graph_odometry_motion_filter))); } MaybeAddPureLocalizationTrimmer(trajectory_id, trajectory_options, pose_graph_.get()); @@ -285,8 +276,14 @@ std::map MapBuilder::LoadState( true); } - MapById submap_id_to_submap; - MapById node_id_to_node; + if (options_.use_trajectory_builder_3d()) { + CHECK_NE(deserializer.header().format_version(), + io::kFormatVersionWithoutSubmapHistograms) + << "The pbstream file contains submaps without rotational histograms. " + "This can be converted with the 'pbstream migrate' tool, see the " + "Cartographer documentation for details. "; + } + SerializedData proto; while (deserializer.ReadNextSerializedData(&proto)) { switch (proto.data_case()) { @@ -303,10 +300,10 @@ std::map MapBuilder::LoadState( proto.mutable_submap()->mutable_submap_id()->set_trajectory_id( trajectory_remapping.at( proto.submap().submap_id().trajectory_id())); - submap_id_to_submap.Insert( - SubmapId{proto.submap().submap_id().trajectory_id(), - proto.submap().submap_id().submap_index()}, - proto.submap()); + const SubmapId submap_id(proto.submap().submap_id().trajectory_id(), + proto.submap().submap_id().submap_index()); + pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id), + proto.submap()); break; } case SerializedData::kNode: { @@ -316,7 +313,6 @@ std::map MapBuilder::LoadState( proto.node().node_id().node_index()); const transform::Rigid3d& node_pose = node_poses.at(node_id); pose_graph_->AddNodeFromProto(node_pose, proto.node()); - node_id_to_node.Insert(node_id, proto.node()); break; } case SerializedData::kTrajectoryData: { @@ -361,23 +357,9 @@ std::map MapBuilder::LoadState( } } - // TODO(schwoere): Remove backwards compatibility once the pbstream format - // version 2 is established. - if (deserializer.header().format_version() == - io::kFormatVersionWithoutSubmapHistograms) { - submap_id_to_submap = - cartographer::io::MigrateSubmapFormatVersion1ToVersion2( - submap_id_to_submap, node_id_to_node, pose_graph_proto); - } - - for (const auto& submap_id_submap : submap_id_to_submap) { - pose_graph_->AddSubmapFromProto(submap_poses.at(submap_id_submap.id), - submap_id_submap.data); - } - if (load_frozen_state) { // Add information about which nodes belong to which submap. - // Required for 3D pure localization. + // This is required, even without constraints. for (const proto::PoseGraph::Constraint& constraint_proto : pose_graph_proto.constraint()) { if (constraint_proto.tag() != @@ -414,5 +396,10 @@ std::map MapBuilder::LoadStateFromFile( return LoadState(&stream, load_frozen_state); } +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options) { + return absl::make_unique(options); +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder.h b/cartographer/mapping/map_builder.h index 72a8e267ff..ee885a062a 100644 --- a/cartographer/mapping/map_builder.h +++ b/cartographer/mapping/map_builder.h @@ -17,11 +17,10 @@ #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ -#include "cartographer/mapping/map_builder_interface.h" - #include #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/sensor/collator_interface.h" @@ -29,9 +28,6 @@ namespace cartographer { namespace mapping { -proto::MapBuilderOptions CreateMapBuilderOptions( - common::LuaParameterDictionary *const parameter_dictionary); - // Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) // and a PoseGraph for loop closure. class MapBuilder : public MapBuilderInterface { @@ -99,6 +95,9 @@ class MapBuilder : public MapBuilderInterface { all_trajectory_builder_options_; }; +std::unique_ptr CreateMapBuilder( + const proto::MapBuilderOptions& options); + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/map_builder_interface.cc b/cartographer/mapping/map_builder_interface.cc new file mode 100644 index 0000000000..af75f6ccb0 --- /dev/null +++ b/cartographer/mapping/map_builder_interface.cc @@ -0,0 +1,43 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/map_builder_interface.h" + +#include "cartographer/mapping/pose_graph.h" + +namespace cartographer { +namespace mapping { + +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::MapBuilderOptions options; + options.set_use_trajectory_builder_2d( + parameter_dictionary->GetBool("use_trajectory_builder_2d")); + options.set_use_trajectory_builder_3d( + parameter_dictionary->GetBool("use_trajectory_builder_3d")); + options.set_num_background_threads( + parameter_dictionary->GetNonNegativeInt("num_background_threads")); + options.set_collate_by_trajectory( + parameter_dictionary->GetBool("collate_by_trajectory")); + *options.mutable_pose_graph_options() = CreatePoseGraphOptions( + parameter_dictionary->GetDictionary("pose_graph").get()); + CHECK_NE(options.use_trajectory_builder_2d(), + options.use_trajectory_builder_3d()); + return options; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/map_builder_interface.h b/cartographer/mapping/map_builder_interface.h index 8c1cfb144b..76062fb165 100644 --- a/cartographer/mapping/map_builder_interface.h +++ b/cartographer/mapping/map_builder_interface.h @@ -27,6 +27,7 @@ #include "cartographer/io/proto_stream_interface.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" @@ -35,6 +36,9 @@ namespace cartographer { namespace mapping { +proto::MapBuilderOptions CreateMapBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary); + // This interface is used for both library and RPC implementations. // Implementations wire up the complete SLAM stack. class MapBuilderInterface { diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index c079775110..4f205c13dc 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -64,7 +64,7 @@ class MapBuilderTestBase : public T { } void BuildMapBuilder() { - map_builder_ = absl::make_unique(map_builder_options_); + map_builder_ = CreateMapBuilder(map_builder_options_); } void SetOptionsTo3D() { diff --git a/cartographer/mapping/pose_extrapolator.cc b/cartographer/mapping/pose_extrapolator.cc index d02fd5b16b..24e3800887 100644 --- a/cartographer/mapping/pose_extrapolator.cc +++ b/cartographer/mapping/pose_extrapolator.cc @@ -242,5 +242,21 @@ Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { return extrapolation_delta * linear_velocity_from_odometry_; } +PoseExtrapolator::ExtrapolationResult +PoseExtrapolator::ExtrapolatePosesWithGravity( + const std::vector& times) { + std::vector poses; + for (auto it = times.begin(); it != std::prev(times.end()); ++it) { + poses.push_back(ExtrapolatePose(*it).cast()); + } + + const Eigen::Vector3d current_velocity = odometry_data_.size() < 2 + ? linear_velocity_from_poses_ + : linear_velocity_from_odometry_; + return ExtrapolationResult{poses, ExtrapolatePose(times.back()), + current_velocity, + EstimateGravityOrientation(times.back())}; +} + } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_extrapolator.h b/cartographer/mapping/pose_extrapolator.h index 0b14c12af2..1c6885d994 100644 --- a/cartographer/mapping/pose_extrapolator.h +++ b/cartographer/mapping/pose_extrapolator.h @@ -22,6 +22,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/imu_tracker.h" +#include "cartographer/mapping/pose_extrapolator_interface.h" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/rigid_transform.h" @@ -32,7 +33,7 @@ namespace mapping { // Keep poses for a certain duration to estimate linear and angular velocity. // Uses the velocities to extrapolate motion. Uses IMU and/or odometry data if // available to improve the extrapolation. -class PoseExtrapolator { +class PoseExtrapolator : public PoseExtrapolatorInterface { public: explicit PoseExtrapolator(common::Duration pose_queue_duration, double imu_gravity_time_constant); @@ -46,17 +47,20 @@ class PoseExtrapolator { // Returns the time of the last added pose or Time::min() if no pose was added // yet. - common::Time GetLastPoseTime() const; - common::Time GetLastExtrapolatedTime() const; + common::Time GetLastPoseTime() const override; + common::Time GetLastExtrapolatedTime() const override; - void AddPose(common::Time time, const transform::Rigid3d& pose); - void AddImuData(const sensor::ImuData& imu_data); - void AddOdometryData(const sensor::OdometryData& odometry_data); - transform::Rigid3d ExtrapolatePose(common::Time time); + void AddPose(common::Time time, const transform::Rigid3d& pose) override; + void AddImuData(const sensor::ImuData& imu_data) override; + void AddOdometryData(const sensor::OdometryData& odometry_data) override; + transform::Rigid3d ExtrapolatePose(common::Time time) override; + + ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) override; // Returns the current gravity alignment estimate as a rotation from // the tracking frame into a gravity aligned frame. - Eigen::Quaterniond EstimateGravityOrientation(common::Time time); + Eigen::Quaterniond EstimateGravityOrientation(common::Time time) override; private: void UpdateVelocitiesFromPoses(); diff --git a/cartographer/mapping/pose_extrapolator_interface.cc b/cartographer/mapping/pose_extrapolator_interface.cc new file mode 100644 index 0000000000..e408d3a018 --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.cc @@ -0,0 +1,99 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/pose_extrapolator_interface.h" + +#include "cartographer/common/internal/ceres_solver_options.h" +#include "cartographer/common/time.h" +#include "cartographer/mapping/internal/imu_based_pose_extrapolator.h" +#include "cartographer/mapping/pose_extrapolator.h" + +namespace cartographer { +namespace mapping { + +namespace { + +proto::ConstantVelocityPoseExtrapolatorOptions +CreateConstantVelocityPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ConstantVelocityPoseExtrapolatorOptions options; + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + return options; +} + +proto::ImuBasedPoseExtrapolatorOptions CreateImuBasedPoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::ImuBasedPoseExtrapolatorOptions options; + + options.set_pose_queue_duration( + parameter_dictionary->GetDouble("pose_queue_duration")); + options.set_gravity_constant( + parameter_dictionary->GetDouble("gravity_constant")); + options.set_pose_translation_weight( + parameter_dictionary->GetDouble("pose_translation_weight")); + options.set_pose_rotation_weight( + parameter_dictionary->GetDouble("pose_rotation_weight")); + options.set_imu_acceleration_weight( + parameter_dictionary->GetDouble("imu_acceleration_weight")); + options.set_imu_rotation_weight( + parameter_dictionary->GetDouble("imu_rotation_weight")); + options.set_odometry_rotation_weight( + parameter_dictionary->GetDouble("odometry_rotation_weight")); + options.set_odometry_translation_weight( + parameter_dictionary->GetDouble("odometry_translation_weight")); + *options.mutable_solver_options() = CreateCeresSolverOptionsProto( + parameter_dictionary->GetDictionary("solver_options").get()); + + return options; +} + +} // namespace + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::PoseExtrapolatorOptions options; + options.set_use_imu_based(parameter_dictionary->GetBool("use_imu_based")); + *options.mutable_constant_velocity() = + CreateConstantVelocityPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("constant_velocity").get()); + *options.mutable_imu_based() = CreateImuBasedPoseExtrapolatorOptions( + parameter_dictionary->GetDictionary("imu_based").get()); + + return options; +} + +std::unique_ptr +PoseExtrapolatorInterface::CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses) { + CHECK(!imu_data.empty()); + if (options.use_imu_based()) { + return ImuBasedPoseExtrapolator::InitializeWithImu(options.imu_based(), + imu_data, initial_poses); + } else { + return PoseExtrapolator::InitializeWithImu( + common::FromSeconds(options.constant_velocity().pose_queue_duration()), + options.constant_velocity().imu_gravity_time_constant(), + imu_data.back()); + } +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/pose_extrapolator_interface.h b/cartographer/mapping/pose_extrapolator_interface.h new file mode 100644 index 0000000000..bf258ce677 --- /dev/null +++ b/cartographer/mapping/pose_extrapolator_interface.h @@ -0,0 +1,82 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ + +#include +#include + +#include "cartographer/common/time.h" +#include "cartographer/mapping/proto/pose_extrapolator_options.pb.h" +#include "cartographer/sensor/imu_data.h" +#include "cartographer/sensor/odometry_data.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/timestamped_transform.h" + +namespace cartographer { +namespace mapping { + +proto::PoseExtrapolatorOptions CreatePoseExtrapolatorOptions( + common::LuaParameterDictionary* const parameter_dictionary); + +class PoseExtrapolatorInterface { + public: + struct ExtrapolationResult { + // The poses for the requested times at index 0 to N-1. + std::vector previous_poses; + // The pose for the requested time at index N. + transform::Rigid3d current_pose; + Eigen::Vector3d current_velocity; + Eigen::Quaterniond gravity_from_tracking; + }; + + PoseExtrapolatorInterface(const PoseExtrapolatorInterface&) = delete; + PoseExtrapolatorInterface& operator=(const PoseExtrapolatorInterface&) = + delete; + virtual ~PoseExtrapolatorInterface() {} + + // TODO: Remove dependency cycle. + static std::unique_ptr CreateWithImuData( + const proto::PoseExtrapolatorOptions& options, + const std::vector& imu_data, + const std::vector& initial_poses); + + // Returns the time of the last added pose or Time::min() if no pose was added + // yet. + virtual common::Time GetLastPoseTime() const = 0; + virtual common::Time GetLastExtrapolatedTime() const = 0; + + virtual void AddPose(common::Time time, const transform::Rigid3d& pose) = 0; + virtual void AddImuData(const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(const sensor::OdometryData& odometry_data) = 0; + virtual transform::Rigid3d ExtrapolatePose(common::Time time) = 0; + + virtual ExtrapolationResult ExtrapolatePosesWithGravity( + const std::vector& times) = 0; + + // Returns the current gravity alignment estimate as a rotation from + // the tracking frame into a gravity aligned frame. + virtual Eigen::Quaterniond EstimateGravityOrientation(common::Time time) = 0; + + protected: + PoseExtrapolatorInterface() {} +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_EXTRAPOLATOR_INTERFACE_H_ diff --git a/cartographer/mapping/pose_extrapolator_test.cc b/cartographer/mapping/pose_extrapolator_test.cc index fe17e1bc22..00ef86fa7b 100644 --- a/cartographer/mapping/pose_extrapolator_test.cc +++ b/cartographer/mapping/pose_extrapolator_test.cc @@ -18,6 +18,7 @@ #include "Eigen/Geometry" #include "absl/memory/memory.h" +#include "cartographer/mapping/internal/eigen_quaterniond_from_two_vectors.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" @@ -71,9 +72,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { angular_velocity}; auto extrapolator = PoseExtrapolator::InitializeWithImu( common::FromSeconds(kPoseQueueDuration), kGravityTimeConstant, imu_data); - Eigen::Quaterniond expected_orientation; - expected_orientation.setFromTwoVectors(initial_gravity_acceleration, - Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond expected_orientation = + FromTwoVectors(initial_gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation), @@ -84,8 +84,8 @@ TEST(PoseExtrapolatorTest, EstimateGravityOrientationWithIMU) { extrapolator->AddImuData( sensor::ImuData{current_time, gravity_acceleration, angular_velocity}); } - expected_orientation.setFromTwoVectors(gravity_acceleration, - Eigen::Vector3d::UnitZ()); + expected_orientation = + FromTwoVectors(gravity_acceleration, Eigen::Vector3d::UnitZ()); EXPECT_NEAR(0., extrapolator->EstimateGravityOrientation(current_time) .angularDistance(expected_orientation), diff --git a/cartographer/mapping/pose_graph.h b/cartographer/mapping/pose_graph.h index d6597b0e0c..b4dfd3bbec 100644 --- a/cartographer/mapping/pose_graph.h +++ b/cartographer/mapping/pose_graph.h @@ -112,11 +112,6 @@ class PoseGraph : public PoseGraphInterface { // Gets the current trajectory clusters. virtual std::vector> GetConnectedTrajectories() const = 0; - // Returns the current optimized transform and submap itself for the given - // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does - // not exist (anymore). - virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; - proto::PoseGraph ToProto(bool include_unfinished_submaps) const override; // Returns the IMU data. diff --git a/cartographer/mapping/pose_graph_interface.h b/cartographer/mapping/pose_graph_interface.h index 2701e93d46..68551f1579 100644 --- a/cartographer/mapping/pose_graph_interface.h +++ b/cartographer/mapping/pose_graph_interface.h @@ -99,6 +99,11 @@ class PoseGraphInterface { // Returns data for all submaps. virtual MapById GetAllSubmapData() const = 0; + // Returns the current optimized transform and submap itself for the given + // 'submap_id'. Returns 'nullptr' for the 'submap' member if the submap does + // not exist (anymore). + virtual SubmapData GetSubmapData(const SubmapId& submap_id) const = 0; + // Returns the global poses for all submaps. virtual MapById GetAllSubmapPoses() const = 0; diff --git a/cartographer/mapping/proto/2d/cell_limits.proto b/cartographer/mapping/proto/cell_limits_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/cell_limits.proto rename to cartographer/mapping/proto/cell_limits_2d.proto diff --git a/cartographer/mapping/proto/2d/grid_2d.proto b/cartographer/mapping/proto/grid_2d.proto similarity index 86% rename from cartographer/mapping/proto/2d/grid_2d.proto rename to cartographer/mapping/proto/grid_2d.proto index 70b246d25f..010d5b6081 100644 --- a/cartographer/mapping/proto/2d/grid_2d.proto +++ b/cartographer/mapping/proto/grid_2d.proto @@ -16,9 +16,9 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/map_limits.proto"; -import "cartographer/mapping/proto/2d/probability_grid.proto"; -import "cartographer/mapping/proto/2d/tsdf_2d.proto"; +import "cartographer/mapping/proto/map_limits.proto"; +import "cartographer/mapping/proto/probability_grid.proto"; +import "cartographer/mapping/proto/tsdf_2d.proto"; message Grid2D { message CellBox { diff --git a/cartographer/mapping/proto/2d/grid_2d_options.proto b/cartographer/mapping/proto/grid_2d_options.proto similarity index 100% rename from cartographer/mapping/proto/2d/grid_2d_options.proto rename to cartographer/mapping/proto/grid_2d_options.proto diff --git a/cartographer/mapping/proto/3d/hybrid_grid.proto b/cartographer/mapping/proto/hybrid_grid.proto similarity index 100% rename from cartographer/mapping/proto/3d/hybrid_grid.proto rename to cartographer/mapping/proto/hybrid_grid.proto diff --git a/cartographer/mapping/proto/internal/legacy_probability_grid.proto b/cartographer/mapping/proto/internal/legacy_probability_grid.proto deleted file mode 100644 index 58d9d68ff4..0000000000 --- a/cartographer/mapping/proto/internal/legacy_probability_grid.proto +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright 2016 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/2d/map_limits.proto"; - -// The legacy probability grid that was used before the generalized -// grid structure was introduced. - -message LegacyProbabilityGrid { - message CellBox { - int32 max_x = 1; - int32 max_y = 2; - int32 min_x = 3; - int32 min_y = 4; - } - - MapLimits limits = 1; - // These values are actually int16s, but protos don't have a native int16 - // type. - repeated int32 cells = 2; - CellBox known_cells_box = 8; -} diff --git a/cartographer/mapping/proto/internal/legacy_serialized_data.proto b/cartographer/mapping/proto/internal/legacy_serialized_data.proto deleted file mode 100644 index 56f9c6a89f..0000000000 --- a/cartographer/mapping/proto/internal/legacy_serialized_data.proto +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/serialization.proto"; -import "cartographer/mapping/proto/internal/legacy_submap.proto"; - -message LegacySerializedData { - Submap submap = 1; - Node node = 2; - ImuData imu_data = 3; - OdometryData odometry_data = 4; - FixedFramePoseData fixed_frame_pose_data = 5; - TrajectoryData trajectory_data = 6; - LandmarkData landmark_data = 7; -} - -// For backwards compatibility with serialized data containing the legacy -// submap format that did not yet use the generalized 2D grid format. -message LegacySerializedDataLegacySubmap { - LegacySubmap submap = 1; - Node node = 2; - ImuData imu_data = 3; - OdometryData odometry_data = 4; - FixedFramePoseData fixed_frame_pose_data = 5; - TrajectoryData trajectory_data = 6; - LandmarkData landmark_data = 7; -} diff --git a/cartographer/mapping/proto/internal/legacy_submap.proto b/cartographer/mapping/proto/internal/legacy_submap.proto deleted file mode 100644 index 31ed41f888..0000000000 --- a/cartographer/mapping/proto/internal/legacy_submap.proto +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2016 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -package cartographer.mapping.proto; - -import "cartographer/mapping/proto/pose_graph.proto"; -import "cartographer/mapping/proto/internal/legacy_probability_grid.proto"; -import "cartographer/mapping/proto/3d/hybrid_grid.proto"; -import "cartographer/transform/proto/transform.proto"; - -// Serialized state of a Submap2D. -// Uses the legacy, non-generalized probability grid format. -message LegacySubmap2D { - transform.proto.Rigid3d local_pose = 1; - int32 num_range_data = 2; - bool finished = 3; - LegacyProbabilityGrid probability_grid = 4; -} - -// Serialized state of a Submap3D. -message LegacySubmap3D { - transform.proto.Rigid3d local_pose = 1; - int32 num_range_data = 2; - bool finished = 3; - HybridGrid high_resolution_hybrid_grid = 4; - HybridGrid low_resolution_hybrid_grid = 5; -} - -message LegacySubmap { - SubmapId submap_id = 1; - LegacySubmap2D submap_2d = 2; - LegacySubmap3D submap_3d = 3; -} diff --git a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto similarity index 89% rename from cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto rename to cartographer/mapping/proto/local_trajectory_builder_options_2d.proto index 21890916ae..6add120384 100644 --- a/cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_2d.proto @@ -17,11 +17,13 @@ syntax = "proto3"; package cartographer.mapping.proto; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; -import "cartographer/mapping/proto/2d/submaps_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_2d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_2d.proto"; +// NEXT ID: 22 message LocalTrajectoryBuilderOptions2D { // Rangefinder points outside these ranges will be dropped. float min_range = 14; @@ -63,7 +65,10 @@ message LocalTrajectoryBuilderOptions2D { // the constant is reduced) and // 2. from integration of angular velocities (which gets worse when the // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 21; SubmapsOptions2D submaps_options = 11; diff --git a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto similarity index 80% rename from cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto rename to cartographer/mapping/proto/local_trajectory_builder_options_3d.proto index 5bdd4f2bbb..2ff762a707 100644 --- a/cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto +++ b/cartographer/mapping/proto/local_trajectory_builder_options_3d.proto @@ -16,13 +16,16 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/3d/submaps_options_3d.proto"; import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/pose_extrapolator_options.proto"; import "cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto"; import "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.proto"; +import "cartographer/mapping/proto/submaps_options_3d.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; +import "cartographer/sensor/proto/sensor.proto"; +import "cartographer/transform/proto/timestamped_transform.proto"; -// NEXT ID: 18 +// NEXT ID: 22 message LocalTrajectoryBuilderOptions3D { // Rangefinder points outside these ranges will be dropped. float min_range = 1; @@ -57,10 +60,20 @@ message LocalTrajectoryBuilderOptions3D { // the constant is reduced) and // 2. from integration of angular velocities (which gets worse when the // constant is increased) is balanced. + // TODO(schwoere,wohe): Remove this constant. This is only used for ROS and + // was replaced by pose_extrapolator_options. double imu_gravity_time_constant = 15; // Number of histogram buckets for the rotational scan matcher. int32 rotational_histogram_size = 17; + mapping.proto.PoseExtrapolatorOptions pose_extrapolator_options = 18; + + repeated transform.proto.TimestampedTransform initial_poses = 19; + repeated sensor.proto.ImuData initial_imu_data = 20; + SubmapsOptions3D submaps_options = 8; + + // Whether to use Lidar intensities in Ceres Scan Matcher. + bool use_intensities = 21; } diff --git a/cartographer/mapping/proto/2d/map_limits.proto b/cartographer/mapping/proto/map_limits.proto similarity index 93% rename from cartographer/mapping/proto/2d/map_limits.proto rename to cartographer/mapping/proto/map_limits.proto index 56f0a99076..53044e6a75 100644 --- a/cartographer/mapping/proto/2d/map_limits.proto +++ b/cartographer/mapping/proto/map_limits.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/cell_limits.proto"; +import "cartographer/mapping/proto/cell_limits_2d.proto"; import "cartographer/transform/proto/transform.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/2d/normal_estimation_options_2d.proto b/cartographer/mapping/proto/normal_estimation_options_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/normal_estimation_options_2d.proto rename to cartographer/mapping/proto/normal_estimation_options_2d.proto diff --git a/cartographer/mapping/proto/pose_extrapolator_options.proto b/cartographer/mapping/proto/pose_extrapolator_options.proto new file mode 100644 index 0000000000..ff6a4538d7 --- /dev/null +++ b/cartographer/mapping/proto/pose_extrapolator_options.proto @@ -0,0 +1,48 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +import "cartographer/common/proto/ceres_solver_options.proto"; + +package cartographer.mapping.proto; + +message ConstantVelocityPoseExtrapolatorOptions { + // Time constant in seconds for the orientation moving average based on + // observed gravity via the IMU. It should be chosen so that the error + // 1. from acceleration measurements not due to gravity (which gets worse when + // the constant is reduced) and + // 2. from integration of angular velocities (which gets worse when the + // constant is increased) is balanced. + double imu_gravity_time_constant = 1; + double pose_queue_duration = 2; +} + +message ImuBasedPoseExtrapolatorOptions { + double pose_queue_duration = 1; + double gravity_constant = 2; + double pose_translation_weight = 3; + double pose_rotation_weight = 4; + double imu_acceleration_weight = 5; + double imu_rotation_weight = 6; + cartographer.common.proto.CeresSolverOptions solver_options = 7; + double odometry_translation_weight = 8; + double odometry_rotation_weight = 9; +} + +message PoseExtrapolatorOptions { + bool use_imu_based = 1; + ConstantVelocityPoseExtrapolatorOptions constant_velocity = 2; + ImuBasedPoseExtrapolatorOptions imu_based = 3; +} diff --git a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto index 6374fb66b0..efc61c184c 100644 --- a/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto +++ b/cartographer/mapping/proto/pose_graph/optimization_problem_options.proto @@ -18,8 +18,9 @@ package cartographer.mapping.optimization.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 19 +// NEXT ID: 26 message OptimizationProblemOptions { + reserved 20 to 22; // For visual constraints. // Scaling parameter for Huber loss function. double huber_scale = 1; @@ -45,12 +46,20 @@ message OptimizationProblemOptions { // odometry. double odometry_rotation_weight = 17; - // Scaling parameter for the FixedFramePose translation. + // Scaling parameter for the FixedFramePose translation. Unit: 1/meters. double fixed_frame_pose_translation_weight = 11; // Scaling parameter for the FixedFramePose rotation. double fixed_frame_pose_rotation_weight = 12; + bool fixed_frame_pose_use_tolerant_loss = 23; + // The following parameters are used only if fixed_frame_pose_use_tolerant_loss is true. + // See http://ceres-solver.org/nnls_modeling.html. + // For large values of s, the tolerant loss function approaches a null loss + // with fixed_frame_pose_translation_weight. + double fixed_frame_pose_tolerant_loss_param_a = 24; + double fixed_frame_pose_tolerant_loss_param_b = 25; + // 3D only: fix Z. bool fix_z_in_3d = 13; diff --git a/cartographer/mapping/proto/2d/probability_grid.proto b/cartographer/mapping/proto/probability_grid.proto similarity index 100% rename from cartographer/mapping/proto/2d/probability_grid.proto rename to cartographer/mapping/proto/probability_grid.proto diff --git a/cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto b/cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto rename to cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto diff --git a/cartographer/mapping/proto/range_data_inserter_options.proto b/cartographer/mapping/proto/range_data_inserter_options.proto index 7b0d5b1049..9470f2c8f2 100644 --- a/cartographer/mapping/proto/range_data_inserter_options.proto +++ b/cartographer/mapping/proto/range_data_inserter_options.proto @@ -14,8 +14,8 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto"; -import "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/probability_grid_range_data_inserter_options_2d.proto"; +import "cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto b/cartographer/mapping/proto/range_data_inserter_options_3d.proto similarity index 90% rename from cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto rename to cartographer/mapping/proto/range_data_inserter_options_3d.proto index 5fcf1985ff..fd5a44a54c 100644 --- a/cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto +++ b/cartographer/mapping/proto/range_data_inserter_options_3d.proto @@ -28,4 +28,7 @@ message RangeDataInserterOptions3D { // Up to how many free space voxels are updated for scan matching. // 0 disables free space. int32 num_free_space_voxels = 3; + + // Do not insert intensities above this threshold into IntensityHybridGrid. + float intensity_threshold = 4; } diff --git a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto index 8c9030a83a..0f80184159 100644 --- a/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto +++ b/cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto @@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 7 +message IntensityCostFunctionOptions { + double weight = 1; + double huber_scale = 2; + // Ignore ranges with intensity above this threshold. + float intensity_threshold = 3; +} + +// NEXT ID: 8 message CeresScanMatcherOptions3D { - // Scaling parameters for each cost functor. + // Scaling parameters for each occupied space cost functor. repeated double occupied_space_weight = 1; double translation_weight = 2; double rotation_weight = 3; @@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D { // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ common.proto.CeresSolverOptions ceres_solver_options = 6; + + // Scaling parameters for each intensity cost functor. + repeated IntensityCostFunctionOptions intensity_cost_function_options = 7; } diff --git a/cartographer/mapping/proto/submap.proto b/cartographer/mapping/proto/submap.proto index 72b4e627c9..50331278cd 100644 --- a/cartographer/mapping/proto/submap.proto +++ b/cartographer/mapping/proto/submap.proto @@ -16,8 +16,8 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/grid_2d.proto"; -import "cartographer/mapping/proto/3d/hybrid_grid.proto"; +import "cartographer/mapping/proto/grid_2d.proto"; +import "cartographer/mapping/proto/hybrid_grid.proto"; import "cartographer/transform/proto/transform.proto"; // Serialized state of a Submap2D. diff --git a/cartographer/mapping/proto/2d/submaps_options_2d.proto b/cartographer/mapping/proto/submaps_options_2d.proto similarity index 94% rename from cartographer/mapping/proto/2d/submaps_options_2d.proto rename to cartographer/mapping/proto/submaps_options_2d.proto index ebb3c58900..65838da82a 100644 --- a/cartographer/mapping/proto/2d/submaps_options_2d.proto +++ b/cartographer/mapping/proto/submaps_options_2d.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/2d/grid_2d_options.proto"; +import "cartographer/mapping/proto/grid_2d_options.proto"; import "cartographer/mapping/proto/range_data_inserter_options.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/3d/submaps_options_3d.proto b/cartographer/mapping/proto/submaps_options_3d.proto similarity index 94% rename from cartographer/mapping/proto/3d/submaps_options_3d.proto rename to cartographer/mapping/proto/submaps_options_3d.proto index 29b4db8665..822c6d9ee1 100644 --- a/cartographer/mapping/proto/3d/submaps_options_3d.proto +++ b/cartographer/mapping/proto/submaps_options_3d.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -import "cartographer/mapping/proto/3d/range_data_inserter_options_3d.proto"; +import "cartographer/mapping/proto/range_data_inserter_options_3d.proto"; package cartographer.mapping.proto; diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index d6d9c2f8dc..13f034470a 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -15,8 +15,9 @@ syntax = "proto3"; import "cartographer/transform/proto/transform.proto"; -import "cartographer/mapping/proto/2d/local_trajectory_builder_options_2d.proto"; -import "cartographer/mapping/proto/3d/local_trajectory_builder_options_3d.proto"; +import "cartographer/mapping/proto/motion_filter_options.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_2d.proto"; +import "cartographer/mapping/proto/local_trajectory_builder_options_3d.proto"; package cartographer.mapping.proto; @@ -41,6 +42,8 @@ message TrajectoryBuilderOptions { bool collate_fixed_frame = 7; bool collate_landmarks = 8; + + MotionFilterOptions pose_graph_odometry_motion_filter = 9; } message SensorId { diff --git a/cartographer/mapping/proto/2d/tsdf_2d.proto b/cartographer/mapping/proto/tsdf_2d.proto similarity index 100% rename from cartographer/mapping/proto/2d/tsdf_2d.proto rename to cartographer/mapping/proto/tsdf_2d.proto diff --git a/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto b/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto similarity index 95% rename from cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto rename to cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto index 1b61f15d0c..d3ef1d3260 100644 --- a/cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto +++ b/cartographer/mapping/proto/tsdf_range_data_inserter_options_2d.proto @@ -16,7 +16,7 @@ syntax = "proto3"; package cartographer.mapping.proto; -import "cartographer/mapping/proto/2d/normal_estimation_options_2d.proto"; +import "cartographer/mapping/proto/normal_estimation_options_2d.proto"; message TSDFRangeDataInserterOptions2D { // Distance to the surface within the signed distance function is evaluated. diff --git a/cartographer/mapping/range_data_inserter_interface.cc b/cartographer/mapping/range_data_inserter_interface.cc index cf01396753..571987c711 100644 --- a/cartographer/mapping/range_data_inserter_interface.cc +++ b/cartographer/mapping/range_data_inserter_interface.cc @@ -15,8 +15,9 @@ */ #include "cartographer/mapping/range_data_inserter_interface.h" + #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h" -#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h" +#include "cartographer/mapping/internal/2d/tsdf_range_data_inserter_2d.h" namespace cartographer { namespace mapping { @@ -45,4 +46,4 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions( return options; } } // namespace mapping -} // namespace cartographer \ No newline at end of file +} // namespace cartographer diff --git a/cartographer/mapping/range_data_inserter_interface.h b/cartographer/mapping/range_data_inserter_interface.h index 5170766d2a..3f3a20e464 100644 --- a/cartographer/mapping/range_data_inserter_interface.h +++ b/cartographer/mapping/range_data_inserter_interface.h @@ -21,7 +21,7 @@ #include #include "cartographer/mapping/grid_interface.h" -#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h" +#include "cartographer/mapping/proto/submaps_options_2d.pb.h" #include "cartographer/sensor/range_data.h" namespace cartographer { diff --git a/cartographer/mapping/trajectory_builder_interface.cc b/cartographer/mapping/trajectory_builder_interface.cc index 8ed8377f11..b81eed5bce 100644 --- a/cartographer/mapping/trajectory_builder_interface.cc +++ b/cartographer/mapping/trajectory_builder_interface.cc @@ -18,7 +18,7 @@ #include "cartographer/mapping/internal/2d/local_trajectory_builder_options_2d.h" #include "cartographer/mapping/internal/3d/local_trajectory_builder_options_3d.h" -#include "cartographer/mapping/local_slam_result_data.h" +#include "cartographer/mapping/internal/local_slam_result_data.h" namespace cartographer { namespace mapping { @@ -37,6 +37,23 @@ void PopulatePureLocalizationTrimmerOptions( options_dictionary->GetInt("max_submaps_to_keep")); } +void PopulatePoseGraphOdometryMotionFilterOptions( + proto::TrajectoryBuilderOptions* const trajectory_builder_options, + common::LuaParameterDictionary* const parameter_dictionary) { + constexpr char kDictionaryKey[] = "pose_graph_odometry_motion_filter"; + if (!parameter_dictionary->HasKey(kDictionaryKey)) return; + + auto options_dictionary = parameter_dictionary->GetDictionary(kDictionaryKey); + auto* options = + trajectory_builder_options->mutable_pose_graph_odometry_motion_filter(); + options->set_max_time_seconds( + options_dictionary->GetDouble("max_time_seconds")); + options->set_max_distance_meters( + options_dictionary->GetDouble("max_distance_meters")); + options->set_max_angle_radians( + options_dictionary->GetDouble("max_angle_radians")); +} + } // namespace proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( @@ -53,6 +70,7 @@ proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( options.set_collate_landmarks( parameter_dictionary->GetBool("collate_landmarks")); PopulatePureLocalizationTrimmerOptions(&options, parameter_dictionary); + PopulatePoseGraphOdometryMotionFilterOptions(&options, parameter_dictionary); return options; } diff --git a/cartographer/mapping/trajectory_node_test.cc b/cartographer/mapping/trajectory_node_test.cc index 029681c6b9..c927a8d776 100644 --- a/cartographer/mapping/trajectory_node_test.cc +++ b/cartographer/mapping/trajectory_node_test.cc @@ -33,11 +33,14 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { common::FromUniversal(42), Eigen::Quaterniond(1., 2., -3., -4.), sensor::CompressedPointCloud( - {{Eigen::Vector3f{1.f, 2.f, 0.f}}, {Eigen::Vector3f{0.f, 0.f, 1.f}}}) + sensor::PointCloud({{Eigen::Vector3f{1.f, 2.f, 0.f}}, + {Eigen::Vector3f{0.f, 0.f, 1.f}}})) .Decompress(), - sensor::CompressedPointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}}) + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{2.f, 3.f, 4.f}}})) .Decompress(), - sensor::CompressedPointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}}) + sensor::CompressedPointCloud( + sensor::PointCloud({{Eigen::Vector3f{-1.f, 2.f, 0.f}}})) .Decompress(), Eigen::VectorXf::Unit(20, 4), transform::Rigid3d({1., 2., 3.}, @@ -46,12 +49,12 @@ TEST(TrajectoryNodeTest, ToAndFromProto) { const TrajectoryNode::Data actual = FromProto(proto); EXPECT_EQ(expected.time, actual.time); EXPECT_TRUE(actual.gravity_alignment.isApprox(expected.gravity_alignment)); - EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud, - actual.filtered_gravity_aligned_point_cloud); - EXPECT_EQ(expected.high_resolution_point_cloud, - actual.high_resolution_point_cloud); - EXPECT_EQ(expected.low_resolution_point_cloud, - actual.low_resolution_point_cloud); + EXPECT_EQ(expected.filtered_gravity_aligned_point_cloud.points(), + actual.filtered_gravity_aligned_point_cloud.points()); + EXPECT_EQ(expected.high_resolution_point_cloud.points(), + actual.high_resolution_point_cloud.points()); + EXPECT_EQ(expected.low_resolution_point_cloud.points(), + actual.low_resolution_point_cloud.points()); EXPECT_EQ(expected.rotational_scan_matcher_histogram, actual.rotational_scan_matcher_histogram); EXPECT_THAT(actual.local_pose, diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc deleted file mode 100644 index 87fbf6a27c..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.cc +++ /dev/null @@ -1,80 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -AccelerationConstraint3D::AccelerationConstraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::Acceleration3D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - third_(proto.third()), - imu_(proto.imu_calibration()), - cost_(new AccelerationCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void AccelerationConstraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(third_node, third_, nodes->pose_3d_nodes, - "Third node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN( - imu_node, imu_, nodes->imu_calibration_nodes, - "IMU calibration node was not found in imu_calibration_nodes."); - - if (first_node->constant() && second_node->constant() && - third_node->constant() && imu_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node, problem); - AddPose3D(second_node, problem); - AddPose3D(third_node, problem); - AddImuCalibration(imu_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - second_node->mutable_rotation()->data(), - second_node->mutable_translation()->data(), - first_node->mutable_translation()->data(), - third_node->mutable_translation()->data(), - imu_node->mutable_gravity_constant(), - imu_node->mutable_orientation()->data()); -} - -proto::CostFunction AccelerationConstraint3D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* acceleration_3d = cost_function.mutable_acceleration_3d(); - *acceleration_3d->mutable_first() = first_.ToProto(); - *acceleration_3d->mutable_second() = second_.ToProto(); - *acceleration_3d->mutable_third() = third_.ToProto(); - *acceleration_3d->mutable_imu_calibration() = imu_.ToProto(); - *acceleration_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h b/cartographer/pose_graph/constraint/acceleration_constraint_3d.h deleted file mode 100644 index 0321a83497..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class AccelerationConstraint3D : public Constraint { - public: - AccelerationConstraint3D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::Acceleration3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - // clang-format off - using AutoDiffFunction = - ceres::AutoDiffCostFunction; - // clang-format on - NodeId first_; - NodeId second_; - NodeId third_; - NodeId imu_; - // The cost function is owned by the ceres cost function. - AccelerationCost3D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ACCELERATION_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc b/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc deleted file mode 100644 index 7378df45c1..0000000000 --- a/cartographer/pose_graph/constraint/acceleration_constraint_3d_test.cc +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::EqualsProto; -using testing::ParseProto; - -constexpr char kConstraint[] = R"PROTO( - id: "hal_acceleration" - cost_function { - acceleration_3d { - first { object_id: "hal9000" timestamp: 100 } - second { object_id: "hal9000" timestamp: 200 } - third { object_id: "hal9000" timestamp: 300 } - imu_calibration { object_id: "hal_imu" } - parameters { - delta_velocity_imu_frame { x: 1 y: 1 z: 1 } - first_to_second_delta_time_seconds: 10.0 - second_to_third_delta_time_seconds: 20.0 - scaling_factor: 2.0 - } - } - } - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(AccelerationConstraint3DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - AccelerationConstraint3D constraint(proto.id(), proto.loss_function(), - proto.cost_function().acceleration_3d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint.cc b/cartographer/pose_graph/constraint/constraint.cc deleted file mode 100644 index b6dce5a5a4..0000000000 --- a/cartographer/pose_graph/constraint/constraint.cc +++ /dev/null @@ -1,32 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/constraint.h" - -namespace cartographer { -namespace pose_graph { - -// TODO(pifon): Add a test. -proto::Constraint Constraint::ToProto() const { - proto::Constraint constraint; - constraint.set_id(constraint_id_); - *constraint.mutable_cost_function() = ToCostFunctionProto(); - *constraint.mutable_loss_function() = loss_function_.ToProto(); - return constraint; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint.h b/cartographer/pose_graph/constraint/constraint.h deleted file mode 100644 index 0fcda85dcd..0000000000 --- a/cartographer/pose_graph/constraint/constraint.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ - -#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" -#include "cartographer/pose_graph/node/nodes.h" -#include "cartographer/pose_graph/proto/constraint.pb.h" -#include "ceres/problem.h" - -#include - -namespace cartographer { -namespace pose_graph { - -using ConstraintId = std::string; - -class Constraint { - public: - Constraint(const ConstraintId& id, - const proto::LossFunction& loss_function_proto) - : constraint_id_(id), loss_function_(loss_function_proto) {} - virtual ~Constraint() = default; - - Constraint(const Constraint&) = delete; - Constraint& operator=(const Constraint&) = delete; - - proto::Constraint ToProto() const; - - const ConstraintId& constraint_id() const { return constraint_id_; } - - virtual void AddToSolver(Nodes* nodes, ceres::Problem* problem) const = 0; - - protected: - virtual proto::CostFunction ToCostFunctionProto() const = 0; - - ceres::LossFunction* ceres_loss() const { - return loss_function_.ceres_loss(); - } - - private: - ConstraintId constraint_id_; - LossFunction loss_function_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_H_ diff --git a/cartographer/pose_graph/constraint/constraint_utils.cc b/cartographer/pose_graph/constraint/constraint_utils.cc deleted file mode 100644 index 89e5dffa1d..0000000000 --- a/cartographer/pose_graph/constraint/constraint_utils.cc +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -void AddPose2D(Pose2D* pose, ceres::Problem* problem) { - auto pose_2d = pose->mutable_pose_2d(); - problem->AddParameterBlock(pose_2d->data(), pose_2d->size()); - if (pose->constant()) { - problem->SetParameterBlockConstant(pose_2d->data()); - } -} - -void AddPose3D(Pose3D* pose, ceres::Problem* problem) { - auto transation = pose->mutable_translation(); - auto rotation = pose->mutable_rotation(); - - problem->AddParameterBlock(transation->data(), transation->size(), - pose->translation_parameterization()); - problem->AddParameterBlock(rotation->data(), rotation->size(), - pose->rotation_parameterization()); - if (pose->constant()) { - problem->SetParameterBlockConstant(transation->data()); - problem->SetParameterBlockConstant(rotation->data()); - } -} - -void AddImuCalibration(ImuCalibration* imu_calibration, - ceres::Problem* problem) { - auto gravity = imu_calibration->mutable_gravity_constant(); - auto orientation = imu_calibration->mutable_orientation(); - - problem->AddParameterBlock(gravity, 1); - problem->AddParameterBlock(orientation->data(), orientation->size(), - imu_calibration->orientation_parameterization()); - if (imu_calibration->constant()) { - problem->SetParameterBlockConstant(gravity); - problem->SetParameterBlockConstant(orientation->data()); - } -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/constraint_utils.h b/cartographer/pose_graph/constraint/constraint_utils.h deleted file mode 100644 index 67cbaed79e..0000000000 --- a/cartographer/pose_graph/constraint/constraint_utils.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ - -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/node/imu_calibration.h" -#include "cartographer/pose_graph/node/pose_2d.h" -#include "cartographer/pose_graph/node/pose_3d.h" -#include "ceres/problem.h" - -namespace cartographer { -namespace pose_graph { - -void AddPose2D(Pose2D* pose, ceres::Problem* problem); - -void AddPose3D(Pose3D* pose, ceres::Problem* problem); - -void AddImuCalibration(ImuCalibration* pose, ceres::Problem* problem); - -template -ValueTyper* FindNodeOrNull(MapType& map, const NodeId& node_id) { - auto node = common::FindOrNull(map, node_id); - return node == nullptr ? nullptr : node->get(); -} - -#define FIND_NODE_OR_RETURN(node_name, node_id, map, log_message) \ - auto node_name = FindNodeOrNull(map, node_id); \ - if (node_name == nullptr) { \ - LOG(INFO) << log_message; \ - return; \ - } - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_CONSTRAINT_UTILS_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc deleted file mode 100644 index 4ac374a595..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h" - -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -AccelerationCost3D::AccelerationCost3D( - const proto::Acceleration3D::Parameters& parameters) - : delta_velocity_imu_frame_( - transform::ToEigen(parameters.delta_velocity_imu_frame())), - first_to_second_delta_time_seconds_( - parameters.first_to_second_delta_time_seconds()), - second_to_third_delta_time_seconds_( - parameters.second_to_third_delta_time_seconds()), - scaling_factor_(parameters.scaling_factor()) {} - -proto::Acceleration3D::Parameters AccelerationCost3D::ToProto() const { - proto::Acceleration3D::Parameters parameters; - *parameters.mutable_delta_velocity_imu_frame() = - transform::ToProto(delta_velocity_imu_frame_); - parameters.set_first_to_second_delta_time_seconds( - first_to_second_delta_time_seconds_); - parameters.set_second_to_third_delta_time_seconds( - second_to_third_delta_time_seconds_); - parameters.set_scaling_factor(scaling_factor_); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h deleted file mode 100644 index e93dc98f4d..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_ACCELERATION_COST_3D_H_ -#define CARTOGRAPHER_ACCELERATION_COST_3D_H_ - -#include "Eigen/Core" -#include "Eigen/Geometry" -#include "cartographer/pose_graph/proto/cost_function.pb.h" - -namespace cartographer { -namespace pose_graph { - -// Penalizes differences between IMU data and optimized accelerations. -class AccelerationCost3D { - public: - explicit AccelerationCost3D( - const proto::Acceleration3D::Parameters& parameters); - - proto::Acceleration3D::Parameters ToProto() const; - - template - bool operator()(const T* const middle_rotation, const T* const start_position, - const T* const middle_position, const T* const end_position, - const T* const gravity_constant, - const T* const imu_calibration, T* residual) const { - using Vector3T = Eigen::Matrix; - using TranslationMap = Eigen::Map; - using RotationMap = Eigen::Map>; - - const Vector3T imu_delta_velocity = - RotationMap(middle_rotation) * RotationMap(imu_calibration) * - delta_velocity_imu_frame_.cast() - - (*gravity_constant * 0.5 * - (first_to_second_delta_time_seconds_ + - second_to_third_delta_time_seconds_)) * - Eigen::Vector3d::UnitZ().cast(); - - const Vector3T start_velocity = - (TranslationMap(middle_position) - TranslationMap(start_position)) / - T(first_to_second_delta_time_seconds_); - const Vector3T end_velocity = - (TranslationMap(end_position) - TranslationMap(middle_position)) / - T(second_to_third_delta_time_seconds_); - const Vector3T delta_velocity = end_velocity - start_velocity; - - (Eigen::Map(residual) = - T(scaling_factor_) * (imu_delta_velocity - delta_velocity)); - return true; - } - - private: - const Eigen::Vector3d delta_velocity_imu_frame_; - const double first_to_second_delta_time_seconds_; - const double second_to_third_delta_time_seconds_; - const double scaling_factor_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_ACCELERATION_COST_3D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc deleted file mode 100644 index fcb464577f..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d_test.cc +++ /dev/null @@ -1,73 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/acceleration_cost_3d.h" - -#include - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::google::protobuf::TextFormat; -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using PositionType = std::array; -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kParameters[] = R"PROTO( - delta_velocity_imu_frame { x: 1 y: 1 z: 1 } - first_to_second_delta_time_seconds: 10.0 - second_to_third_delta_time_seconds: 20.0 - scaling_factor: 2.0 -)PROTO"; - -TEST(AccelerationCost3DTest, SerializesCorrectly) { - AccelerationCost3D acceleration_cost_3d( - ParseProto(kParameters)); - EXPECT_THAT(acceleration_cost_3d.ToProto(), EqualsProto(kParameters)); -} - -TEST(AccelerationCost3DTest, EvaluatesCorrectly) { - AccelerationCost3D acceleration_cost_3d( - ParseProto(kParameters)); - const PositionType kStartPosition{{1., 1., 1.}}; - - const PositionType kMiddlePosition{{2., 2., 2.}}; - const RotationType kMiddleRotation{{0., 0., 0., 1.}}; - - const PositionType kEndPosition{{3., 3., 3.}}; - - const double kGravityConstant{10.}; - const RotationType kImuCalibration{{0., 0., 0., 1.0}}; - - ResidualType residuals; - EXPECT_TRUE(acceleration_cost_3d( - kMiddleRotation.data(), kStartPosition.data(), kMiddlePosition.data(), - kEndPosition.data(), &kGravityConstant, kImuCalibration.data(), - residuals.data())); - EXPECT_THAT(residuals, ElementsAre(Near(2.1), Near(2.1), Near(-297.9))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc deleted file mode 100644 index b09c74e478..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.cc +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h" - -namespace cartographer { -namespace pose_graph { - -InterpolatedRelativePoseCost2D::InterpolatedRelativePoseCost2D( - const proto::InterpolatedRelativePose2D::Parameters& parameters) - : translation_weight_(parameters.translation_weight()), - rotation_weight_(parameters.rotation_weight()), - interpolation_factor_(parameters.interpolation_factor()), - first_T_second_(transform::ToRigid3(parameters.first_t_second())), - gravity_alignment_first_start_( - transform::ToEigen(parameters.gravity_alignment_first_start())), - gravity_alignment_first_end_( - transform::ToEigen(parameters.gravity_alignment_first_end())) {} - -proto::InterpolatedRelativePose2D::Parameters -InterpolatedRelativePoseCost2D::ToProto() const { - proto::InterpolatedRelativePose2D::Parameters parameters; - parameters.set_translation_weight(translation_weight_); - parameters.set_rotation_weight(rotation_weight_); - parameters.set_interpolation_factor(interpolation_factor_); - *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_); - *parameters.mutable_gravity_alignment_first_start() = - transform::ToProto(gravity_alignment_first_start_); - *parameters.mutable_gravity_alignment_first_end() = - transform::ToProto(gravity_alignment_first_end_); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h deleted file mode 100644 index e86dfb5ca0..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H - -#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" -#include "cartographer/pose_graph/proto/cost_function.pb.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -// Provides cost function for relative pose and de/serialization methods. -class InterpolatedRelativePoseCost2D { - public: - explicit InterpolatedRelativePoseCost2D( - const proto::InterpolatedRelativePose2D::Parameters& parameters); - - proto::InterpolatedRelativePose2D::Parameters ToProto() const; - - template - bool operator()(const T* const first_start_pose, - const T* const first_end_pose, - const T* const second_translation, - const T* const second_rotation, T* const e) const { - const std::tuple, std::array> - interpolated_first_pose = mapping::optimization::InterpolateNodes2D( - first_start_pose, gravity_alignment_first_start_, first_end_pose, - gravity_alignment_first_end_, interpolation_factor_); - const std::array error = mapping::optimization::ScaleError( - mapping::optimization::ComputeUnscaledError( - first_T_second_, std::get<0>(interpolated_first_pose).data(), - std::get<1>(interpolated_first_pose).data(), second_rotation, - second_translation), - translation_weight_, rotation_weight_); - std::copy(std::begin(error), std::end(error), e); - return true; - } - - private: - const double translation_weight_; - const double rotation_weight_; - const double interpolation_factor_; - const transform::Rigid3d first_T_second_; - const Eigen::Quaterniond gravity_alignment_first_start_; - const Eigen::Quaterniond gravity_alignment_first_end_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_2D_H diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc deleted file mode 100644 index 7464e10774..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using Position2DType = std::array; -using TranslationType = std::array; -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 1 - rotation_weight: 2 - gravity_alignment_first_start { x: 0.4 y: 0.1 z: 0.3 w: 0.2 } - gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 } - interpolation_factor: 0.3 -)PROTO"; - -TEST(InterpolatedRelativePoseCost2DTest, SerializesCorrectly) { - const auto parameters = - ParseProto(kParameters); - InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters); - const auto actual_proto = interpolated_relative_pose_cost_2d.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kParameters)); -} - -TEST(InterpolatedRelativePoseCost2DTest, EvaluatesCorrectly) { - const auto parameters = - ParseProto(kParameters); - InterpolatedRelativePoseCost2D interpolated_relative_pose_cost_2d(parameters); - - const Position2DType kFirstStartPose{{1., 1., 1.}}; - const Position2DType kFirstEndPose{{2., 3., 4.}}; - const TranslationType kSecondTranslation{{0., -1., -2.}}; - const RotationType kSecondRotation{{.4, .2, .3, .1}}; - - ResidualType residuals; - EXPECT_TRUE(interpolated_relative_pose_cost_2d( - kFirstStartPose.data(), kFirstEndPose.data(), kSecondTranslation.data(), - kSecondRotation.data(), residuals.data())); - EXPECT_THAT(residuals, - ElementsAre(Near(4.49044), Near(1.8527), Near(3.49511), - Near(-1.93746), Near(3.54854), Near(4.50243))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc deleted file mode 100644 index 5e3003527c..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.cc +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -InterpolatedRelativePoseCost3D::InterpolatedRelativePoseCost3D( - const proto::InterpolatedRelativePose3D::Parameters& parameters) - : translation_weight_(parameters.translation_weight()), - rotation_weight_(parameters.rotation_weight()), - interpolation_factor_(parameters.interpolation_factor()), - first_T_second_(transform::ToRigid3(parameters.first_t_second())) {} - -proto::InterpolatedRelativePose3D::Parameters -InterpolatedRelativePoseCost3D::ToProto() const { - proto::InterpolatedRelativePose3D::Parameters parameters; - parameters.set_translation_weight(translation_weight_); - parameters.set_rotation_weight(rotation_weight_); - parameters.set_interpolation_factor(interpolation_factor_); - *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer \ No newline at end of file diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h deleted file mode 100644 index dab6c9c0aa..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h +++ /dev/null @@ -1,71 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_ - -#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" -#include "cartographer/pose_graph/proto/cost_function.pb.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -// Provides cost function for interpolated relative pose in 3d and -// de/serialization methods; only the first pose is linearly interpolated -// between two nodes. -class InterpolatedRelativePoseCost3D { - public: - InterpolatedRelativePoseCost3D( - const proto::InterpolatedRelativePose3D::Parameters& parameters); - - proto::InterpolatedRelativePose3D::Parameters ToProto() const; - - template - bool operator()(const T* const first_start_translation, - const T* const first_start_rotation, - const T* const first_end_translation, - const T* const first_end_rotation, - const T* const second_translation, - const T* const second_rotation, T* const error_out) const { - const std::tuple, std::array> - interpolated_rotation_and_translation = - mapping::optimization::InterpolateNodes3D( - first_start_rotation, first_start_translation, - first_end_rotation, first_end_translation, - interpolation_factor_); - const std::array error = mapping::optimization::ScaleError( - mapping::optimization::ComputeUnscaledError( - first_T_second_, - std::get<0>(interpolated_rotation_and_translation).data(), - std::get<1>(interpolated_rotation_and_translation).data(), - second_rotation, second_translation), - translation_weight_, rotation_weight_); - std::copy(std::begin(error), std::end(error), error_out); - return true; - } - - private: - const double translation_weight_; - const double rotation_weight_; - const double interpolation_factor_; - const transform::Rigid3d first_T_second_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_INTERPOLATED_RELATIVE_POSE_COST_3D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc deleted file mode 100644 index f5e88a08b7..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using PositionType = std::array; -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 1 - rotation_weight: 10 - interpolation_factor: 0.3 -)PROTO"; - -TEST(InterpolatedRelativePoseCost3DTest, SerializesCorrectly) { - const auto parameters = - ParseProto(kParameters); - InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters); - const auto actual_proto = interpolated_relative_pose_cost_3d.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kParameters)); -} - -TEST(InterpolatedRelativePoseCost3DTest, EvaluatesCorrectly) { - const auto parameters = - ParseProto(kParameters); - InterpolatedRelativePoseCost3D interpolated_relative_pose_cost_3d(parameters); - - const PositionType kFirstStartPosition{{1., 1., 1.}}; - const RotationType kFirstStartRotation{{1., 1., 1., 1.}}; - const PositionType kFirstEndPosition{{2., 3., 4.}}; - const RotationType kFirstEndRotation{{1.1, 1.2, 1.3, 1.4}}; - const PositionType kSecondPosition{{0., -1., -2.}}; - const RotationType kSecondRotation{{.1, .2, .3, .4}}; - - ResidualType residuals; - EXPECT_TRUE(interpolated_relative_pose_cost_3d( - kFirstStartPosition.data(), kFirstStartRotation.data(), - kFirstEndPosition.data(), kFirstEndRotation.data(), - kSecondPosition.data(), kSecondRotation.data(), residuals.data())); - EXPECT_THAT(residuals, - ElementsAre(Near(8.4594), Near(10.27735), Near(-4.45472), - Near(0.968852), Near(11.96531), Near(3.34254))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc deleted file mode 100644 index fae18cc425..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc +++ /dev/null @@ -1,94 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseCost2D::RelativePoseCost2D( - const proto::RelativePose2D::Parameters& parameters) - : translation_weight_(parameters.translation_weight()), - rotation_weight_(parameters.rotation_weight()), - first_T_second_(transform::ToRigid2(parameters.first_t_second())) {} - -proto::RelativePose2D::Parameters RelativePoseCost2D::ToProto() const { - proto::RelativePose2D::Parameters parameters; - parameters.set_translation_weight(translation_weight_); - parameters.set_rotation_weight(rotation_weight_); - *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_); - return parameters; -} - -bool RelativePoseCost2D::Evaluate(double const* const* parameters, - double* residuals, double** jacobians) const { - double const* start = parameters[0]; - double const* end = parameters[1]; - - const double cos_start_rotation = cos(start[2]); - const double sin_start_rotation = sin(start[2]); - const double delta_x = end[0] - start[0]; - const double delta_y = end[1] - start[1]; - - residuals[0] = - translation_weight_ * - (first_T_second_.translation().x() - - (cos_start_rotation * delta_x + sin_start_rotation * delta_y)); - residuals[1] = - translation_weight_ * - (first_T_second_.translation().y() - - (-sin_start_rotation * delta_x + cos_start_rotation * delta_y)); - residuals[2] = rotation_weight_ * - common::NormalizeAngleDifference( - first_T_second_.rotation().angle() - (end[2] - start[2])); - if (jacobians == nullptr) return true; - - const double weighted_cos_start_rotation = - translation_weight_ * cos_start_rotation; - const double weighted_sin_start_rotation = - translation_weight_ * sin_start_rotation; - - // Jacobians in Ceres are ordered by the parameter blocks: - // jacobian[i] = [(dr_0 / dx_i)^T, ..., (dr_n / dx_i)^T]. - if (jacobians[0] != nullptr) { - jacobians[0][0] = weighted_cos_start_rotation; - jacobians[0][1] = weighted_sin_start_rotation; - jacobians[0][2] = weighted_sin_start_rotation * delta_x - - weighted_cos_start_rotation * delta_y; - jacobians[0][3] = -weighted_sin_start_rotation; - jacobians[0][4] = weighted_cos_start_rotation; - jacobians[0][5] = weighted_cos_start_rotation * delta_x + - weighted_sin_start_rotation * delta_y; - jacobians[0][6] = 0; - jacobians[0][7] = 0; - jacobians[0][8] = rotation_weight_; - } - if (jacobians[1] != nullptr) { - jacobians[1][0] = -weighted_cos_start_rotation; - jacobians[1][1] = -weighted_sin_start_rotation; - jacobians[1][2] = 0; - jacobians[1][3] = weighted_sin_start_rotation; - jacobians[1][4] = -weighted_cos_start_rotation; - jacobians[1][5] = 0; - jacobians[1][6] = 0; - jacobians[1][7] = 0; - jacobians[1][8] = -rotation_weight_; - } - return true; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h deleted file mode 100644 index 5395e89862..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_ - -#include "cartographer/pose_graph/proto/cost_function.pb.h" -#include "cartographer/transform/transform.h" -#include "ceres/sized_cost_function.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseCost2D - : public ceres::SizedCostFunction<3 /* number of residuals */, - 3 /* size of first pose */, - 3 /* size of second pose */> { - public: - explicit RelativePoseCost2D( - const proto::RelativePose2D::Parameters& parameters); - - proto::RelativePose2D::Parameters ToProto() const; - - // Parameters are packed as [first_pose_2d, second_pose_2d], where each 2D - // pose is [translation_x, translation_y, rotation]. - bool Evaluate(double const* const* parameters, double* residuals, - double** jacobians) const final; - - private: - const double translation_weight_; - const double rotation_weight_; - const transform::Rigid2d first_T_second_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_ diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc deleted file mode 100644 index cf1a23fd4c..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" - -#include "absl/memory/memory.h" -#include "cartographer/testing/test_helpers.h" -#include "ceres/gradient_checker.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -constexpr int kPoseDimension = 3; -constexpr int kResidualsCount = 3; -constexpr int kParameterBlocksCount = 2; -constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension; - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using ResidualType = std::array; -using JacobianType = std::array, - kParameterBlocksCount>; - -constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 1 } - rotation: -2.214297 - } - translation_weight: 1 - rotation_weight: 10 -)PROTO"; - -class RelativePoseCost2DTest : public ::testing::Test { - public: - RelativePoseCost2DTest() - : relative_pose_cost_2d_(absl::make_unique( - ParseProto(kParameters))) { - for (int i = 0; i < kParameterBlocksCount; ++i) { - jacobian_ptrs_[i] = jacobian_[i].data(); - } - } - - std::pair - EvaluateRelativePoseCost2D( - const std::array& parameter_blocks) { - relative_pose_cost_2d_->Evaluate(parameter_blocks.data(), residuals_.data(), - jacobian_ptrs_.data()); - return std::make_pair(std::cref(residuals_), std::cref(jacobian_)); - } - - protected: - ResidualType residuals_; - JacobianType jacobian_; - std::array jacobian_ptrs_; - std::unique_ptr relative_pose_cost_2d_; -}; - -TEST_F(RelativePoseCost2DTest, SerializesCorrectly) { - EXPECT_THAT(relative_pose_cost_2d_->ToProto(), EqualsProto(kParameters)); -} - -TEST_F(RelativePoseCost2DTest, CheckGradient) { - std::array start_pose{{1., 1., 1.}}; - std::array end_pose{{10., 1., 100.}}; - std::array parameter_blocks{ - {start_pose.data(), end_pose.data()}}; - - using ::ceres::GradientChecker; - GradientChecker gradient_checker(relative_pose_cost_2d_.get(), - {} /* local parameterizations */, - ceres::NumericDiffOptions{}); - - GradientChecker::ProbeResults probe_results; - gradient_checker.Probe(parameter_blocks.data(), - 1e-08 /* relative precision */, &probe_results); - EXPECT_TRUE(probe_results.return_value); -} - -TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) { - std::array start_pose{{1., 1., 1.}}; - std::array end_pose{{10., 1., 100.}}; - std::array parameter_blocks{ - {start_pose.data(), end_pose.data()}}; - - auto residuals_and_jacobian = EvaluateRelativePoseCost2D(parameter_blocks); - EXPECT_THAT(residuals_and_jacobian.first, - ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333))); - EXPECT_THAT( - residuals_and_jacobian.second, - ElementsAre( - ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324), - Near(-0.841471), Near(0.540302), Near(4.86272), Near(0), - Near(0), Near(10)), - ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471), - Near(-0.540302), Near(0), Near(0), Near(0), Near(-10)))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc deleted file mode 100644 index 9293fd9fb9..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.cc +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseCost3D::RelativePoseCost3D( - const proto::RelativePose3D::Parameters& parameters) - : translation_weight_(parameters.translation_weight()), - rotation_weight_(parameters.rotation_weight()), - first_T_second_(transform::ToRigid3(parameters.first_t_second())) {} - -proto::RelativePose3D::Parameters RelativePoseCost3D::ToProto() const { - proto::RelativePose3D::Parameters parameters; - parameters.set_translation_weight(translation_weight_); - parameters.set_rotation_weight(rotation_weight_); - *parameters.mutable_first_t_second() = transform::ToProto(first_T_second_); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer \ No newline at end of file diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h deleted file mode 100644 index 9c68a7e245..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_RELATIVE_POSE_COST_3D_H -#define CARTOGRAPHER_RELATIVE_POSE_COST_3D_H - -#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" -#include "cartographer/pose_graph/proto/cost_function.pb.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -// Provides cost function for relative pose and de/serialization methods. -class RelativePoseCost3D { - public: - explicit RelativePoseCost3D( - const proto::RelativePose3D::Parameters& parameters); - - proto::RelativePose3D::Parameters ToProto() const; - - template - bool operator()(const T* const c_i_translation, const T* const c_i_rotation, - const T* const c_j_translation, const T* const c_j_rotation, - T* const error_out) const { - const std::array error = mapping::optimization::ScaleError( - mapping::optimization::ComputeUnscaledError( - first_T_second_, c_i_rotation, c_i_translation, c_j_rotation, - c_j_translation), - translation_weight_, rotation_weight_); - std::copy(std::begin(error), std::end(error), error_out); - return true; - } - - private: - const double translation_weight_; - const double rotation_weight_; - const transform::Rigid3d first_T_second_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_RELATIVE_POSE_COST_3D_H diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc deleted file mode 100644 index d44ad61fba..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d_test.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using PositionType = std::array; -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 1 - rotation_weight: 10 -)PROTO"; - -TEST(RelativePoseCost3DTest, SerializesCorrectly) { - auto relative_pose_parameters = - ParseProto(kParameters); - RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters); - const auto actual_proto = relative_pose_cost_3d.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kParameters)); -} - -TEST(RelativePoseCost3DTest, EvaluatesCorrectly) { - auto relative_pose_parameters = - ParseProto(kParameters); - RelativePoseCost3D relative_pose_cost_3d(relative_pose_parameters); - - const PositionType kPosition1{{1., 1., 1.}}; - const RotationType kRotation1{{1., 1., 1., 1.}}; - ResidualType residuals; - EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(), - kPosition1.data(), kRotation1.data(), - residuals.data())); - EXPECT_THAT(residuals, ElementsAre(Near(1), Near(2), Near(3), Near(0), - Near(19.1037), Near(6.3679))); - - const PositionType kPosition2{{0., -1., -2.}}; - const RotationType kRotation2{{.1, .2, .3, .4}}; - EXPECT_TRUE(relative_pose_cost_3d(kPosition1.data(), kRotation1.data(), - kPosition2.data(), kRotation2.data(), - residuals.data())); - EXPECT_THAT(residuals, ElementsAre(Near(6), Near(8), Near(-2), Near(1.03544), - Near(11.38984), Near(3.10632))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc deleted file mode 100644 index 24aa9d8363..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.cc +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h" - -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -RotationCost3D::RotationCost3D(const proto::Rotation3D::Parameters& parameters) - : scaling_factor_(parameters.scaling_factor()), - delta_rotation_imu_frame_( - transform::ToEigen(parameters.delta_rotation_imu_frame())) {} - -proto::Rotation3D::Parameters RotationCost3D::ToProto() const { - proto::Rotation3D::Parameters parameters; - parameters.set_scaling_factor(scaling_factor_); - *parameters.mutable_delta_rotation_imu_frame() = - transform::ToProto(delta_rotation_imu_frame_); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer \ No newline at end of file diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h deleted file mode 100644 index 45842bcd81..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_ROTATION_COST_3D_H -#define CARTOGRAPHER_ROTATION_COST_3D_H - -#include "Eigen/Core" -#include "Eigen/Geometry" -#include "cartographer/pose_graph/proto/cost_function.pb.h" - -namespace cartographer { -namespace pose_graph { - -// Provides cost function for 3D rotation and de/serialization methods. The cost -// function penalizes differences between IMU data and optimized orientations. -class RotationCost3D { - public: - explicit RotationCost3D(const proto::Rotation3D::Parameters& parameters); - - proto::Rotation3D::Parameters ToProto() const; - - // Cost function expecting three quaternions as input and 3D vector as output. - template - bool operator()(const T* const start_rotation, const T* const end_rotation, - const T* const imu_calibration, T* residual) const { - const Eigen::Quaternion start(start_rotation[0], start_rotation[1], - start_rotation[2], start_rotation[3]); - const Eigen::Quaternion end(end_rotation[0], end_rotation[1], - end_rotation[2], end_rotation[3]); - const Eigen::Quaternion eigen_imu_calibration( - imu_calibration[0], imu_calibration[1], imu_calibration[2], - imu_calibration[3]); - const Eigen::Quaternion error = - end.conjugate() * start * eigen_imu_calibration * - delta_rotation_imu_frame_.cast() * eigen_imu_calibration.conjugate(); - residual[0] = scaling_factor_ * error.x(); - residual[1] = scaling_factor_ * error.y(); - residual[2] = scaling_factor_ * error.z(); - return true; - } - - private: - const double scaling_factor_; - const Eigen::Quaterniond delta_rotation_imu_frame_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_ROTATION_COST_3D_H diff --git a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc b/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc deleted file mode 100644 index ca6d1ea3bf..0000000000 --- a/cartographer/pose_graph/constraint/cost_function/rotation_cost_3d_test.cc +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h" - -#include - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kParameters[] = R"PROTO( - delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 } - scaling_factor: 0.4 -)PROTO"; - -TEST(RotationCost3DTest, SerializesCorrectly) { - const auto parameters = - ParseProto(kParameters); - RotationCost3D rotation_cost_3d(parameters); - const auto actual_proto = rotation_cost_3d.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kParameters)); -} - -TEST(RotationCost3DTest, EvaluatesCorrectly) { - const auto parameters = - ParseProto(kParameters); - RotationCost3D rotation_cost_3d(parameters); - - const RotationType kStartRotation{{1., 1., 1., 1.}}; - const RotationType kEndRotation{{1.1, 1.2, 1.3, 1.4}}; - const RotationType kImuCalibration{{0.1, 0.1, 0.1, 0.1}}; - ResidualType residuals; - rotation_cost_3d(kStartRotation.data(), kEndRotation.data(), - kImuCalibration.data(), residuals.data()); - - EXPECT_THAT(residuals, - ElementsAre(Near(0.01536), Near(-0.00256), Near(0.00832))); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc deleted file mode 100644 index 7e8154b23f..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.cc +++ /dev/null @@ -1,75 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -InterpolatedRelativePoseConstraint2D::InterpolatedRelativePoseConstraint2D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::InterpolatedRelativePose2D& proto) - : Constraint(id, loss_function_proto), - first_start_(proto.first_start()), - first_end_(proto.first_end()), - second_(proto.second()), - cost_(new InterpolatedRelativePoseCost2D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void InterpolatedRelativePoseConstraint2D::AddToSolver( - Nodes* nodes, ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_2d_nodes, - "First node (start) was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_2d_nodes, - "First node (end) was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node_start->constant() && first_node_end->constant() && - second_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose2D(first_node_start, problem); - AddPose2D(first_node_end, problem); - AddPose3D(second_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node_start->mutable_pose_2d()->data(), - first_node_end->mutable_pose_2d()->data(), - second_node->mutable_translation()->data(), - second_node->mutable_rotation()->data()); -} - -proto::CostFunction InterpolatedRelativePoseConstraint2D::ToCostFunctionProto() - const { - proto::CostFunction cost_function; - auto* interpolated_relative_pose_2d = - cost_function.mutable_interpolated_relative_pose_2d(); - *interpolated_relative_pose_2d->mutable_first_start() = - first_start_.ToProto(); - *interpolated_relative_pose_2d->mutable_first_end() = first_end_.ToProto(); - *interpolated_relative_pose_2d->mutable_second() = second_.ToProto(); - *interpolated_relative_pose_2d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h deleted file mode 100644 index 5635334c9a..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_2d.h" - -namespace cartographer { -namespace pose_graph { - -class InterpolatedRelativePoseConstraint2D : public Constraint { - public: - InterpolatedRelativePoseConstraint2D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::InterpolatedRelativePose2D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - // clang-format off - using AutoDiffFunction = ceres::AutoDiffCostFunction< - InterpolatedRelativePoseCost2D, - 6 /* residuals */, - 3 /* 2d pose variables of first start pose */, - 3 /* 2d pose variables of first end pose */, - 3 /* translation of second pose */, - 4 /* rotation of second pose */>; - // clang-format on - NodeId first_start_; - NodeId first_end_; - NodeId second_; - - // The cost function is owned by the ceres cost function. - InterpolatedRelativePoseCost2D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_2D_H_ diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc deleted file mode 100644 index 8659876bc3..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d_test.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::EqualsProto; -using testing::ParseProto; - -constexpr char kConstraint[] = R"PROTO( - id: "narf" - cost_function { - interpolated_relative_pose_2d { - first_start { object_id: "node0_start" } - first_end { object_id: "node0_end" } - second { object_id: "node1" } - parameters { - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 0.5 - rotation_weight: 1.0 - gravity_alignment_first_start { x: 0.4 y: 0.2 z: 0.3 w: 0.2 } - gravity_alignment_first_end { x: 0.3 y: 0.4 z: 0.2 w: 0.1 } - interpolation_factor: 0.3 - } - } - } - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(InterpolatedRelativePostConstraint2DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - InterpolatedRelativePoseConstraint2D constraint( - proto.id(), proto.loss_function(), - proto.cost_function().interpolated_relative_pose_2d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc deleted file mode 100644 index 73f31fa02d..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.cc +++ /dev/null @@ -1,77 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -InterpolatedRelativePoseConstraint3D::InterpolatedRelativePoseConstraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::InterpolatedRelativePose3D& proto) - : Constraint(id, loss_function_proto), - first_start_(proto.first_start()), - first_end_(proto.first_end()), - second_(proto.second()), - cost_(new InterpolatedRelativePoseCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void InterpolatedRelativePoseConstraint3D::AddToSolver( - Nodes* nodes, ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node_start, first_start_, nodes->pose_3d_nodes, - "First node (start) was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(first_node_end, first_end_, nodes->pose_3d_nodes, - "First node (end) was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node_start->constant() && first_node_end->constant() && - second_node->constant()) { - LOG(INFO) << "All nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node_start, problem); - AddPose3D(first_node_end, problem); - AddPose3D(second_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node_start->mutable_translation()->data(), - first_node_start->mutable_rotation()->data(), - first_node_end->mutable_translation()->data(), - first_node_end->mutable_rotation()->data(), - second_node->mutable_translation()->data(), - second_node->mutable_rotation()->data()); -} - -proto::CostFunction InterpolatedRelativePoseConstraint3D::ToCostFunctionProto() - const { - proto::CostFunction cost_function; - auto* interpolated_relative_pose_3d = - cost_function.mutable_interpolated_relative_pose_3d(); - *interpolated_relative_pose_3d->mutable_first_start() = - first_start_.ToProto(); - *interpolated_relative_pose_3d->mutable_first_end() = first_end_.ToProto(); - *interpolated_relative_pose_3d->mutable_second() = second_.ToProto(); - *interpolated_relative_pose_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h deleted file mode 100644 index d836561e73..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/interpolated_relative_pose_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class InterpolatedRelativePoseConstraint3D : public Constraint { - public: - InterpolatedRelativePoseConstraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::InterpolatedRelativePose3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - // clang-format off - using AutoDiffFunction = ceres::AutoDiffCostFunction< - InterpolatedRelativePoseCost3D, - 6 /* number of residuals */, - 3 /* translation of first start pose */, - 4 /* rotation of first start pose */, - 3 /* translation of first end pose */, - 4 /* rotation of first end pose */, - 3 /* translation of second pose */, - 4 /* rotation of second pose */>; - // clang-format on - NodeId first_start_; - NodeId first_end_; - NodeId second_; - - // The cost function is owned by the ceres cost function. - InterpolatedRelativePoseCost3D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_INTERPOLATED_RELATIVE_POSE_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc b/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc deleted file mode 100644 index 642188af0c..0000000000 --- a/cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d_test.cc +++ /dev/null @@ -1,67 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using ::testing::ElementsAre; -using testing::EqualsProto; -using testing::Near; -using testing::ParseProto; - -using PositionType = std::array; -using RotationType = std::array; -using ResidualType = std::array; - -constexpr char kConstraint[] = R"PROTO( - id: "narf" - cost_function { - interpolated_relative_pose_3d { - first_start { object_id: "node0_start" } - first_end { object_id: "node0_end" } - second { object_id: "node1" } - parameters { - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 0.5 - rotation_weight: 1.0 - interpolation_factor: 0.3 - } - } - } - - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(InterpolatedRelativePostConstraint3DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - InterpolatedRelativePoseConstraint3D constraint( - proto.id(), proto.loss_function(), - proto.cost_function().interpolated_relative_pose_3d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function.cc b/cartographer/pose_graph/constraint/loss_function/loss_function.cc deleted file mode 100644 index 6620520b0d..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function.cc +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" - -#include "absl/memory/memory.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -std::unique_ptr CeresLossFromProto( - const proto::LossFunction& proto) { - switch (proto.Type_case()) { - case proto::LossFunction::kHuberLoss: - return absl::make_unique(proto.huber_loss().scale()); - case proto::LossFunction::kQuadraticLoss: - return nullptr; - default: - LOG(FATAL) << "The loss function is not specified."; - return nullptr; - } -} - -} // namespace - -LossFunction::LossFunction(const proto::LossFunction& proto) - : proto_(proto), ceres_loss_(CeresLossFromProto(proto_)) {} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function.h b/cartographer/pose_graph/constraint/loss_function/loss_function.h deleted file mode 100644 index 5cbd53338a..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ - -#include - -#include "cartographer/pose_graph/proto/loss_function.pb.h" -#include "ceres/loss_function.h" - -namespace cartographer { -namespace pose_graph { - -class LossFunction { - public: - explicit LossFunction(const proto::LossFunction& proto); - - const proto::LossFunction& ToProto() const { return proto_; } - - ceres::LossFunction* ceres_loss() const { return ceres_loss_.get(); } - - private: - const proto::LossFunction proto_; - const std::unique_ptr ceres_loss_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_LOSS_FUNCTION_H_ diff --git a/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc b/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc deleted file mode 100644 index 9f29be1eaa..0000000000 --- a/cartographer/pose_graph/constraint/loss_function/loss_function_test.cc +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/loss_function/loss_function.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -TEST(LossFunctionTest, ConstructQuadraticLoss) { - LossFunction quadratic_loss( - ParseProto(R"(quadratic_loss: {})")); - EXPECT_EQ(nullptr, quadratic_loss.ceres_loss()); -} - -TEST(LossFunctionTest, ConstructHuberLoss) { - LossFunction huber_loss( - ParseProto(R"(huber_loss: { scale: 0.5 })")); - EXPECT_NE(nullptr, dynamic_cast(huber_loss.ceres_loss())); -} - -TEST(LossFunctionDeathTest, FailToConstructUnspecifiedLoss) { - EXPECT_DEATH(LossFunction(proto::LossFunction{}), ""); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc deleted file mode 100644 index 46f8264365..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.cc +++ /dev/null @@ -1,72 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseConstraint2D::RelativePoseConstraint2D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::RelativePose2D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - ceres_cost_(absl::make_unique(proto.parameters())) {} - -// TODO(pifon): Add a test. -void RelativePoseConstraint2D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_2d_nodes, - "First node was not found in pose_2d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_2d_nodes, - "Second node was not found in pose_2d_nodes."); - - if (first_node->constant() && second_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - auto first_pose = first_node->mutable_pose_2d(); - problem->AddParameterBlock(first_pose->data(), first_pose->size()); - if (first_node->constant()) { - problem->SetParameterBlockConstant(first_pose->data()); - } - - auto second_pose = second_node->mutable_pose_2d(); - problem->AddParameterBlock(second_pose->data(), second_pose->size()); - if (second_node->constant()) { - problem->SetParameterBlockConstant(second_pose->data()); - } - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), first_pose->data(), - second_pose->data()); -} - -proto::CostFunction RelativePoseConstraint2D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* relative_pose_2d = cost_function.mutable_relative_pose_2d(); - *relative_pose_2d->mutable_first() = first_.ToProto(); - *relative_pose_2d->mutable_second() = second_.ToProto(); - *relative_pose_2d->mutable_parameters() = ceres_cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h deleted file mode 100644 index 23c0bff474..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseConstraint2D : public Constraint { - public: - RelativePoseConstraint2D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::RelativePose2D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - NodeId first_; - NodeId second_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_2D_H_ diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc deleted file mode 100644 index 85c396e5d2..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_2d_test.cc +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::EqualsProto; -using testing::ParseProto; - -constexpr char kConstraint[] = R"PROTO( - id: "narf" - cost_function { - relative_pose_2d { - first { object_id: "node0" } - second { object_id: "node1" } - parameters { - first_t_second { - translation: { x: 1 y: 1 } - rotation: -2.214297 - } - translation_weight: 1 - rotation_weight: 10 - } - } - } - - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RelativePoseConstraint2DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RelativePoseConstraint2D constraint(proto.id(), proto.loss_function(), - proto.cost_function().relative_pose_2d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc deleted file mode 100644 index e688118e69..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.cc +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/utils.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -namespace cartographer { -namespace pose_graph { - -RelativePoseConstraint3D::RelativePoseConstraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::RelativePose3D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - cost_(new RelativePoseCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void RelativePoseConstraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - - if (first_node->constant() && second_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - AddPose3D(first_node, problem); - AddPose3D(second_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node->mutable_translation()->data(), - first_node->mutable_rotation()->data(), - second_node->mutable_translation()->data(), - second_node->mutable_rotation()->data()); -} - -proto::CostFunction RelativePoseConstraint3D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* relative_pose_3d = cost_function.mutable_relative_pose_3d(); - *relative_pose_3d->mutable_first() = first_.ToProto(); - *relative_pose_3d->mutable_second() = second_.ToProto(); - *relative_pose_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h b/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h deleted file mode 100644 index aadc066eba..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class RelativePoseConstraint3D : public Constraint { - public: - RelativePoseConstraint3D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::RelativePose3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - using AutoDiffFunction = - ceres::AutoDiffCostFunction; - NodeId first_; - NodeId second_; - // The cost function is owned by the ceres cost function. - RelativePoseCost3D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_RELATIVE_POSE_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc b/cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc deleted file mode 100644 index 6af981be78..0000000000 --- a/cartographer/pose_graph/constraint/relative_pose_constraint_3d_test.cc +++ /dev/null @@ -1,58 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::EqualsProto; -using testing::ParseProto; - -constexpr char kConstraint[] = R"PROTO( - id: "narf" - cost_function { - relative_pose_3d { - first { object_id: "node0" } - second { object_id: "node1" } - parameters { - first_t_second { - translation: { x: 1 y: 2 z: 3 } - rotation: { x: 0 y: 0.3 z: 0.1 w: 0.2 } - } - translation_weight: 1 - rotation_weight: 10 - } - } - } - - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RelativePoseConstraint2DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RelativePoseConstraint3D constraint(proto.id(), proto.loss_function(), - proto.cost_function().relative_pose_3d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d.cc deleted file mode 100644 index b9bf6ee909..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.cc +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/constraint_utils.h" - -#include "cartographer/common/utils.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -void AddRotation3D(Pose3D* pose, ceres::Problem* problem) { - auto rotation = pose->mutable_rotation(); - problem->AddParameterBlock(rotation->data(), rotation->size()); - if (pose->constant()) { - problem->SetParameterBlockConstant(rotation->data()); - } -} - -void AddImuOrientation(ImuCalibration* imu_node, ceres::Problem* problem) { - auto imu_orientation = imu_node->mutable_orientation(); - problem->AddParameterBlock(imu_orientation->data(), imu_orientation->size()); - if (imu_node->constant()) { - problem->SetParameterBlockConstant(imu_orientation->data()); - } -} - -} // namespace - -RotationContraint3D::RotationContraint3D( - const ConstraintId& id, const proto::LossFunction& loss_function_proto, - const proto::Rotation3D& proto) - : Constraint(id, loss_function_proto), - first_(proto.first()), - second_(proto.second()), - imu_calibration_(proto.imu_calibration()), - cost_(new RotationCost3D(proto.parameters())), - ceres_cost_(absl::make_unique(cost_)) {} - -void RotationContraint3D::AddToSolver(Nodes* nodes, - ceres::Problem* problem) const { - FIND_NODE_OR_RETURN(first_node, first_, nodes->pose_3d_nodes, - "First node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(second_node, second_, nodes->pose_3d_nodes, - "Second node was not found in pose_3d_nodes."); - FIND_NODE_OR_RETURN(imu_node, imu_calibration_, nodes->imu_calibration_nodes, - "Imu calibration node was not found."); - - if (first_node->constant() && second_node->constant() && - imu_node->constant()) { - LOG(INFO) << "Both nodes are constant, skipping the constraint."; - return; - } - - AddRotation3D(first_node, problem); - AddRotation3D(second_node, problem); - AddImuOrientation(imu_node, problem); - problem->AddResidualBlock(ceres_cost_.get(), ceres_loss(), - first_node->mutable_rotation()->data(), - second_node->mutable_rotation()->data(), - imu_node->mutable_orientation()->data()); -} - -proto::CostFunction RotationContraint3D::ToCostFunctionProto() const { - proto::CostFunction cost_function; - auto* rotation_3d = cost_function.mutable_rotation_3d(); - *rotation_3d->mutable_first() = first_.ToProto(); - *rotation_3d->mutable_second() = second_.ToProto(); - *rotation_3d->mutable_imu_calibration() = imu_calibration_.ToProto(); - *rotation_3d->mutable_parameters() = cost_->ToProto(); - return cost_function; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d.h b/cartographer/pose_graph/constraint/rotation_constraint_3d.h deleted file mode 100644 index b0dbe24298..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d.h +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/constraint/cost_function/rotation_cost_3d.h" - -namespace cartographer { -namespace pose_graph { - -class RotationContraint3D : public Constraint { - public: - RotationContraint3D(const ConstraintId& id, - const proto::LossFunction& loss_function_proto, - const proto::Rotation3D& proto); - - void AddToSolver(Nodes* nodes, ceres::Problem* problem) const final; - - protected: - proto::CostFunction ToCostFunctionProto() const final; - - private: - using AutoDiffFunction = ceres::AutoDiffCostFunction< - RotationCost3D, 3 /* number of residuals */, 4 /* rotation first pose */, - 4 /* rotation second pose */, 4 /* imu calibration */>; - NodeId first_; - NodeId second_; - NodeId imu_calibration_; - // The cost function is owned by the ceres cost function. - RotationCost3D* const cost_; - std::unique_ptr ceres_cost_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_ROTATION_CONSTRAINT_3D_H_ diff --git a/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc b/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc deleted file mode 100644 index 0c2d8615c7..0000000000 --- a/cartographer/pose_graph/constraint/rotation_constraint_3d_test.cc +++ /dev/null @@ -1,54 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::EqualsProto; -using testing::ParseProto; - -constexpr char kConstraint[] = R"PROTO( - id: "narf" - cost_function { - rotation_3d { - first { object_id: "node0" } - second { object_id: "node1" } - imu_calibration { object_id: "imu_node" } - parameters { - delta_rotation_imu_frame { x: 0 y: 0.1 z: 0.2 w: 0.3 } - scaling_factor: 0.4 - } - } - } - loss_function { quadratic_loss {} } -)PROTO"; - -TEST(RotationConstraint3DTest, SerializesCorrectly) { - const auto proto = ParseProto(kConstraint); - RotationContraint3D constraint(proto.id(), proto.loss_function(), - proto.cost_function().rotation_3d()); - const auto actual_proto = constraint.ToProto(); - EXPECT_THAT(actual_proto, EqualsProto(kConstraint)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/imu_calibration.cc b/cartographer/pose_graph/node/imu_calibration.cc deleted file mode 100644 index 3389c7db61..0000000000 --- a/cartographer/pose_graph/node/imu_calibration.cc +++ /dev/null @@ -1,50 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/imu_calibration.h" - -namespace cartographer { -namespace pose_graph { - -ImuCalibration::ImuCalibration(const NodeId& node_id, bool constant, - const proto::ImuCalibration& imu) - : Node(node_id, constant), - gravity_constant_(imu.gravity_constant()), - orientation_{{imu.orientation().x(), imu.orientation().y(), - imu.orientation().z(), imu.orientation().w()}}, - orientation_parameterization_(imu.orientation_parameterization()) {} - -proto::Parameters ImuCalibration::ToParametersProto() const { - proto::Parameters parameters; - auto* imu_calibration = parameters.mutable_imu_calibration(); - - imu_calibration->set_gravity_constant(gravity_constant_); - - // TODO(pifon): Use a common method to convert from Eigen::Quaterniond to - // proto. Probably, the one defined in transform.h. - auto* orientation = imu_calibration->mutable_orientation(); - orientation->set_x(orientation_[0]); - orientation->set_y(orientation_[1]); - orientation->set_z(orientation_[2]); - orientation->set_w(orientation_[3]); - imu_calibration->set_orientation_parameterization( - orientation_parameterization_.ToProto()); - - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/imu_calibration.h b/cartographer/pose_graph/node/imu_calibration.h deleted file mode 100644 index 6febd5b959..0000000000 --- a/cartographer/pose_graph/node/imu_calibration.h +++ /dev/null @@ -1,57 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "cartographer/pose_graph/node/parameterization/parameterization.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -class ImuCalibration : public Node { - public: - ImuCalibration(const NodeId& node_id, bool constant, - const proto::ImuCalibration& imu_calibration); - - double* mutable_gravity_constant() { return &gravity_constant_; } - double gravity_constant() const { return gravity_constant_; } - - std::array* mutable_orientation() { return &orientation_; } - const std::array& orientation() const { return orientation_; } - - ceres::LocalParameterization* orientation_parameterization() const { - return orientation_parameterization_.ceres_parameterization(); - } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - double gravity_constant_; - std::array orientation_; - Parameterization orientation_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_IMU_CALIBRATION_H_ diff --git a/cartographer/pose_graph/node/imu_calibration_test.cc b/cartographer/pose_graph/node/imu_calibration_test.cc deleted file mode 100644 index 1894043b1c..0000000000 --- a/cartographer/pose_graph/node/imu_calibration_test.cc +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/imu_calibration.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "accelerometer" timestamp: 1 } - constant: true - parameters { - imu_calibration { - gravity_constant: 10 - orientation: { w: 0 x: 1 y: 2 z: 3 } - orientation_parameterization: YAW_ONLY - } - } -)PROTO"; - -TEST(Pose3DTest, ToProto) { - ImuCalibration imu_calibration({"accelerometer", common::FromUniversal(1)}, - true, ParseProto(R"( - gravity_constant: 10 - orientation: { w: 0 x: 1 y: 2 z: 3 } - orientation_parameterization: YAW_ONLY - )")); - EXPECT_THAT(imu_calibration.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/node.h b/cartographer/pose_graph/node/node.h deleted file mode 100644 index 8e29ea6a42..0000000000 --- a/cartographer/pose_graph/node/node.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ - -#include "cartographer/pose_graph/node/node_id.h" -#include "cartographer/pose_graph/proto/node.pb.h" - -#include - -namespace cartographer { -namespace pose_graph { - -class Node { - public: - explicit Node(const NodeId& id, bool constant) - : node_id_(id), constant_(constant) {} - - ~Node() = default; - - proto::Node ToProto() const; - - const NodeId node_id() const { return node_id_; } - - bool constant() const { return constant_; } - void set_constant(bool constant) { constant_ = constant; } - - protected: - virtual proto::Parameters ToParametersProto() const = 0; - - private: - NodeId node_id_; - bool constant_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_H_ diff --git a/cartographer/pose_graph/node/node_id.cc b/cartographer/pose_graph/node/node_id.cc deleted file mode 100644 index b242b333fe..0000000000 --- a/cartographer/pose_graph/node/node_id.cc +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/node_id.h" - -namespace cartographer { -namespace pose_graph { - -NodeId::NodeId(const std::string& object_id, common::Time time) - : object_id(object_id), time(time) {} - -NodeId::NodeId(const std::string& object_id, const std::string& group_id, - common::Time time) - : object_id(object_id), group_id(group_id), time(time) {} - -NodeId::NodeId(const proto::NodeId& node_id) - : NodeId(node_id.object_id(), node_id.group_id(), - common::FromUniversal(node_id.timestamp())) {} - -proto::NodeId NodeId::ToProto() const { - proto::NodeId node_id; - node_id.set_object_id(object_id); - node_id.set_group_id(group_id); - node_id.set_timestamp(common::ToUniversal(time)); - return node_id; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/node_id.h b/cartographer/pose_graph/node/node_id.h deleted file mode 100644 index ecb62b1ae4..0000000000 --- a/cartographer/pose_graph/node/node_id.h +++ /dev/null @@ -1,63 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_ - -#include -#include - -#include "cartographer/common/time.h" -#include "cartographer/pose_graph/proto/node.pb.h" - -namespace cartographer { -namespace pose_graph { - -struct NodeId { - NodeId(const std::string& object_id, common::Time time); - NodeId(const std::string& object_id, const std::string& group_id, - common::Time time); - explicit NodeId(const proto::NodeId& node_id); - - // Object refers to dynamic/static objects, e.g. robot/landmark/submap poses, - // IMU calibration, etc. - std::string object_id; - // Id of the group to which the node belongs, e.g. "submap". - std::string group_id; - // Time associated with the object's pose. - common::Time time; - - proto::NodeId ToProto() const; -}; - -inline bool operator<(const NodeId& lhs, const NodeId& rhs) { - return std::forward_as_tuple(lhs.object_id, lhs.group_id, lhs.time) < - std::forward_as_tuple(rhs.object_id, rhs.group_id, rhs.time); -} - -inline std::ostream& operator<<(std::ostream& os, const NodeId& id) { - std::string group_message; - if (!id.group_id.empty()) { - group_message = ", group_id: " + id.group_id; - } - return os << "(object_id: " << id.object_id << group_message - << ", time: " << id.time << ")"; -} - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODE_ID_ diff --git a/cartographer/pose_graph/node/node_id_test.cc b/cartographer/pose_graph/node/node_id_test.cc deleted file mode 100644 index 37a5a18f68..0000000000 --- a/cartographer/pose_graph/node/node_id_test.cc +++ /dev/null @@ -1,62 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/node_id.h" - -#include - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -TEST(NodeIdTest, FromProto) { - proto::NodeId proto; - proto.set_object_id("some_object"); - proto.set_group_id("submap"); - proto.set_timestamp(1); - - NodeId node_id(proto); - EXPECT_EQ(node_id.object_id, "some_object"); - EXPECT_EQ(node_id.group_id, "submap"); - EXPECT_EQ(common::ToUniversal(node_id.time), 1); -} - -TEST(NodeIdTest, ToProto) { - NodeId node_id{"some_object", "submap", common::FromUniversal(1)}; - EXPECT_THAT(node_id.ToProto(), - testing::EqualsProto( - "object_id: 'some_object' group_id: 'submap' timestamp: 1")); -} - -TEST(NodeIdTest, ToStringWithoutGroupId) { - std::stringstream ss; - ss << NodeId{"some_object", common::FromUniversal(1)}; - - EXPECT_EQ("(object_id: some_object, time: 1)", ss.str()); -} - -TEST(NodeIdTest, ToStringWithGroupId) { - std::stringstream ss; - ss << NodeId{"some_object", "submap", common::FromUniversal(1)}; - - EXPECT_EQ("(object_id: some_object, group_id: submap, time: 1)", ss.str()); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/nodes.h b/cartographer/pose_graph/node/nodes.h deleted file mode 100644 index 21f98602be..0000000000 --- a/cartographer/pose_graph/node/nodes.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ - -#include "cartographer/pose_graph/node/imu_calibration.h" -#include "cartographer/pose_graph/node/pose_2d.h" -#include "cartographer/pose_graph/node/pose_3d.h" - -#include - -namespace cartographer { -namespace pose_graph { - -struct Nodes { - // TODO(pifon): Migrate to Swiss Tables when they are out. - std::map> pose_2d_nodes; - std::map> pose_3d_nodes; - std::map> imu_calibration_nodes; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_NODES_H_ diff --git a/cartographer/pose_graph/node/parameterization/parameterization.cc b/cartographer/pose_graph/node/parameterization/parameterization.cc deleted file mode 100644 index 44a5be4529..0000000000 --- a/cartographer/pose_graph/node/parameterization/parameterization.cc +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/parameterization/parameterization.h" - -#include "absl/memory/memory.h" -#include "cartographer/common/math.h" -#include "cartographer/mapping/internal/3d/rotation_parameterization.h" -#include "ceres/autodiff_local_parameterization.h" -#include "ceres/rotation.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using absl::make_unique; -using ceres::AutoDiffLocalParameterization; -using ceres::LocalParameterization; - -// TODO(pifon): Check if the functors are compatible with our quaternions. Test! -std::unique_ptr CeresParameterizationFromProto( - const proto::Parameterization& parameterization) { - switch (parameterization) { - case (proto::Parameterization::NONE): - return nullptr; - case (proto::Parameterization::YAW_ONLY): - return make_unique>(); - case (proto::Parameterization::YAW_CONSTANT): - return make_unique>(); - case (proto::Parameterization::FIX_Z): - return make_unique( - 3, /* constant parameters */ std::vector{2}); - default: - LOG(FATAL) << "Parameterization is not known."; - } - return nullptr; -} - -} // namespace - -Parameterization::Parameterization(const proto::Parameterization& proto) - : proto_(proto), - ceres_parameterization_(CeresParameterizationFromProto(proto_)) {} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/parameterization/parameterization.h b/cartographer/pose_graph/node/parameterization/parameterization.h deleted file mode 100644 index eab1e58cc6..0000000000 --- a/cartographer/pose_graph/node/parameterization/parameterization.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ - -#include "cartographer/pose_graph/proto/node.pb.h" -#include "ceres/local_parameterization.h" - -namespace cartographer { -namespace pose_graph { - -class Parameterization { - public: - explicit Parameterization(const proto::Parameterization& proto); - - const proto::Parameterization& ToProto() const { return proto_; } - - ceres::LocalParameterization* ceres_parameterization() const { - return ceres_parameterization_.get(); - } - - private: - const proto::Parameterization proto_; - const std::unique_ptr ceres_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_PARAMETERIZATION_PARAMETERIZATION_H_ diff --git a/cartographer/pose_graph/node/pose_2d.cc b/cartographer/pose_graph/node/pose_2d.cc deleted file mode 100644 index 50cba73eef..0000000000 --- a/cartographer/pose_graph/node/pose_2d.cc +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/pose_2d.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -constexpr size_t kXIndex = 0; -constexpr size_t kYIndex = 1; -constexpr size_t kRotationIndex = 2; - -} // namespace - -Pose2D::Pose2D(const NodeId& node_id, bool constant, - const proto::Pose2D& pose_2d) - : Node(node_id, constant), - pose_2d_{{pose_2d.translation().x(), pose_2d.translation().y(), - pose_2d.rotation()}} {} - -proto::Parameters Pose2D::ToParametersProto() const { - proto::Parameters parameters; - auto* pose_2d = parameters.mutable_pose_2d(); - pose_2d->set_rotation(pose_2d_[kRotationIndex]); - - auto* translation = pose_2d->mutable_translation(); - translation->set_x(pose_2d_[kXIndex]); - translation->set_y(pose_2d_[kYIndex]); - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_2d.h b/cartographer/pose_graph/node/pose_2d.h deleted file mode 100644 index 5b94bb3e58..0000000000 --- a/cartographer/pose_graph/node/pose_2d.h +++ /dev/null @@ -1,46 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "Eigen/Core" - -namespace cartographer { -namespace pose_graph { - -class Pose2D : public Node { - public: - Pose2D(const NodeId& node_id, bool constant, const proto::Pose2D& pose_2d); - - std::array* mutable_pose_2d() { return &pose_2d_; } - const std::array& pose_2d() const { return pose_2d_; } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - std::array pose_2d_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_2D_H_ diff --git a/cartographer/pose_graph/node/pose_2d_test.cc b/cartographer/pose_graph/node/pose_2d_test.cc deleted file mode 100644 index 793277f3cc..0000000000 --- a/cartographer/pose_graph/node/pose_2d_test.cc +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/pose_2d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "flat_world" timestamp: 1 } - constant: true - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -TEST(Pose2DTest, ToProto) { - Pose2D pose_2d({"flat_world", common::FromUniversal(1)}, true, - ParseProto(R"( - translation { x: 1 y: 2 } - rotation: 5 - )")); - EXPECT_THAT(pose_2d.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_3d.cc b/cartographer/pose_graph/node/pose_3d.cc deleted file mode 100644 index d743e72ec8..0000000000 --- a/cartographer/pose_graph/node/pose_3d.cc +++ /dev/null @@ -1,54 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/pose_3d.h" - -namespace cartographer { -namespace pose_graph { - -Pose3D::Pose3D(const NodeId& node_id, bool constant, - const proto::Pose3D& pose_3d) - : Node(node_id, constant), - translation_{{pose_3d.translation().x(), pose_3d.translation().y(), - pose_3d.translation().z()}}, - translation_parameterization_(pose_3d.translation_parameterization()), - rotation_{{pose_3d.rotation().x(), pose_3d.rotation().y(), - pose_3d.rotation().z(), pose_3d.rotation().w()}}, - rotation_parameterization_(pose_3d.rotation_parameterization()) {} - -proto::Parameters Pose3D::ToParametersProto() const { - proto::Parameters parameters; - auto* pose_3d = parameters.mutable_pose_3d(); - - auto* translation = pose_3d->mutable_translation(); - translation->set_x(translation_[0]); - translation->set_y(translation_[1]); - translation->set_z(translation_[2]); - pose_3d->set_translation_parameterization( - translation_parameterization_.ToProto()); - - auto* rotation = pose_3d->mutable_rotation(); - rotation->set_x(rotation_[0]); - rotation->set_y(rotation_[1]); - rotation->set_z(rotation_[2]); - rotation->set_w(rotation_[3]); - pose_3d->set_rotation_parameterization(rotation_parameterization_.ToProto()); - - return parameters; -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/node/pose_3d.h b/cartographer/pose_graph/node/pose_3d.h deleted file mode 100644 index 1b09781bc8..0000000000 --- a/cartographer/pose_graph/node/pose_3d.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ -#define CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ - -#include "cartographer/pose_graph/node/node.h" - -#include - -#include "cartographer/pose_graph/node/parameterization/parameterization.h" -#include "cartographer/transform/transform.h" - -namespace cartographer { -namespace pose_graph { - -class Pose3D : public Node { - public: - Pose3D(const NodeId& node_id, bool constant, const proto::Pose3D& pose_3d); - - std::array* mutable_translation() { return &translation_; } - const std::array& translation() const { return translation_; } - ceres::LocalParameterization* translation_parameterization() const { - return translation_parameterization_.ceres_parameterization(); - } - - std::array* mutable_rotation() { return &rotation_; } - const std::array& rotation() const { return rotation_; } - ceres::LocalParameterization* rotation_parameterization() const { - return rotation_parameterization_.ceres_parameterization(); - } - - protected: - proto::Parameters ToParametersProto() const final; - - private: - std::array translation_; - Parameterization translation_parameterization_; - - std::array rotation_; - Parameterization rotation_parameterization_; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_NODE_POSE_3D_H_ diff --git a/cartographer/pose_graph/node/pose_3d_test.cc b/cartographer/pose_graph/node/pose_3d_test.cc deleted file mode 100644 index 25b1059a5d..0000000000 --- a/cartographer/pose_graph/node/pose_3d_test.cc +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/node/pose_3d.h" - -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kExpectedNode[] = R"PROTO( - id { object_id: "bumpy_world" timestamp: 1 } - constant: true - parameters { - pose_3d { - translation { x: 1 y: 2 z: 3 } - translation_parameterization: FIX_Z - rotation: { w: 0 x: 1 y: 2 z: 3 } - } - } -)PROTO"; - -TEST(Pose3DTest, ToProto) { - Pose3D pose_3d({"bumpy_world", common::FromUniversal(1)}, true, - ParseProto(R"( - translation { x: 1 y: 2 z: 3 } - translation_parameterization: FIX_Z - rotation: { w: 0 x: 1 y: 2 z: 3 } - rotation_parameterization: NONE - )")); - EXPECT_THAT(pose_3d.ToProto(), testing::EqualsProto(kExpectedNode)); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_controller.cc b/cartographer/pose_graph/pose_graph_controller.cc deleted file mode 100644 index 5a7aa2cec3..0000000000 --- a/cartographer/pose_graph/pose_graph_controller.cc +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/pose_graph_controller.h" - -namespace cartographer { -namespace pose_graph { - -void PoseGraphController::AddData(const proto::PoseGraphData& data) { - absl::MutexLock locker(&mutex_); - for (const auto& node : data.nodes()) { - AddNodeToPoseGraphData(node, &data_); - } - for (const auto& constraint : data.constraints()) { - AddConstraintToPoseGraphData(constraint, &data_); - } -} - -void PoseGraphController::AddNode(const proto::Node& node) { - absl::MutexLock locker(&mutex_); - AddNodeToPoseGraphData(node, &data_); -} - -void PoseGraphController::AddConstraint(const proto::Constraint& constraint) { - absl::MutexLock locker(&mutex_); - AddConstraintToPoseGraphData(constraint, &data_); -} - -Solver::SolverStatus PoseGraphController::Optimize() { - absl::MutexLock locker(&mutex_); - return solver_->Solve(&data_); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_controller.h b/cartographer/pose_graph/pose_graph_controller.h deleted file mode 100644 index 3c915edfc6..0000000000 --- a/cartographer/pose_graph/pose_graph_controller.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ -#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ - -#include "absl/synchronization/mutex.h" -#include "cartographer/pose_graph/pose_graph_data.h" -#include "cartographer/pose_graph/proto/pose_graph_data.pb.h" -#include "cartographer/pose_graph/solver/solver.h" - -namespace cartographer { -namespace pose_graph { - -class PoseGraphController { - public: - PoseGraphController(std::unique_ptr optimizer) - : solver_(std::move(optimizer)) {} - - PoseGraphController(const PoseGraphController&) = delete; - PoseGraphController& operator=(const PoseGraphController&) = delete; - - void AddData(const proto::PoseGraphData& data) LOCKS_EXCLUDED(mutex_); - void AddNode(const proto::Node& node) LOCKS_EXCLUDED(mutex_); - void AddConstraint(const proto::Constraint& constraint) - LOCKS_EXCLUDED(mutex_); - - Solver::SolverStatus Optimize() LOCKS_EXCLUDED(mutex_); - - private: - std::unique_ptr solver_; - - mutable absl::Mutex mutex_; - PoseGraphData data_ GUARDED_BY(mutex_); -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_CONTROLLER_H_ diff --git a/cartographer/pose_graph/pose_graph_data.cc b/cartographer/pose_graph/pose_graph_data.cc deleted file mode 100644 index 87686c9252..0000000000 --- a/cartographer/pose_graph/pose_graph_data.cc +++ /dev/null @@ -1,100 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/pose_graph_data.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/acceleration_constraint_3d.h" -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_2d.h" -#include "cartographer/pose_graph/constraint/interpolated_relative_pose_constraint_3d.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_3d.h" -#include "cartographer/pose_graph/constraint/rotation_constraint_3d.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using absl::make_unique; - -std::unique_ptr CreateConstraint( - const proto::Constraint& constraint) { - const auto& id = constraint.id(); - const auto& loss = constraint.loss_function(); - const auto& cost = constraint.cost_function(); - switch (cost.type_case()) { - case (proto::CostFunction::kRelativePose2D): - return make_unique(id, loss, - cost.relative_pose_2d()); - case (proto::CostFunction::kRelativePose3D): - return make_unique(id, loss, - cost.relative_pose_3d()); - case (proto::CostFunction::kAcceleration3D): - return make_unique(id, loss, - cost.acceleration_3d()); - case (proto::CostFunction::kRotation3D): - return make_unique(id, loss, cost.rotation_3d()); - case (proto::CostFunction::kInterpolatedRelativePose2D): - return make_unique( - id, loss, cost.interpolated_relative_pose_2d()); - case (proto::CostFunction::kInterpolatedRelativePose3D): - return make_unique( - id, loss, cost.interpolated_relative_pose_3d()); - case (proto::CostFunction::TYPE_NOT_SET): - LOG(FATAL) << "Constraint cost function type is not set."; - } - return nullptr; -} - -} // namespace - -void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data) { - NodeId node_id(node.id()); - switch (node.parameters().type_case()) { - case (proto::Parameters::kPose2D): { - data->nodes.pose_2d_nodes.emplace( - node_id, make_unique(node_id, node.constant(), - node.parameters().pose_2d())); - return; - } - case (proto::Parameters::kPose3D): { - data->nodes.pose_3d_nodes.emplace( - node_id, make_unique(node_id, node.constant(), - node.parameters().pose_3d())); - - return; - } - case (proto::Parameters::kImuCalibration): { - data->nodes.imu_calibration_nodes.emplace( - node_id, - make_unique(node_id, node.constant(), - node.parameters().imu_calibration())); - return; - } - case (proto::Parameters::TYPE_NOT_SET): { - LOG(FATAL) << "Node parameter type is not set."; - return; - } - } -} - -void AddConstraintToPoseGraphData(const proto::Constraint& constraint, - PoseGraphData* data) { - data->constraints.emplace_back(CreateConstraint(constraint)); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/pose_graph_data.h b/cartographer/pose_graph/pose_graph_data.h deleted file mode 100644 index 1a6b30c84c..0000000000 --- a/cartographer/pose_graph/pose_graph_data.h +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ -#define CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ - -#include "cartographer/pose_graph/constraint/constraint.h" -#include "cartographer/pose_graph/node/nodes.h" - -namespace cartographer { -namespace pose_graph { - -struct PoseGraphData { - Nodes nodes; - std::vector> constraints; -}; - -void AddNodeToPoseGraphData(const proto::Node& node, PoseGraphData* data); -void AddConstraintToPoseGraphData(const proto::Constraint& constraint, - PoseGraphData* data); - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_POSE_GRAPH_DATA_H_ diff --git a/cartographer/pose_graph/proto/constraint.proto b/cartographer/pose_graph/proto/constraint.proto deleted file mode 100644 index 44b6d0538d..0000000000 --- a/cartographer/pose_graph/proto/constraint.proto +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/loss_function.proto"; -import "cartographer/pose_graph/proto/cost_function.proto"; - -package cartographer.pose_graph.proto; - -message Constraint { - string id = 1; - CostFunction cost_function = 2; - LossFunction loss_function = 3; -} diff --git a/cartographer/pose_graph/proto/cost_function.proto b/cartographer/pose_graph/proto/cost_function.proto deleted file mode 100644 index 877f66f4c7..0000000000 --- a/cartographer/pose_graph/proto/cost_function.proto +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/node.proto"; -import "cartographer/transform/proto/transform.proto"; - -package cartographer.pose_graph.proto; - -message RelativePose2D { - NodeId first = 1; - NodeId second = 2; - - message Parameters { - transform.proto.Rigid2d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - } - Parameters parameters = 3; -} - -message RelativePose3D { - NodeId first = 1; - NodeId second = 2; - - message Parameters { - transform.proto.Rigid3d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - } - Parameters parameters = 3; -} - -message Acceleration3D { - NodeId first = 1; - NodeId second = 2; - NodeId third = 3; - NodeId imu_calibration = 4; - - message Parameters { - transform.proto.Vector3d delta_velocity_imu_frame = 1; - double first_to_second_delta_time_seconds = 2; - double second_to_third_delta_time_seconds = 3; - double scaling_factor = 4; - } - Parameters parameters = 5; -} - -message Rotation3D { - NodeId first = 1; - NodeId second = 2; - NodeId imu_calibration = 3; - - message Parameters { - transform.proto.Quaterniond delta_rotation_imu_frame = 1; - double scaling_factor = 2; - } - Parameters parameters = 4; -} - -message InterpolatedRelativePose2D { - NodeId first_start = 1; - NodeId first_end = 2; - NodeId second = 3; - - message Parameters { - transform.proto.Rigid3d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - transform.proto.Quaterniond gravity_alignment_first_start = 4; - transform.proto.Quaterniond gravity_alignment_first_end = 5; - // Interpolates between first_start and first_end. - double interpolation_factor = 6; - } - Parameters parameters = 4; -} - -message InterpolatedRelativePose3D { - NodeId first_start = 1; - NodeId first_end = 2; - NodeId second = 3; - - message Parameters { - transform.proto.Rigid3d first_t_second = 1; - double translation_weight = 2; - double rotation_weight = 3; - // Interpolates between first_start and first_end. - double interpolation_factor = 4; - } - Parameters parameters = 4; -} - -message CostFunction { - oneof type { - RelativePose2D relative_pose_2d = 1; - RelativePose3D relative_pose_3d = 2; - Acceleration3D acceleration_3d = 3; - Rotation3D rotation_3d = 4; - InterpolatedRelativePose2D interpolated_relative_pose_2d = 5; - InterpolatedRelativePose3D interpolated_relative_pose_3d = 6; - } -} diff --git a/cartographer/pose_graph/proto/loss_function.proto b/cartographer/pose_graph/proto/loss_function.proto deleted file mode 100644 index a45e557f97..0000000000 --- a/cartographer/pose_graph/proto/loss_function.proto +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -package cartographer.pose_graph.proto; - -message QuadraticLoss {} - -message HuberLoss { - double scale = 1; -} - -message LossFunction { - oneof Type { - QuadraticLoss quadratic_loss = 1; - HuberLoss huber_loss = 2; - } -} diff --git a/cartographer/pose_graph/proto/node.proto b/cartographer/pose_graph/proto/node.proto deleted file mode 100644 index 597a95ea8c..0000000000 --- a/cartographer/pose_graph/proto/node.proto +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -package cartographer.pose_graph.proto; - -import "cartographer/transform/proto/transform.proto"; - -enum Parameterization { - NONE = 0; - FIX_Z = 1; - YAW_ONLY = 2; - YAW_CONSTANT = 3; -} - -message Pose2D { - transform.proto.Vector2d translation = 1; - double rotation = 2; -} - -message Pose3D { - transform.proto.Vector3d translation = 1; - transform.proto.Quaterniond rotation = 2; - Parameterization translation_parameterization = 3; - Parameterization rotation_parameterization = 4; -} - -message ImuCalibration { - double gravity_constant = 1; - transform.proto.Quaterniond orientation = 2; - Parameterization orientation_parameterization = 3; -} - -message NodeId { - string object_id = 1; - string group_id = 2; - int64 timestamp = 3; -} - -message Parameters { - oneof type { - Pose2D pose_2d = 1; - Pose3D pose_3d = 2; - ImuCalibration imu_calibration = 3; - } -} - -message Node { - NodeId id = 1; - bool constant = 2; - Parameters parameters = 3; -} - diff --git a/cartographer/pose_graph/proto/pose_graph_data.proto b/cartographer/pose_graph/proto/pose_graph_data.proto deleted file mode 100644 index b76966ae85..0000000000 --- a/cartographer/pose_graph/proto/pose_graph_data.proto +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2018 The Cartographer Authors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -syntax = "proto3"; - -import "cartographer/pose_graph/proto/node.proto"; -import "cartographer/pose_graph/proto/constraint.proto"; - -package cartographer.pose_graph.proto; - -message PoseGraphData { - repeated Node nodes = 1; - repeated Constraint constraints = 2; -} diff --git a/cartographer/pose_graph/proto/solver_config.proto b/cartographer/pose_graph/proto/solver_config.proto deleted file mode 100644 index cec67ccdb9..0000000000 --- a/cartographer/pose_graph/proto/solver_config.proto +++ /dev/null @@ -1,14 +0,0 @@ -syntax = "proto3"; - -import "cartographer/common/proto/ceres_solver_options.proto"; - -package cartographer.pose_graph.proto; - -// Configuration for the non-linear least squares solver. -message SolverConfig { - // If true, logs a summary of the optimization in every iteration. - bool log_solver_summary = 1; - - // Options specific for the Ceres solver. - cartographer.common.proto.CeresSolverOptions ceres_options = 2; -} diff --git a/cartographer/pose_graph/solver/ceres_solver.cc b/cartographer/pose_graph/solver/ceres_solver.cc deleted file mode 100644 index 3a7421b3db..0000000000 --- a/cartographer/pose_graph/solver/ceres_solver.cc +++ /dev/null @@ -1,64 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/solver/ceres_solver.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -ceres::Problem::Options CreateCeresProblemOptions() { - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = - ceres::Ownership::DO_NOT_TAKE_OWNERSHIP; - return problem_options; -} - -Solver::SolverStatus ToSolverStatus( - const ceres::TerminationType& termination_type) { - switch (termination_type) { - case (ceres::TerminationType::CONVERGENCE): - return Solver::SolverStatus::CONVERGENCE; - case (ceres::TerminationType::NO_CONVERGENCE): - return Solver::SolverStatus::NO_CONVERGENCE; - default: - return Solver::SolverStatus::FAILURE; - } -} - -} // namespace - -CeresSolver::CeresSolver(const ceres::Solver::Options& options) - : problem_options_(CreateCeresProblemOptions()), solver_options_(options) {} - -Solver::SolverStatus CeresSolver::Solve(PoseGraphData* data) const { - ceres::Problem problem(problem_options_); - - for (const auto& constraint : data->constraints) { - constraint->AddToSolver(&data->nodes, &problem); - } - - ceres::Solver::Summary summary; - ceres::Solve(solver_options_, &problem, &summary); - return ToSolverStatus(summary.termination_type); -} - -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/solver/ceres_solver_test.cc b/cartographer/pose_graph/solver/ceres_solver_test.cc deleted file mode 100644 index f86b9ae5b1..0000000000 --- a/cartographer/pose_graph/solver/ceres_solver_test.cc +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "cartographer/pose_graph/solver/ceres_solver.h" - -#include "absl/memory/memory.h" -#include "cartographer/pose_graph/constraint/relative_pose_constraint_2d.h" -#include "cartographer/testing/test_helpers.h" - -namespace cartographer { -namespace pose_graph { -namespace { - -using testing::ParseProto; - -constexpr char kStartNode[] = R"PROTO( - id { object_id: "start_node" timestamp: 1 } - constant: false - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -constexpr char kEndNode[] = R"PROTO( - id { object_id: "end_node" timestamp: 1 } - constant: false - parameters { - pose_2d { - translation { x: 1 y: 2 } - rotation: 5 - } - } -)PROTO"; - -constexpr char kRelativePose2D[] = R"PROTO( - id: "constraint_1" - loss_function { quadratic_loss: {} } - cost_function { - relative_pose_2d { - first { object_id: "start_node" timestamp: 1 } - second { object_id: "end_node" timestamp: 1 } - parameters { - first_t_second { - translation { x: 1 y: 1 } - rotation: 0 - } - translation_weight: 1 - rotation_weight: 1 - } - } - } -)PROTO"; - -TEST(CeresSolverTest, SmokeTest) { - PoseGraphData data; - AddNodeToPoseGraphData(ParseProto(kStartNode), &data); - AddNodeToPoseGraphData(ParseProto(kEndNode), &data); - AddConstraintToPoseGraphData(ParseProto(kRelativePose2D), - &data); - CeresSolver optimizer(ceres::Solver::Options{}); - EXPECT_EQ(optimizer.Solve(&data), Solver::SolverStatus::CONVERGENCE); -} - -} // namespace -} // namespace pose_graph -} // namespace cartographer diff --git a/cartographer/pose_graph/solver/solver.h b/cartographer/pose_graph/solver/solver.h deleted file mode 100644 index 11bb2a199d..0000000000 --- a/cartographer/pose_graph/solver/solver.h +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ -#define CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ - -#include "cartographer/pose_graph/pose_graph_data.h" - -namespace cartographer { -namespace pose_graph { - -class Solver { - public: - enum class SolverStatus { - CONVERGENCE, - NO_CONVERGENCE, - FAILURE, - }; - - Solver() = default; - virtual ~Solver() = default; - - Solver(const Solver&) = delete; - Solver& operator=(const Solver&) = delete; - - virtual SolverStatus Solve(PoseGraphData* data) const = 0; -}; - -} // namespace pose_graph -} // namespace cartographer - -#endif // CARTOGRAPHER_POSE_GRAPH_SOLVER_SOLVER_H_ diff --git a/cartographer/sensor/compressed_point_cloud_test.cc b/cartographer/sensor/compressed_point_cloud_test.cc index 341ad7841d..7dcd6bc131 100644 --- a/cartographer/sensor/compressed_point_cloud_test.cc +++ b/cartographer/sensor/compressed_point_cloud_test.cc @@ -48,10 +48,10 @@ MATCHER_P(ApproximatelyEquals, expected, // Helper function to test the mapping of a single point. Includes test for // recompressing the same point again. void TestPoint(const Eigen::Vector3f& p) { - CompressedPointCloud compressed({{p}}); + CompressedPointCloud compressed(PointCloud({{p}})); EXPECT_EQ(1, compressed.size()); EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); - CompressedPointCloud recompressed({*compressed.begin()}); + CompressedPointCloud recompressed(PointCloud({*compressed.begin()})); EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p)); } @@ -70,18 +70,19 @@ TEST(CompressPointCloudTest, CompressesPointsCorrectly) { } TEST(CompressPointCloudTest, Compresses) { - const CompressedPointCloud compressed({{Eigen::Vector3f(0.838f, 0, 0)}, - {Eigen::Vector3f(0.839f, 0, 0)}, - {Eigen::Vector3f(0.840f, 0, 0)}}); + const CompressedPointCloud compressed( + PointCloud({{Eigen::Vector3f(0.838f, 0, 0)}, + {Eigen::Vector3f(0.839f, 0, 0)}, + {Eigen::Vector3f(0.840f, 0, 0)}})); EXPECT_FALSE(compressed.empty()); EXPECT_EQ(3, compressed.size()); const PointCloud decompressed = compressed.Decompress(); EXPECT_EQ(3, decompressed.size()); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0)))); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0)))); - EXPECT_THAT(decompressed, + EXPECT_THAT(decompressed.points(), Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0)))); } diff --git a/cartographer/sensor/internal/ordered_multi_queue.h b/cartographer/sensor/internal/ordered_multi_queue.h index 36f17e371a..d4849863b8 100644 --- a/cartographer/sensor/internal/ordered_multi_queue.h +++ b/cartographer/sensor/internal/ordered_multi_queue.h @@ -23,7 +23,7 @@ #include #include -#include "cartographer/common/blocking_queue.h" +#include "cartographer/common/internal/blocking_queue.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/sensor/internal/dispatchable.h" diff --git a/cartographer/sensor/internal/voxel_filter.cc b/cartographer/sensor/internal/voxel_filter.cc index 4185b33600..a6e5c0b574 100644 --- a/cartographer/sensor/internal/voxel_filter.cc +++ b/cartographer/sensor/internal/voxel_filter.cc @@ -17,7 +17,10 @@ #include "cartographer/sensor/internal/voxel_filter.h" #include +#include +#include +#include "absl/container/flat_hash_map.h" #include "cartographer/common/math.h" namespace cartographer { @@ -27,13 +30,9 @@ namespace { PointCloud FilterByMaxRange(const PointCloud& point_cloud, const float max_range) { - PointCloud result; - for (const RangefinderPoint& point : point_cloud) { - if (point.position.norm() <= max_range) { - result.push_back(point); - } - } - return result; + return point_cloud.copy_if([max_range](const RangefinderPoint& point) { + return point.position.norm() <= max_range; + }); } PointCloud AdaptivelyVoxelFiltered( @@ -43,7 +42,7 @@ PointCloud AdaptivelyVoxelFiltered( // 'point_cloud' is already sparse enough. return point_cloud; } - PointCloud result = VoxelFilter(options.max_length()).Filter(point_cloud); + PointCloud result = VoxelFilter(point_cloud, options.max_length()); if (result.size() >= options.min_num_points()) { // Filtering with 'max_length' resulted in a sufficiently dense point cloud. return result; @@ -54,15 +53,14 @@ PointCloud AdaptivelyVoxelFiltered( for (float high_length = options.max_length(); high_length > 1e-2f * options.max_length(); high_length /= 2.f) { float low_length = high_length / 2.f; - result = VoxelFilter(low_length).Filter(point_cloud); + result = VoxelFilter(point_cloud, low_length); if (result.size() >= options.min_num_points()) { // Binary search to find the right amount of filtering. 'low_length' gave // a sufficiently dense 'result', 'high_length' did not. We stop when the // edge length is at most 10% off. while ((high_length - low_length) / low_length > 1e-1f) { const float mid_length = (low_length + high_length) / 2.f; - const PointCloud candidate = - VoxelFilter(mid_length).Filter(point_cloud); + const PointCloud candidate = VoxelFilter(point_cloud, mid_length); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; @@ -76,58 +74,109 @@ PointCloud AdaptivelyVoxelFiltered( return result; } -} // namespace +using VoxelKeyType = uint64_t; -PointCloud VoxelFilter::Filter(const PointCloud& point_cloud) { - PointCloud results; - for (const RangefinderPoint& point : point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); +VoxelKeyType GetVoxelCellIndex(const Eigen::Vector3f& point, + const float resolution) { + const Eigen::Array3f index = point.array() / resolution; + const uint64_t x = common::RoundToInt(index.x()); + const uint64_t y = common::RoundToInt(index.y()); + const uint64_t z = common::RoundToInt(index.z()); + return (x << 42) + (y << 21) + z; +} + +template +std::vector RandomizedVoxelFilterIndices( + const std::vector& point_cloud, const float resolution, + PointFunction&& point_function) { + // According to https://en.wikipedia.org/wiki/Reservoir_sampling + std::minstd_rand0 generator; + absl::flat_hash_map> + voxel_count_and_point_index; + for (size_t i = 0; i < point_cloud.size(); i++) { + auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex( + point_function(point_cloud[i]), resolution)]; + voxel.first++; + if (voxel.first == 1) { + voxel.second = i; + } else { + std::uniform_int_distribution<> distribution(1, voxel.first); + if (distribution(generator) == voxel.first) { + voxel.second = i; + } } } - return results; + std::vector points_used(point_cloud.size(), false); + for (const auto& voxel_and_index : voxel_count_and_point_index) { + points_used[voxel_and_index.second.second] = true; + } + return points_used; } -TimedPointCloud VoxelFilter::Filter(const TimedPointCloud& timed_point_cloud) { - TimedPointCloud results; - for (const TimedRangefinderPoint& point : timed_point_cloud) { - auto it_inserted = - voxel_set_.insert(IndexToKey(GetCellIndex(point.position))); - if (it_inserted.second) { - results.push_back(point); +template +std::vector RandomizedVoxelFilter(const std::vector& point_cloud, + const float resolution, + PointFunction&& point_function) { + const std::vector points_used = + RandomizedVoxelFilterIndices(point_cloud, resolution, point_function); + + std::vector results; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + results.push_back(point_cloud[i]); } } return results; } -std::vector VoxelFilter::Filter( - const std::vector& - range_measurements) { - std::vector results; - for (const auto& range_measurement : range_measurements) { - auto it_inserted = voxel_set_.insert( - IndexToKey(GetCellIndex(range_measurement.point_time.position))); - if (it_inserted.second) { - results.push_back(range_measurement); +} // namespace + +std::vector VoxelFilter( + const std::vector& points, const float resolution) { + return RandomizedVoxelFilter( + points, resolution, + [](const RangefinderPoint& point) { return point.position; }); +} + +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) { + const std::vector points_used = RandomizedVoxelFilterIndices( + point_cloud.points(), resolution, + [](const RangefinderPoint& point) { return point.position; }); + + std::vector filtered_points; + for (size_t i = 0; i < point_cloud.size(); i++) { + if (points_used[i]) { + filtered_points.push_back(point_cloud[i]); } } - return results; + std::vector filtered_intensities; + CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size()); + for (size_t i = 0; i < point_cloud.intensities().size(); i++) { + if (points_used[i]) { + filtered_intensities.push_back(point_cloud.intensities()[i]); + } + } + return PointCloud(std::move(filtered_points), + std::move(filtered_intensities)); } -VoxelFilter::KeyType VoxelFilter::IndexToKey(const Eigen::Array3i& index) { - KeyType k_0(static_cast(index[0])); - KeyType k_1(static_cast(index[1])); - KeyType k_2(static_cast(index[2])); - return (k_0 << 2 * 32) | (k_1 << 1 * 32) | k_2; +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution) { + return RandomizedVoxelFilter( + timed_point_cloud, resolution, + [](const TimedRangefinderPoint& point) { return point.position; }); } -Eigen::Array3i VoxelFilter::GetCellIndex(const Eigen::Vector3f& point) const { - Eigen::Array3f index = point.array() / resolution_; - return Eigen::Array3i(common::RoundToInt(index.x()), - common::RoundToInt(index.y()), - common::RoundToInt(index.z())); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution) { + return RandomizedVoxelFilter( + range_measurements, resolution, + [](const sensor::TimedPointCloudOriginData::RangeMeasurement& + range_measurement) { + return range_measurement.point_time.position; + }); } proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( @@ -140,13 +189,11 @@ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( return options; } -AdaptiveVoxelFilter::AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options) - : options_(options) {} - -PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options) { return AdaptivelyVoxelFiltered( - options_, FilterByMaxRange(point_cloud, options_.max_range())); + options, FilterByMaxRange(point_cloud, options.max_range())); } } // namespace sensor diff --git a/cartographer/sensor/internal/voxel_filter.h b/cartographer/sensor/internal/voxel_filter.h index f14575b7c2..a99f861650 100644 --- a/cartographer/sensor/internal/voxel_filter.h +++ b/cartographer/sensor/internal/voxel_filter.h @@ -19,7 +19,6 @@ #include -#include "absl/container/flat_hash_set.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" @@ -28,55 +27,22 @@ namespace cartographer { namespace sensor { -// Voxel filter for point clouds. For each voxel, the assembled point cloud -// contains the first point that fell into it from any of the inserted point -// clouds. -class VoxelFilter { - public: - // 'size' is the length of a voxel edge. - explicit VoxelFilter(float size) : resolution_(size) {} - - VoxelFilter(const VoxelFilter&) = delete; - VoxelFilter& operator=(const VoxelFilter&) = delete; - - // Returns a voxel filtered copy of 'point_cloud'. - PointCloud Filter(const PointCloud& point_cloud); - - // Same for TimedPointCloud. - TimedPointCloud Filter(const TimedPointCloud& timed_point_cloud); - - // Same for RangeMeasurement. - std::vector Filter( - const std::vector& - range_measurements); - - private: - using KeyType = std::bitset<3 * 32>; - - static KeyType IndexToKey(const Eigen::Array3i& index); - - Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const; - - float resolution_; - absl::flat_hash_set voxel_set_; -}; +std::vector VoxelFilter( + const std::vector& points, const float resolution); +PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution); +TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud, + const float resolution); +std::vector VoxelFilter( + const std::vector& + range_measurements, + const float resolution); proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary); -class AdaptiveVoxelFilter { - public: - explicit AdaptiveVoxelFilter( - const proto::AdaptiveVoxelFilterOptions& options); - - AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; - AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; - - PointCloud Filter(const PointCloud& point_cloud) const; - - private: - const proto::AdaptiveVoxelFilterOptions options_; -}; +PointCloud AdaptiveVoxelFilter( + const PointCloud& point_cloud, + const proto::AdaptiveVoxelFilterOptions& options); } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/internal/voxel_filter_test.cc b/cartographer/sensor/internal/voxel_filter_test.cc index 320a99e3fc..b84152f166 100644 --- a/cartographer/sensor/internal/voxel_filter_test.cc +++ b/cartographer/sensor/internal/voxel_filter_test.cc @@ -24,33 +24,61 @@ namespace cartographer { namespace sensor { namespace { -using ::testing::ContainerEq; - -TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { - PointCloud point_cloud = {{Eigen::Vector3f{0.f, 0.f, 0.f}}, - {Eigen::Vector3f{0.1f, -0.1f, 0.1f}}, - {Eigen::Vector3f{0.3f, -0.1f, 0.f}}, - {Eigen::Vector3f{0.f, 0.f, 0.1f}}}; - EXPECT_THAT(VoxelFilter(0.3f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); +using ::testing::Contains; +using ::testing::IsEmpty; + +TEST(VoxelFilterTest, ReturnsOnePointInEachVoxel) { + const PointCloud point_cloud({{{0.f, 0.f, 0.f}}, + {{0.1f, -0.1f, 0.1f}}, + {{0.3f, -0.1f, 0.f}}, + {{0.f, 0.f, 0.1f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + ASSERT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); + EXPECT_THAT(point_cloud.points(), Contains(result[0])); + EXPECT_THAT(point_cloud.points(), Contains(result[1])); + EXPECT_THAT(result.points(), Contains(point_cloud[2])); +} + +TEST(VoxelFilterTest, CorrectIntensities) { + std::vector points; + std::vector intensities; + for (int i = 0; i < 100; ++i) { + const float value = 0.1f * i; + // We add points with intensity equal to the z coordinate, so we can later + // verify that the resulting intensities are corresponding to the filtered + // points. + points.push_back({{-100.f, 0.3f, value}}); + intensities.push_back(value); + } + const PointCloud point_cloud(points, intensities); + const PointCloud result = VoxelFilter(point_cloud, 0.3f); + + ASSERT_EQ(result.intensities().size(), result.points().size()); + for (size_t i = 0; i < result.size(); ++i) { + ASSERT_NEAR(result[i].position.z(), result.intensities()[i], 1e-6); + } } TEST(VoxelFilterTest, HandlesLargeCoordinates) { - PointCloud point_cloud = {{Eigen::Vector3f{100000.f, 0.f, 0.f}}, - {Eigen::Vector3f{100000.001f, -0.0001f, 0.0001f}}, - {Eigen::Vector3f{100000.003f, -0.0001f, 0.f}}, - {Eigen::Vector3f{-200000.f, 0.f, 0.f}}}; - EXPECT_THAT(VoxelFilter(0.01f).Filter(point_cloud), - ContainerEq(PointCloud{point_cloud[0], point_cloud[3]})); + const PointCloud point_cloud({{{100000.f, 0.f, 0.f}}, + {{100000.001f, -0.0001f, 0.0001f}}, + {{100000.003f, -0.0001f, 0.f}}, + {{-200000.f, 0.f, 0.f}}}); + const PointCloud result = VoxelFilter(point_cloud, 0.01f); + EXPECT_EQ(result.size(), 2); + EXPECT_THAT(result.intensities(), IsEmpty()); + EXPECT_THAT(result.points(), Contains(point_cloud[3])); } TEST(VoxelFilterTest, IgnoresTime) { TimedPointCloud timed_point_cloud; for (int i = 0; i < 100; ++i) { - timed_point_cloud.push_back({Eigen::Vector3f{-100.f, 0.3f, 0.4f}, 1.f * i}); + timed_point_cloud.push_back({{-100.f, 0.3f, 0.4f}, 1.f * i}); } - EXPECT_THAT(VoxelFilter(0.3f).Filter(timed_point_cloud), - ContainerEq(TimedPointCloud{timed_point_cloud[0]})); + const TimedPointCloud result = VoxelFilter(timed_point_cloud, 0.3f); + ASSERT_EQ(result.size(), 1); + EXPECT_THAT(timed_point_cloud, Contains(result[0])); } } // namespace diff --git a/cartographer/sensor/point_cloud.cc b/cartographer/sensor/point_cloud.cc index 1d5652027c..f616c190ca 100644 --- a/cartographer/sensor/point_cloud.cc +++ b/cartographer/sensor/point_cloud.cc @@ -22,14 +22,45 @@ namespace cartographer { namespace sensor { +PointCloud::PointCloud() {} +PointCloud::PointCloud(std::vector points) + : points_(std::move(points)) {} +PointCloud::PointCloud(std::vector points, + std::vector intensities) + : points_(std::move(points)), intensities_(std::move(intensities)) { + if (!intensities_.empty()) { + CHECK_EQ(points_.size(), intensities_.size()); + } +} + +size_t PointCloud::size() const { return points_.size(); } +bool PointCloud::empty() const { return points_.empty(); } + +const std::vector& PointCloud::points() const { + return points_; +} +const std::vector& PointCloud::intensities() const { + return intensities_; +} +const PointCloud::PointType& PointCloud::operator[](const size_t index) const { + return points_[index]; +} + +PointCloud::ConstIterator PointCloud::begin() const { return points_.begin(); } +PointCloud::ConstIterator PointCloud::end() const { return points_.end(); } + +void PointCloud::push_back(PointCloud::PointType value) { + points_.push_back(std::move(value)); +} + PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform) { - PointCloud result; - result.reserve(point_cloud.size()); - for (const RangefinderPoint& point : point_cloud) { - result.emplace_back(transform * point); + std::vector points; + points.reserve(point_cloud.size()); + for (const RangefinderPoint& point : point_cloud.points()) { + points.emplace_back(transform * point); } - return result; + return PointCloud(points, point_cloud.intensities()); } TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, @@ -44,24 +75,9 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, PointCloud CropPointCloud(const PointCloud& point_cloud, const float min_z, const float max_z) { - PointCloud cropped_point_cloud; - for (const RangefinderPoint& point : point_cloud) { - if (min_z <= point.position.z() && point.position.z() <= max_z) { - cropped_point_cloud.push_back(point); - } - } - return cropped_point_cloud; -} - -TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, - const float min_z, const float max_z) { - TimedPointCloud cropped_point_cloud; - for (const TimedRangefinderPoint& point : point_cloud) { - if (min_z <= point.position.z() && point.position.z() <= max_z) { - cropped_point_cloud.push_back(point); - } - } - return cropped_point_cloud; + return point_cloud.copy_if([min_z, max_z](const RangefinderPoint& point) { + return min_z <= point.position.z() && point.position.z() <= max_z; + }); } } // namespace sensor diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h index dd96965a98..289e367b6d 100644 --- a/cartographer/sensor/point_cloud.h +++ b/cartographer/sensor/point_cloud.h @@ -28,9 +28,68 @@ namespace cartographer { namespace sensor { -// Stores 3D positions of points. -// For 2D points, the third entry is 0.f. -using PointCloud = std::vector; +// Stores 3D positions of points together with some additional data, e.g. +// intensities. +class PointCloud { + public: + using PointType = RangefinderPoint; + + PointCloud(); + explicit PointCloud(std::vector points); + PointCloud(std::vector points, std::vector intensities); + + // Returns the number of points in the point cloud. + size_t size() const; + // Checks whether there are any points in the point cloud. + bool empty() const; + + const std::vector& points() const; + const std::vector& intensities() const; + const PointType& operator[](const size_t index) const; + + // Iterator over the points in the point cloud. + using ConstIterator = std::vector::const_iterator; + ConstIterator begin() const; + ConstIterator end() const; + + void push_back(PointType value); + + // Creates a PointCloud consisting of all the points for which `predicate` + // returns true, together with the corresponding intensities. + template + PointCloud copy_if(UnaryPredicate predicate) const { + std::vector points; + std::vector intensities; + + // Note: benchmarks show that it is better to have this conditional outside + // the loop. + if (intensities_.empty()) { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + if (predicate(point)) { + points.push_back(point); + } + } + } else { + for (size_t index = 0; index < size(); ++index) { + const PointType& point = points_[index]; + if (predicate(point)) { + points.push_back(point); + intensities.push_back(intensities_[index]); + } + } + } + + return PointCloud(points, intensities); + } + + private: + // For 2D points, the third entry is 0.f. + std::vector points_; + // Intensities are optional. If non-empty, they must have the same size as + // points. + std::vector intensities_; +}; // Stores 3D positions of points with their relative measurement time in the // fourth entry. Time is in seconds, increasing and relative to the moment when @@ -39,6 +98,8 @@ using PointCloud = std::vector; // third entry is 0.f (and the fourth entry is time). using TimedPointCloud = std::vector; +// TODO(wohe): Retained for cartographer_ros. To be removed once it is no +// longer used there. struct PointCloudWithIntensities { TimedPointCloud points; std::vector intensities; @@ -57,11 +118,6 @@ TimedPointCloud TransformTimedPointCloud(const TimedPointCloud& point_cloud, PointCloud CropPointCloud(const PointCloud& point_cloud, float min_z, float max_z); -// Returns a new point cloud without points that fall outside the region defined -// by 'min_z' and 'max_z'. -TimedPointCloud CropTimedPointCloud(const TimedPointCloud& point_cloud, - float min_z, float max_z); - } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/point_cloud_test.cc b/cartographer/sensor/point_cloud_test.cc index 0a6d9dfd48..47c6febcdd 100644 --- a/cartographer/sensor/point_cloud_test.cc +++ b/cartographer/sensor/point_cloud_test.cc @@ -15,20 +15,25 @@ */ #include "cartographer/sensor/point_cloud.h" -#include "cartographer/transform/transform.h" #include +#include "cartographer/transform/transform.h" +#include "gmock/gmock.h" #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { +using ::testing::ElementsAre; +using ::testing::FloatNear; +using ::testing::IsEmpty; + TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; - point_cloud.push_back({Eigen::Vector3f{0.5f, 0.5f, 1.f}}); - point_cloud.push_back({Eigen::Vector3f{3.5f, 0.5f, 42.f}}); + point_cloud.push_back({{Eigen::Vector3f{0.5f, 0.5f, 1.f}}}); + point_cloud.push_back({{Eigen::Vector3f{3.5f, 0.5f, 42.f}}}); const PointCloud transformed_point_cloud = TransformPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); EXPECT_NEAR(-0.5f, transformed_point_cloud[0].position.x(), 1e-6); @@ -49,6 +54,38 @@ TEST(PointCloudTest, TransformTimedPointCloud) { EXPECT_NEAR(3.5f, transformed_point_cloud[1].position.y(), 1e-6); } +TEST(PointCloudTest, CopyIf) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + + const PointCloud point_cloud(points); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_THAT(copied_point_cloud.intensities(), IsEmpty()); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); +} + +TEST(PointCloudTest, CopyIfWithIntensities) { + std::vector points = { + {{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}}; + std::vector intensities = {0.f, 1.f, 2.f}; + + const PointCloud point_cloud(points, intensities); + const PointCloud copied_point_cloud = point_cloud.copy_if( + [&](const RangefinderPoint& point) { return point.position.x() > 0.1f; }); + EXPECT_EQ(copied_point_cloud.size(), 2); + EXPECT_EQ(copied_point_cloud.intensities().size(), 2); + EXPECT_THAT(copied_point_cloud.points(), + ElementsAre(RangefinderPoint{Eigen::Vector3f{1.f, 1.f, 1.f}}, + RangefinderPoint{Eigen::Vector3f{2.f, 2.f, 2.f}})); + EXPECT_THAT(copied_point_cloud.intensities(), + ElementsAre(FloatNear(1.f, 1e-6), FloatNear(2.f, 1e-6))); +} + } // namespace } // namespace sensor } // namespace cartographer diff --git a/cartographer/sensor/proto/sensor.proto b/cartographer/sensor/proto/sensor.proto index 0b445b3ca5..62cc640a93 100644 --- a/cartographer/sensor/proto/sensor.proto +++ b/cartographer/sensor/proto/sensor.proto @@ -41,6 +41,7 @@ message TimedPointCloudData { transform.proto.Vector3f origin = 2; repeated transform.proto.Vector4f point_data_legacy = 3; repeated TimedRangefinderPoint point_data = 4; + repeated float intensities = 5; } // Proto representation of ::cartographer::sensor::RangeData. diff --git a/cartographer/sensor/range_data.cc b/cartographer/sensor/range_data.cc index 2eea185554..c30e68d36b 100644 --- a/cartographer/sensor/range_data.cc +++ b/cartographer/sensor/range_data.cc @@ -31,15 +31,6 @@ RangeData TransformRangeData(const RangeData& range_data, }; } -TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, - const transform::Rigid3f& transform) { - return TimedRangeData{ - transform * range_data.origin, - TransformTimedPointCloud(range_data.returns, transform), - TransformTimedPointCloud(range_data.misses, transform), - }; -} - RangeData CropRangeData(const RangeData& range_data, const float min_z, const float max_z) { return RangeData{range_data.origin, @@ -47,13 +38,6 @@ RangeData CropRangeData(const RangeData& range_data, const float min_z, CropPointCloud(range_data.misses, min_z, max_z)}; } -TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, - const float min_z, const float max_z) { - return TimedRangeData{range_data.origin, - CropTimedPointCloud(range_data.returns, min_z, max_z), - CropTimedPointCloud(range_data.misses, min_z, max_z)}; -} - proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); @@ -69,7 +53,7 @@ proto::RangeData ToProto(const RangeData& range_data) { } RangeData FromProto(const proto::RangeData& proto) { - PointCloud returns; + std::vector returns; if (proto.returns_size() > 0) { returns.reserve(proto.returns().size()); for (const auto& point_proto : proto.returns()) { @@ -81,7 +65,7 @@ RangeData FromProto(const proto::RangeData& proto) { returns.push_back({transform::ToEigen(point_proto)}); } } - PointCloud misses; + std::vector misses; if (proto.misses_size() > 0) { misses.reserve(proto.misses().size()); for (const auto& point_proto : proto.misses()) { @@ -93,7 +77,8 @@ RangeData FromProto(const proto::RangeData& proto) { misses.push_back({transform::ToEigen(point_proto)}); } } - return RangeData{transform::ToEigen(proto.origin()), returns, misses}; + return RangeData{transform::ToEigen(proto.origin()), PointCloud(returns), + PointCloud(misses)}; } } // namespace sensor diff --git a/cartographer/sensor/range_data.h b/cartographer/sensor/range_data.h index 7439bc7768..e7cd658d9b 100644 --- a/cartographer/sensor/range_data.h +++ b/cartographer/sensor/range_data.h @@ -35,26 +35,12 @@ struct RangeData { PointCloud misses; }; -// Like 'RangeData', but with 'TimedPointClouds'. -struct TimedRangeData { - Eigen::Vector3f origin; - TimedPointCloud returns; - TimedPointCloud misses; -}; - RangeData TransformRangeData(const RangeData& range_data, const transform::Rigid3f& transform); -TimedRangeData TransformTimedRangeData(const TimedRangeData& range_data, - const transform::Rigid3f& transform); - // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); -// Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. -TimedRangeData CropTimedRangeData(const TimedRangeData& range_data, float min_z, - float max_z); - // Converts 'range_data' to a proto::RangeData. proto::RangeData ToProto(const RangeData& range_data); diff --git a/cartographer/sensor/timed_point_cloud_data.cc b/cartographer/sensor/timed_point_cloud_data.cc index 63a17af397..2f01f08a4e 100644 --- a/cartographer/sensor/timed_point_cloud_data.cc +++ b/cartographer/sensor/timed_point_cloud_data.cc @@ -31,10 +31,15 @@ proto::TimedPointCloudData ToProto( for (const TimedRangefinderPoint& range : timed_point_cloud_data.ranges) { *proto.add_point_data() = ToProto(range); } + for (const float intensity : timed_point_cloud_data.intensities) { + proto.add_intensities(intensity); + } return proto; } TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { + CHECK(proto.intensities().size() == 0 || + proto.intensities().size() == proto.point_data().size()); TimedPointCloud timed_point_cloud; if (proto.point_data().size() > 0) { timed_point_cloud.reserve(proto.point_data().size()); @@ -50,7 +55,9 @@ TimedPointCloudData FromProto(const proto::TimedPointCloudData& proto) { } return TimedPointCloudData{common::FromUniversal(proto.timestamp()), transform::ToEigen(proto.origin()), - timed_point_cloud}; + timed_point_cloud, + std::vector(proto.intensities().begin(), + proto.intensities().end())}; } } // namespace sensor diff --git a/cartographer/sensor/timed_point_cloud_data.h b/cartographer/sensor/timed_point_cloud_data.h index 4901e3587b..8bdd928027 100644 --- a/cartographer/sensor/timed_point_cloud_data.h +++ b/cartographer/sensor/timed_point_cloud_data.h @@ -28,11 +28,14 @@ struct TimedPointCloudData { common::Time time; Eigen::Vector3f origin; TimedPointCloud ranges; + // 'intensities' has to be same size as 'ranges', or empty. + std::vector intensities; }; struct TimedPointCloudOriginData { struct RangeMeasurement { TimedRangefinderPoint point_time; + float intensity; size_t origin_index; }; common::Time time; diff --git a/cartographer/testing/test_helpers.h b/cartographer/testing/test_helpers.h deleted file mode 100644 index 4a7dbdfd19..0000000000 --- a/cartographer/testing/test_helpers.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright 2018 The Cartographer Authors - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#ifndef CARTOGRAPHER_TESTING_TEST_HELPERS_H_ -#define CARTOGRAPHER_TESTING_TEST_HELPERS_H_ - -#include "gmock/gmock.h" -#include "google/protobuf/text_format.h" -#include "google/protobuf/util/message_differencer.h" - -namespace cartographer { -namespace testing { - -template -ProtoType ParseProto(const std::string& proto_string) { - ProtoType proto; - EXPECT_TRUE( - ::google::protobuf::TextFormat::ParseFromString(proto_string, &proto)); - return proto; -} - -MATCHER_P(EqualsProto, expected_proto_string, "") { - using ConstProtoType = typename std::remove_reference::type; - using ProtoType = typename std::remove_cv::type; - - return google::protobuf::util::MessageDifferencer::Equals( - arg, ParseProto(expected_proto_string)); -} - -::testing::Matcher Near(double expected) { - constexpr double kPrecision = 1e-05; - return ::testing::DoubleNear(expected, kPrecision); -} - -} // namespace testing -} // namespace cartographer - -#endif // CARTOGRAPHER_TESTING_TEST_HELPERS_H_ diff --git a/cartographer/transform/transform_interpolation_buffer.cc b/cartographer/transform/transform_interpolation_buffer.cc index 7d9c3d7d78..ae65721c19 100644 --- a/cartographer/transform/transform_interpolation_buffer.cc +++ b/cartographer/transform/transform_interpolation_buffer.cc @@ -40,8 +40,17 @@ void TransformInterpolationBuffer::Push(const common::Time time, CHECK_GE(time, latest_time()) << "New transform is older than latest."; } timestamped_transforms_.push_back(TimestampedTransform{time, transform}); + RemoveOldTransformsIfNeeded(); } +void TransformInterpolationBuffer::SetSizeLimit( + const size_t buffer_size_limit) { + buffer_size_limit_ = buffer_size_limit; + RemoveOldTransformsIfNeeded(); +} + +void TransformInterpolationBuffer::Clear() { timestamped_transforms_.clear(); } + bool TransformInterpolationBuffer::Has(const common::Time time) const { if (timestamped_transforms_.empty()) { return false; @@ -65,6 +74,12 @@ transform::Rigid3d TransformInterpolationBuffer::Lookup( return Interpolate(*start, *end, time).transform; } +void TransformInterpolationBuffer::RemoveOldTransformsIfNeeded() { + while (timestamped_transforms_.size() > buffer_size_limit_) { + timestamped_transforms_.pop_front(); + } +} + common::Time TransformInterpolationBuffer::earliest_time() const { CHECK(!empty()) << "Empty buffer."; return timestamped_transforms_.front().time; @@ -79,5 +94,13 @@ bool TransformInterpolationBuffer::empty() const { return timestamped_transforms_.empty(); } +size_t TransformInterpolationBuffer::size_limit() const { + return buffer_size_limit_; +} + +size_t TransformInterpolationBuffer::size() const { + return timestamped_transforms_.size(); +} + } // namespace transform } // namespace cartographer diff --git a/cartographer/transform/transform_interpolation_buffer.h b/cartographer/transform/transform_interpolation_buffer.h index 4dc51bb9d4..84a7dcd2b4 100644 --- a/cartographer/transform/transform_interpolation_buffer.h +++ b/cartographer/transform/transform_interpolation_buffer.h @@ -17,7 +17,8 @@ #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ -#include +#include +#include #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory.pb.h" @@ -27,18 +28,28 @@ namespace cartographer { namespace transform { +constexpr size_t kUnlimitedBufferSize = std::numeric_limits::max(); + // A time-ordered buffer of transforms that supports interpolated lookups. +// Unless explicitly set, the buffer size is unlimited. class TransformInterpolationBuffer { public: TransformInterpolationBuffer() = default; explicit TransformInterpolationBuffer( const mapping::proto::Trajectory& trajectory); + // Sets the transform buffer size limit and removes old transforms + // if it is exceeded. + void SetSizeLimit(size_t buffer_size_limit); + // Adds a new transform to the buffer and removes the oldest transform if the // buffer size limit is exceeded. void Push(common::Time time, const transform::Rigid3d& transform); - // Returns true if an interpolated transfrom can be computed at 'time'. + // Clears the transform buffer. + void Clear(); + + // Returns true if an interpolated transform can be computed at 'time'. bool Has(common::Time time) const; // Returns an interpolated transform at 'time'. CHECK()s that a transform at @@ -56,8 +67,17 @@ class TransformInterpolationBuffer { // Returns true if the buffer is empty. bool empty() const; + // Returns the maximum allowed size of the transform buffer. + size_t size_limit() const; + + // Returns the current size of the transform buffer. + size_t size() const; + private: - std::vector timestamped_transforms_; + void RemoveOldTransformsIfNeeded(); + + std::deque timestamped_transforms_; + size_t buffer_size_limit_ = kUnlimitedBufferSize; }; } // namespace transform diff --git a/cartographer/transform/transform_interpolation_buffer_test.cc b/cartographer/transform/transform_interpolation_buffer_test.cc index 193f95e419..b78d5692ec 100644 --- a/cartographer/transform/transform_interpolation_buffer_test.cc +++ b/cartographer/transform/transform_interpolation_buffer_test.cc @@ -70,6 +70,22 @@ TEST(TransformInterpolationBufferTest, testLookupSingleTransform) { EXPECT_THAT(interpolated, IsNearly(transform::Rigid3d::Identity(), 1e-6)); } +TEST(TransformInterpolationBufferTest, testSetSizeLimit) { + TransformInterpolationBuffer buffer; + EXPECT_EQ(buffer.size_limit(), kUnlimitedBufferSize); + buffer.Push(common::FromUniversal(0), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(1), transform::Rigid3d::Identity()); + buffer.Push(common::FromUniversal(2), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 3); + buffer.SetSizeLimit(2); + EXPECT_EQ(buffer.size_limit(), 2); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(0))); + buffer.Push(common::FromUniversal(3), transform::Rigid3d::Identity()); + EXPECT_EQ(buffer.size(), 2); + EXPECT_FALSE(buffer.Has(common::FromUniversal(1))); +} + } // namespace } // namespace transform } // namespace cartographer diff --git a/cmake/functions.cmake b/cmake/functions.cmake index a9336aa535..8cfc05ff97 100644 --- a/cmake/functions.cmake +++ b/cmake/functions.cmake @@ -22,7 +22,7 @@ macro(_parse_arguments ARGS) "${OPTIONS}" "${ONE_VALUE_ARG}" "${MULTI_VALUE_ARGS}" ${ARGS}) endmacro(_parse_arguments) -macro(_common_compile_stuff VISIBILITY) +macro(_common_compile_stuff) set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}") set_target_properties(${NAME} PROPERTIES @@ -34,7 +34,7 @@ endmacro(_common_compile_stuff) function(google_test NAME ARG_SRC) add_executable(${NAME} ${ARG_SRC}) - _common_compile_stuff("PRIVATE") + _common_compile_stuff() # Make sure that gmock always includes the correct gtest/gtest.h. target_include_directories("${NAME}" SYSTEM PRIVATE @@ -49,7 +49,7 @@ function(google_binary NAME) add_executable(${NAME} ${ARG_SRCS}) - _common_compile_stuff("PRIVATE") + _common_compile_stuff() install(TARGETS "${NAME}" RUNTIME DESTINATION bin) endfunction() @@ -76,7 +76,7 @@ macro(google_initialize_cartographer_project) if(WIN32) # TODO turn on equivalent warnings on Windows else() - set(GOOG_CXX_FLAGS "-pthread -std=c++11 -fPIC ${GOOG_CXX_FLAGS}") + set(GOOG_CXX_FLAGS "-pthread -fPIC ${GOOG_CXX_FLAGS}") google_add_flag(GOOG_CXX_FLAGS "-Wall") google_add_flag(GOOG_CXX_FLAGS "-Wpedantic") diff --git a/cmake/modules/FindAbseil.cmake b/cmake/modules/FindAbseil.cmake deleted file mode 100644 index 80d35c5e6d..0000000000 --- a/cmake/modules/FindAbseil.cmake +++ /dev/null @@ -1,96 +0,0 @@ -# Copyright 2018 The Cartographer Authors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -cmake_minimum_required(VERSION 3.2) - -if(NOT TARGET standalone_absl) - set(prefix ${CMAKE_STATIC_LIBRARY_PREFIX}) - set(suffix ${CMAKE_STATIC_LIBRARY_SUFFIX}) - include(${CMAKE_ROOT}/Modules/ExternalProject.cmake) - set(ABSEIL_PROJECT_NAME abseil) - set(ABSEIL_PROJECT_SRC_DIR - ${CMAKE_CURRENT_BINARY_DIR}/${ABSEIL_PROJECT_NAME}/src/${ABSEIL_PROJECT_NAME}) - set(ABSEIL_PROJECT_BUILD_DIR - ${CMAKE_CURRENT_BINARY_DIR}/${ABSEIL_PROJECT_NAME}/src/${ABSEIL_PROJECT_NAME}-build) - set(ABSEIL_INCLUDE_DIRS ${ABSEIL_PROJECT_SRC_DIR}) - set(ABSEIL_LIBRARY_PATH - "${ABSEIL_PROJECT_BUILD_DIR}/absl/synchronization/${prefix}absl_synchronization${suffix}") - set(ABSEIL_DEPENDENT_LIBRARIES - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_symbolize${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/time/${prefix}absl_time${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}str_format_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}str_format_extension_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}absl_str_format${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/strings/${prefix}absl_strings${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/hash/${prefix}absl_hash${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/algorithm/${prefix}absl_algorithm${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_base${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_dynamic_annotations${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_malloc_internal${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_spinlock_wait${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/base/${prefix}absl_internal_throw_delegate${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/container/${prefix}absl_container${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/container/${prefix}test_instance_tracker_lib${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_debugging${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_examine_stack${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_failure_signal_handler${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_leak_check${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_stack_consumption${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/debugging/${prefix}absl_stacktrace${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/memory/${prefix}absl_memory${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/meta/${prefix}absl_meta${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_int128${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/numeric/${prefix}absl_numeric${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/synchronization/${prefix}absl_synchronization${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_any${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_any_cast${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_bad_optional_access${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_optional${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_span${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/types/${prefix}absl_variant${suffix}" - "${ABSEIL_PROJECT_BUILD_DIR}/absl/utility/${prefix}absl_utility${suffix}" - ) - ExternalProject_Add(${ABSEIL_PROJECT_NAME} - PREFIX ${ABSEIL_PROJECT_NAME} - GIT_REPOSITORY https://github.com/abseil/abseil-cpp.git - GIT_TAG 7b46e1d31a6b08b1c6da2a13e7b151a20446fa07 - INSTALL_COMMAND "" - BUILD_COMMAND ${CMAKE_COMMAND} --build "${ABSEIL_PROJECT_BUILD_DIR}" - CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=ON;-DBUILD_TESTING:BOOL=OFF;-DCMAKE_BUILD_TYPE:STRING=Release" - BUILD_BYPRODUCTS "${ABSEIL_LIBRARY_PATH};${ABSEIL_DEPENDENT_LIBRARIES}" - ) - add_library(standalone_absl STATIC IMPORTED GLOBAL) - set_target_properties(standalone_absl - PROPERTIES INTERFACE_INCLUDE_DIRECTORIES - ${ABSEIL_INCLUDE_DIRS} - ) - set_target_properties(standalone_absl - PROPERTIES IMPORTED_LOCATION - ${ABSEIL_LIBRARY_PATH} - INTERFACE_LINK_LIBRARIES - "${ABSEIL_DEPENDENT_LIBRARIES}" - ) - if(MSVC) - # /wd4005 macro-redefinition - # /wd4068 unknown pragma - # /wd4244 conversion from 'type1' to 'type2' - # /wd4267 conversion from 'size_t' to 'type2' - # /wd4800 force value to bool 'true' or 'false' (performance warning) - target_compile_options(standalone_absl INTERFACE /wd4005 /wd4068 /wd4244 /wd4267 /wd4800) - target_compile_definitions(standalone_absl INTERFACE -DNOMINMAX -DWIN32_LEAN_AND_MEAN=1 -D_CRT_SECURE_NO_WARNINGS) - endif() - add_dependencies(standalone_absl ${ABSEIL_PROJECT_NAME}) - unset(prefix) - unset(suffix) -endif() diff --git a/cmake/modules/FindGMock.cmake b/cmake/modules/FindGMock.cmake index c663e98368..b29c0fe364 100644 --- a/cmake/modules/FindGMock.cmake +++ b/cmake/modules/FindGMock.cmake @@ -30,6 +30,25 @@ if(NOT GMock_FOUND) PATHS /usr ) + if(GMOCK_LIBRARIES) + find_library(GMOCK_LIBRARY + NAMES gmock + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + find_library(GTEST_LIBRARY + NAMES gtest + HINTS + ENV GMOCK_DIR + PATH_SUFFIXES lib + PATHS + /usr + ) + list(APPEND GMOCK_LIBRARIES ${GMOCK_LIBRARY} ${GTEST_LIBRARY}) + endif() # Find system-wide gtest header. find_path(GTEST_INCLUDE_DIRS gtest/gtest.h @@ -57,9 +76,8 @@ if(NOT GMock_FOUND) add_subdirectory(${GMOCK_SRC_DIR} "${CMAKE_CURRENT_BINARY_DIR}/gmock" EXCLUDE_FROM_ALL) endif() - # The next line is needed for Ubuntu Trusty. - set(GMOCK_INCLUDE_DIRS "${GMOCK_SRC_DIR}/gtest/include") set(GMOCK_LIBRARIES gmock_main) + set(GMOCK_INCLUDE_DIRS ${GMOCK_SRC_DIR}/include) endif() endif() diff --git a/configuration_files/pose_graph.lua b/configuration_files/pose_graph.lua index 797e407d66..a962641695 100644 --- a/configuration_files/pose_graph.lua +++ b/configuration_files/pose_graph.lua @@ -63,14 +63,17 @@ POSE_GRAPH = { matcher_rotation_weight = 1.6e3, optimization_problem = { huber_scale = 1e1, - acceleration_weight = 1e3, - rotation_weight = 3e5, + acceleration_weight = 1.1e2, + rotation_weight = 1.6e4, local_slam_pose_translation_weight = 1e5, local_slam_pose_rotation_weight = 1e5, odometry_translation_weight = 1e5, odometry_rotation_weight = 1e5, fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_rotation_weight = 1e2, + fixed_frame_pose_use_tolerant_loss = false, + fixed_frame_pose_tolerant_loss_param_a = 1, + fixed_frame_pose_tolerant_loss_param_b = 1, log_solver_summary = false, use_online_imu_extrinsics_in_3d = true, fix_z_in_3d = false, diff --git a/configuration_files/trajectory_builder_2d.lua b/configuration_files/trajectory_builder_2d.lua index 483f47f0d9..373b099908 100644 --- a/configuration_files/trajectory_builder_2d.lua +++ b/configuration_files/trajectory_builder_2d.lua @@ -59,7 +59,30 @@ TRAJECTORY_BUILDER_2D = { max_angle_radians = math.rad(1.), }, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. imu_gravity_time_constant = 10., + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, submaps = { num_range_data = 90, diff --git a/configuration_files/trajectory_builder_3d.lua b/configuration_files/trajectory_builder_3d.lua index 49567f4cf3..b9208d1b27 100644 --- a/configuration_files/trajectory_builder_3d.lua +++ b/configuration_files/trajectory_builder_3d.lua @@ -13,6 +13,7 @@ -- limitations under the License. MAX_3D_RANGE = 60. +INTENSITY_THRESHOLD = 40 TRAJECTORY_BUILDER_3D = { min_range = 1., @@ -43,6 +44,11 @@ TRAJECTORY_BUILDER_3D = { ceres_scan_matcher = { occupied_space_weight_0 = 1., occupied_space_weight_1 = 6., + intensity_cost_function_options_0 = { + weight = 0.5, + huber_scale = 0.3, + intensity_threshold = INTENSITY_THRESHOLD, + }, translation_weight = 5., rotation_weight = 4e2, only_optimize_yaw = false, @@ -59,9 +65,34 @@ TRAJECTORY_BUILDER_3D = { max_angle_radians = 0.004, }, - imu_gravity_time_constant = 10., rotational_histogram_size = 120, + -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS. + imu_gravity_time_constant = 10., + pose_extrapolator = { + use_imu_based = false, + constant_velocity = { + imu_gravity_time_constant = 10., + pose_queue_duration = 0.001, + }, + -- TODO(wohe): Tune these parameters on the example datasets. + imu_based = { + pose_queue_duration = 5., + gravity_constant = 9.806, + pose_translation_weight = 1., + pose_rotation_weight = 1., + imu_acceleration_weight = 1., + imu_rotation_weight = 1., + odometry_translation_weight = 1., + odometry_rotation_weight = 1., + solver_options = { + use_nonmonotonic_steps = false; + max_num_iterations = 10; + num_threads = 1; + }, + }, + }, + submaps = { high_resolution = 0.10, high_resolution_max_range = 20., @@ -71,6 +102,12 @@ TRAJECTORY_BUILDER_3D = { hit_probability = 0.55, miss_probability = 0.49, num_free_space_voxels = 2, + intensity_threshold = INTENSITY_THRESHOLD, }, }, + + -- When setting use_intensities to true, the intensity_cost_function_options_0 + -- parameter in ceres_scan_matcher has to be set up as well or otherwise + -- CeresScanMatcher will CHECK-fail. + use_intensities = false, } diff --git a/docs/source/index.rst b/docs/source/index.rst index 0ebfe5ccdd..6614f6fe0b 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -30,7 +30,7 @@ Cartographer and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor configurations. -.. _Cartographer: https://github.com/googlecartographer/cartographer +.. _Cartographer: https://github.com/cartographer-project/cartographer .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping Technical Overview @@ -38,7 +38,7 @@ Technical Overview * High level system overview of Cartographer .. image:: high_level_system_overview.png - :target: https://github.com/googlecartographer/cartographer/blob/master/docs/source/high_level_system_overview.png + :target: https://github.com/cartographer-project/cartographer/blob/master/docs/source/high_level_system_overview.png .. To make modifications, edit the original Google Sketch and export a png. .. https://docs.google.com/drawings/d/1kCJ_dEbSvV83THCUfMikCPw7xFrTkrvRw5r6Ji8C90c/edit?usp=sharing @@ -56,7 +56,7 @@ ROS integration is provided by the `Cartographer ROS repository`_. You will find complete documentation for using Cartographer with ROS at the `Cartographer ROS Read the Docs site`_. -.. _Cartographer ROS repository: https://github.com/googlecartographer/cartographer_ros +.. _Cartographer ROS repository: https://github.com/cartographer-project/cartographer_ros .. _Cartographer ROS Read the Docs site: https://google-cartographer-ros.readthedocs.io Getting started without ROS @@ -65,13 +65,13 @@ Getting started without ROS Please see our ROS integration as a starting point for integrating your system with the standalone library. Currently, it is the best available reference. -On Ubuntu 14.04 (Trusty): +On Ubuntu 18.04 (Bionic): .. literalinclude:: ../../scripts/install_debs_cmake.sh :language: bash :lines: 20- -.. literalinclude:: ../../scripts/install_ceres.sh +.. literalinclude:: ../../scripts/install_abseil.sh :language: bash :lines: 20- @@ -93,8 +93,8 @@ on systems that meet the following requirements: * 64-bit, modern CPU (e.g. 3rd generation i7) * 16 GB RAM -* Ubuntu 14.04 (Trusty) and 16.04 (Xenial) -* gcc version 4.8.4 and 5.4.0 +* Ubuntu 18.04 (Bionic), 20.04 (Focal), 22.04 (Jammy) +* gcc version 7.5.0, 8.3.0, 9.3.0, 10.2.1, 11.2.0 Known Issues ------------ diff --git a/docs/source/pbstream_migration.rst b/docs/source/pbstream_migration.rst index 14b9cee5c8..20ce54350e 100644 --- a/docs/source/pbstream_migration.rst +++ b/docs/source/pbstream_migration.rst @@ -16,22 +16,30 @@ Migration tool for pbstream files ================================= -With the update of the pbstream serialization format as discussed in -`RFC-0021`_, previously serialized pbstream files are not loadable in -Cartographer 1.0 anymore. - -In order to enable users to reuse previously generated pbstream files, we -provide a migration tool which converts pbstreams from Cartographer 0.3 to the -new serialization format used in Cartographer 1.0. +The pbstream serialization format for 3D has changed to include additional +data (histograms) in each submap. Code to load old data by migrating +on-the-fly will be removed soon. Once this happened, users who wish to +migrate old pbstream files can use a migration tool. The tool is shipped as part of Cartographer's pbstream tool (`source`_) and once built can be invoked as follows::: cartographer_pbstream migrate old.pbstream new.pbstream -The tool assumes that the first pbstream provided as commandline argument, -follows the serialization format of Cartographer 0.3. The resulting -1.0 pbstream will be saved to the second commandline argument location. +The tool assumes 3D data in the old submap format as input and converts it +to the currently used format version. + +Migrating pre-1.0 pbstream files +================================ + +With the update of the pbstream serialization format as discussed in +`RFC-0021`_, previously serialized pbstream files are not loadable in +Cartographer 1.0 anymore. + +In order to enable users to reuse previously generated pbstream files, +migration using an older version of the migration tool is necessary. +The current tool does not support this migration anymore. Please use +the version at Git SHA 6c889490e245cc5d9da15023249c6fc7119def3f. -.. _RFC-0021: https://github.com/googlecartographer/rfcs/blob/master/text/0021-serialization-format.md -.. _source: https://github.com/googlecartographer/cartographer/blob/master/cartographer/io/pbstream_main.cc +.. _RFC-0021: https://github.com/cartographer-project/rfcs/blob/master/text/0021-serialization-format.md +.. _source: https://github.com/cartographer-project/cartographer/blob/master/cartographer/io/pbstream_main.cc diff --git a/package.xml b/package.xml index e50c112401..cabc5576b9 100644 --- a/package.xml +++ b/package.xml @@ -15,9 +15,9 @@ limitations under the License. --> - + cartographer - 1.0.0 + 2.0.0 Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor @@ -28,22 +28,24 @@ Apache 2.0 - https://github.com/googlecartographer/cartographer + https://github.com/cartographer-project/cartographer The Cartographer Authors - catkin + cmake git google-mock - python-sphinx + gtest + python3-sphinx - boost - ceres-solver + libboost-iostreams-dev eigen + libabsl-dev libcairo2-dev + libceres-dev libgflags-dev libgoogle-glog-dev lua5.2-dev diff --git a/scripts/install_ceres.sh b/scripts/install_abseil.sh similarity index 64% rename from scripts/install_ceres.sh rename to scripts/install_abseil.sh index 56999d09c4..ca5740b7e9 100755 --- a/scripts/install_ceres.sh +++ b/scripts/install_abseil.sh @@ -1,6 +1,6 @@ #!/bin/sh -# Copyright 2016 The Cartographer Authors +# Copyright 2019 The Cartographer Authors # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -17,15 +17,17 @@ set -o errexit set -o verbose -VERSION="1.13.0" - -# Build and install Ceres. -git clone https://ceres-solver.googlesource.com/ceres-solver -cd ceres-solver -git checkout tags/${VERSION} +git clone https://github.com/abseil/abseil-cpp.git +cd abseil-cpp +git checkout 215105818dfde3174fe799600bb0f3cae233d0bf # 20211102.0 mkdir build cd build -cmake .. -G Ninja -DCXX11=ON +cmake -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON \ + -DCMAKE_INSTALL_PREFIX=/usr/local/stow/absl \ + .. ninja -CTEST_OUTPUT_ON_FAILURE=1 ninja test sudo ninja install +cd /usr/local/stow +sudo stow absl diff --git a/scripts/install_async_grpc.sh b/scripts/install_async_grpc.sh index 95ac84766a..73c8bbd8bc 100755 --- a/scripts/install_async_grpc.sh +++ b/scripts/install_async_grpc.sh @@ -17,7 +17,7 @@ set -o errexit set -o verbose -git clone https://github.com/googlecartographer/async_grpc +git clone https://github.com/cartographer-project/async_grpc cd async_grpc git checkout 771af45374af7f7bfc3b622ed7efbe29a4aba403 mkdir build diff --git a/scripts/install_debs_cmake.sh b/scripts/install_debs_cmake.sh index bfa67a7277..18752ec69d 100755 --- a/scripts/install_debs_cmake.sh +++ b/scripts/install_debs_cmake.sh @@ -19,33 +19,33 @@ set -o verbose # Install the required libraries that are available as debs. sudo apt-get update - -# Install CMake 3.2 for Ubuntu Trusty and Debian Jessie. -sudo apt-get install lsb-release -y -if [[ "$(lsb_release -sc)" = "trusty" ]] -then - sudo apt-get install cmake3 -y -elif [[ "$(lsb_release -sc)" = "jessie" ]] -then - sudo sh -c "echo 'deb http://ftp.debian.org/debian jessie-backports main' >> /etc/apt/sources.list" - sudo apt-get update - sudo apt-get -t jessie-backports install cmake -y -else - sudo apt-get install cmake -y -fi - sudo apt-get install -y \ clang \ + cmake \ g++ \ git \ google-mock \ libboost-all-dev \ libcairo2-dev \ + libceres-dev \ libcurl4-openssl-dev \ libeigen3-dev \ libgflags-dev \ libgoogle-glog-dev \ liblua5.2-dev \ libsuitesparse-dev \ + lsb-release \ ninja-build \ - python-sphinx + python3-sphinx \ + stow + +# Install Protocol Buffers and Abseil if available. +# No need to build it ourselves. +case "$(lsb_release -sc)" in + jammy|bullseye) + sudo apt-get install -y libgmock-dev protobuf-compiler libabsl-dev ;; + focal|buster) + sudo apt-get install -y libgmock-dev protobuf-compiler ;; + bionic) + ;; +esac diff --git a/scripts/load_docker_cache.sh b/scripts/load_docker_cache.sh deleted file mode 100755 index 4c37247b2b..0000000000 --- a/scripts/load_docker_cache.sh +++ /dev/null @@ -1,26 +0,0 @@ -#!/bin/bash - -# Copyright 2016 The Cartographer Authors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html - -set -o errexit -set -o verbose -set -o pipefail - -if [ -f ${DOCKER_CACHE_FILE} ]; then - gunzip -c ${DOCKER_CACHE_FILE} | docker load; -fi diff --git a/scripts/save_docker_cache.sh b/scripts/save_docker_cache.sh deleted file mode 100755 index 7b2000e576..0000000000 --- a/scripts/save_docker_cache.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/bash - -# Copyright 2016 The Cartographer Authors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# Cache intermediate Docker layers. For a description of how this works, see: -# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html - -set -o errexit -set -o verbose -set -o pipefail - -if [[ ${TRAVIS_BRANCH} == "master" ]] && - [[ ${TRAVIS_PULL_REQUEST} == "false" ]]; then - mkdir -p $(dirname ${DOCKER_CACHE_FILE}) - IMAGE_NAMES=$(docker history -q cartographer:${LSB_RELEASE} | grep -v '') - docker save ${IMAGE_NAMES} | gzip > ${DOCKER_CACHE_FILE}.new - mv ${DOCKER_CACHE_FILE}.new ${DOCKER_CACHE_FILE} -fi diff --git a/scripts/update_configuration_doc.py b/scripts/update_configuration_doc.py index 920c466ec2..846859495c 100755 --- a/scripts/update_configuration_doc.py +++ b/scripts/update_configuration_doc.py @@ -179,7 +179,7 @@ def GenerateDocumentation(output_file, root): assert message.name not in output_dict output_dict[message.name] = content if message.preceding_comments: - content.extend(preceding_comments) + content.extend(message.preceding_comments) content.append('') for option_type, option_name, option_comments in message.options: # TODO(whess): For now we exclude InitialTrajectoryPose from the