├── .travis.yml ├── AUTHORS ├── CMakeLists.txt ├── CONTRIBUTING.md ├── Dockerfile ├── LICENSE ├── README.rst ├── cartographer-config.cmake.in ├── cartographer ├── CMakeLists.txt ├── common │ ├── CMakeLists.txt │ ├── blocking_queue.h │ ├── blocking_queue_test.cc │ ├── ceres_solver_options.cc │ ├── ceres_solver_options.h │ ├── config.h.cmake │ ├── configuration_file_resolver.cc │ ├── configuration_file_resolver.h │ ├── fixed_ratio_sampler.cc │ ├── fixed_ratio_sampler.h │ ├── fixed_ratio_sampler_test.cc │ ├── histogram.cc │ ├── histogram.h │ ├── lua.h │ ├── lua_parameter_dictionary.cc │ ├── lua_parameter_dictionary.h │ ├── lua_parameter_dictionary_test.cc │ ├── lua_parameter_dictionary_test_helpers.h │ ├── make_unique.h │ ├── math.h │ ├── math_test.cc │ ├── mutex.h │ ├── ordered_multi_queue.h │ ├── ordered_multi_queue_test.cc │ ├── port.h │ ├── proto │ │ ├── CMakeLists.txt │ │ └── ceres_solver_options.proto │ ├── rate_timer.h │ ├── rate_timer_test.cc │ ├── thread_pool.cc │ ├── thread_pool.h │ ├── time.cc │ └── time.h ├── kalman_filter │ ├── CMakeLists.txt │ ├── gaussian_distribution.h │ ├── gaussian_distribution_test.cc │ ├── odometry_state_tracker.cc │ ├── odometry_state_tracker.h │ ├── pose_tracker.cc │ ├── pose_tracker.h │ ├── pose_tracker_test.cc │ ├── proto │ │ ├── CMakeLists.txt │ │ └── pose_tracker_options.proto │ ├── unscented_kalman_filter.h │ └── unscented_kalman_filter_test.cc ├── mapping │ ├── CMakeLists.txt │ ├── global_trajectory_builder_interface.cc │ ├── global_trajectory_builder_interface.h │ ├── map_builder.cc │ ├── map_builder.h │ ├── probability_values.cc │ ├── probability_values.h │ ├── probability_values_test.cc │ ├── proto │ │ ├── CMakeLists.txt │ │ ├── map_builder_options.proto │ │ ├── scan_matching_progress.proto │ │ ├── sparse_pose_graph_options.proto │ │ ├── submaps.proto │ │ └── trajectory_connectivity.proto │ ├── sensor_collator.h │ ├── sensor_collator_test.cc │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph │ │ ├── CMakeLists.txt │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem_options.cc │ │ ├── optimization_problem_options.h │ │ └── proto │ │ │ ├── CMakeLists.txt │ │ │ ├── constraint_builder_options.proto │ │ │ └── optimization_problem_options.proto │ ├── sparse_pose_graph_test.cc │ ├── submaps.cc │ ├── submaps.h │ ├── submaps_test.cc │ ├── trajectory_connectivity.cc │ ├── trajectory_connectivity.h │ ├── trajectory_connectivity_test.cc │ └── trajectory_node.h ├── mapping_2d │ ├── CMakeLists.txt │ ├── global_trajectory_builder.cc │ ├── global_trajectory_builder.h │ ├── laser_fan_inserter.cc │ ├── laser_fan_inserter.h │ ├── laser_fan_inserter_test.cc │ ├── local_trajectory_builder.cc │ ├── local_trajectory_builder.h │ ├── map_limits.h │ ├── map_limits_test.cc │ ├── probability_grid.h │ ├── probability_grid_test.cc │ ├── proto │ │ ├── CMakeLists.txt │ │ ├── laser_fan_inserter_options.proto │ │ ├── local_trajectory_builder_options.proto │ │ └── submaps_options.proto │ ├── ray_casting.cc │ ├── ray_casting.h │ ├── scan_matching │ │ ├── CMakeLists.txt │ │ ├── ceres_scan_matcher.cc │ │ ├── ceres_scan_matcher.h │ │ ├── ceres_scan_matcher_test.cc │ │ ├── correlative_scan_matcher.cc │ │ ├── correlative_scan_matcher.h │ │ ├── correlative_scan_matcher_test.cc │ │ ├── fast_correlative_scan_matcher.cc │ │ ├── fast_correlative_scan_matcher.h │ │ ├── fast_correlative_scan_matcher_test.cc │ │ ├── fast_global_localizer.cc │ │ ├── fast_global_localizer.h │ │ ├── occupied_space_cost_functor.h │ │ ├── proto │ │ │ ├── CMakeLists.txt │ │ │ ├── ceres_scan_matcher_options.proto │ │ │ ├── fast_correlative_scan_matcher_options.proto │ │ │ └── real_time_correlative_scan_matcher_options.proto │ │ ├── real_time_correlative_scan_matcher.cc │ │ ├── real_time_correlative_scan_matcher.h │ │ ├── real_time_correlative_scan_matcher_test.cc │ │ ├── rotation_delta_cost_functor.h │ │ └── translation_delta_cost_functor.h │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph │ │ ├── CMakeLists.txt │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ └── optimization_problem.h │ ├── sparse_pose_graph_test.cc │ ├── submaps.cc │ ├── submaps.h │ ├── submaps_test.cc │ ├── xy_index.h │ └── xy_index_test.cc ├── mapping_3d │ ├── CMakeLists.txt │ ├── acceleration_cost_function.h │ ├── ceres_pose.cc │ ├── ceres_pose.h │ ├── global_trajectory_builder.cc │ ├── global_trajectory_builder.h │ ├── hybrid_grid.h │ ├── hybrid_grid_test.cc │ ├── imu_integration.cc │ ├── imu_integration.h │ ├── kalman_local_trajectory_builder.cc │ ├── kalman_local_trajectory_builder.h │ ├── kalman_local_trajectory_builder_options.cc │ ├── kalman_local_trajectory_builder_options.h │ ├── kalman_local_trajectory_builder_test.cc │ ├── laser_fan_inserter.cc │ ├── laser_fan_inserter.h │ ├── laser_fan_inserter_test.cc │ ├── local_trajectory_builder.cc │ ├── local_trajectory_builder.h │ ├── local_trajectory_builder_interface.h │ ├── local_trajectory_builder_options.cc │ ├── local_trajectory_builder_options.h │ ├── motion_filter.cc │ ├── motion_filter.h │ ├── motion_filter_test.cc │ ├── optimizing_local_trajectory_builder.cc │ ├── optimizing_local_trajectory_builder.h │ ├── optimizing_local_trajectory_builder_options.cc │ ├── optimizing_local_trajectory_builder_options.h │ ├── proto │ │ ├── CMakeLists.txt │ │ ├── kalman_local_trajectory_builder_options.proto │ │ ├── laser_fan_inserter_options.proto │ │ ├── local_trajectory_builder_options.proto │ │ ├── motion_filter_options.proto │ │ ├── optimizing_local_trajectory_builder_options.proto │ │ └── submaps_options.proto │ ├── rotation_cost_function.h │ ├── scan_matching │ │ ├── CMakeLists.txt │ │ ├── ceres_scan_matcher.cc │ │ ├── ceres_scan_matcher.h │ │ ├── ceres_scan_matcher_test.cc │ │ ├── fast_correlative_scan_matcher.cc │ │ ├── fast_correlative_scan_matcher.h │ │ ├── fast_correlative_scan_matcher_test.cc │ │ ├── interpolated_grid.h │ │ ├── interpolated_grid_test.cc │ │ ├── occupied_space_cost_functor.h │ │ ├── precomputation_grid.cc │ │ ├── precomputation_grid.h │ │ ├── precomputation_grid_test.cc │ │ ├── proto │ │ │ ├── CMakeLists.txt │ │ │ ├── ceres_scan_matcher_options.proto │ │ │ └── fast_correlative_scan_matcher_options.proto │ │ ├── real_time_correlative_scan_matcher.cc │ │ ├── real_time_correlative_scan_matcher.h │ │ ├── real_time_correlative_scan_matcher_test.cc │ │ ├── rotation_delta_cost_functor.h │ │ ├── rotational_scan_matcher.cc │ │ ├── rotational_scan_matcher.h │ │ └── translation_delta_cost_functor.h │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph │ │ ├── CMakeLists.txt │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ ├── optimization_problem.h │ │ └── optimization_problem_test.cc │ ├── submaps.cc │ ├── submaps.h │ └── translation_cost_function.h ├── proto │ ├── CMakeLists.txt │ └── trajectory.proto ├── sensor │ ├── CMakeLists.txt │ ├── compressed_point_cloud.cc │ ├── compressed_point_cloud.h │ ├── compressed_point_cloud_test.cc │ ├── configuration.cc │ ├── configuration.h │ ├── laser.cc │ ├── laser.h │ ├── laser_test.cc │ ├── point_cloud.cc │ ├── point_cloud.h │ ├── point_cloud_test.cc │ ├── proto │ │ ├── CMakeLists.txt │ │ ├── adaptive_voxel_filter_options.proto │ │ ├── configuration.proto │ │ └── sensor.proto │ ├── sensor_packet_period_histogram_builder.cc │ ├── sensor_packet_period_histogram_builder.h │ ├── voxel_filter.cc │ ├── voxel_filter.h │ └── voxel_filter_test.cc └── transform │ ├── CMakeLists.txt │ ├── proto │ ├── CMakeLists.txt │ └── transform.proto │ ├── rigid_transform.cc │ ├── rigid_transform.h │ ├── rigid_transform_test_helpers.h │ ├── transform.cc │ ├── transform.h │ ├── transform_interpolation_buffer.cc │ ├── transform_interpolation_buffer.h │ ├── transform_interpolation_buffer_test.cc │ └── transform_test.cc ├── cmake ├── functions.cmake └── modules │ ├── FindEigen3.cmake │ ├── FindLuaGoogle.cmake │ └── FindSphinx.cmake ├── configuration_files ├── map_builder.lua ├── sparse_pose_graph.lua ├── trajectory_builder_2d.lua └── trajectory_builder_3d.lua ├── docs ├── CMakeLists.txt └── source │ ├── conf.py │ └── index.rst ├── package.xml └── scripts ├── install_cartographer.sh ├── install_ceres.sh ├── install_debs.sh └── update_cmakelists.py /.travis.yml: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | sudo: required 16 | services: docker 17 | install: true 18 | script: docker build ${TRAVIS_BUILD_DIR} -t cartographer 19 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | # This is the list of Cartographer authors for copyright purposes. 2 | # 3 | # This does not necessarily list everyone who has contributed code, since in 4 | # some cases, their employer may be the copyright holder. To see the full list 5 | # of contributors, see the revision history in source control. 6 | Google Inc. 7 | and other contributors 8 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Want to contribute? Great! First, read this page (including the small print at the end). 2 | 3 | ### Before you contribute 4 | Before we can use your code, you must sign the 5 | [Google Individual Contributor License Agreement] 6 | (https://cla.developers.google.com/about/google-individual) 7 | (CLA), which you can do online. The CLA is necessary mainly because you own the 8 | copyright to your changes, even after your contribution becomes part of our 9 | codebase, so we need your permission to use and distribute your code. We also 10 | need to be sure of various other things—for instance that you'll tell us if you 11 | know that your code infringes on other people's patents. You don't have to sign 12 | the CLA until after you've submitted your code for review and a member has 13 | approved it, but you must do it before we can put your code into our codebase. 14 | Before you start working on a larger contribution, you should get in touch with 15 | us first through the issue tracker with your idea so that we can help out and 16 | possibly guide you. Coordinating up front makes it much easier to avoid 17 | frustration later on. 18 | 19 | ### Code reviews 20 | All submissions, including submissions by project members, require review. We 21 | use Github pull requests for this purpose. 22 | 23 | ### The small print 24 | Contributions made by corporations are covered by a different agreement than 25 | the one above, the 26 | [Software Grant and Corporate Contributor License Agreement] 27 | (https://cla.developers.google.com/about/google-corporate). 28 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | FROM ubuntu:trusty 16 | COPY . cartographer 17 | RUN cartographer/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/* 18 | RUN cartographer/scripts/install_ceres.sh && rm -rf ceres-solver 19 | RUN cartographer/scripts/install_cartographer.sh && rm -rf cartographer 20 | -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | .. Copyright 2016 The Cartographer Authors 2 | 3 | .. Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | .. http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | .. Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | 15 | ============ 16 | Cartographer 17 | ============ 18 | 19 | |build| |docs| 20 | 21 | Purpose 22 | ======= 23 | 24 | `Cartographer`_ is a system that provides real-time simultaneous localization 25 | and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor 26 | configurations. 27 | 28 | |video| 29 | 30 | .. _Cartographer: https://github.com/googlecartographer/cartographer 31 | .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping 32 | 33 | Documentation 34 | ============= 35 | 36 | You will find complete documentation for using Cartographer at `our Read the 37 | Docs site`_. 38 | 39 | .. _our Read the Docs site: https://google-cartographer.readthedocs.io 40 | 41 | Contributing 42 | ============ 43 | 44 | You can find information about contributing to Cartographer at `our Contribution 45 | page`_. 46 | 47 | .. _our Contribution page: https://github.com/googlecartographer/cartographer/blob/master/CONTRIBUTING.md 48 | 49 | .. |build| image:: https://travis-ci.org/googlecartographer/cartographer.svg?branch=master 50 | :alt: Build Status 51 | :scale: 100% 52 | :target: https://travis-ci.org/googlecartographer/cartographer 53 | 54 | .. |docs| image:: https://readthedocs.org/projects/google-cartographer/badge/?version=latest 55 | :alt: Documentation Status 56 | :scale: 100% 57 | :target: https://google-cartographer.readthedocs.io/en/latest/?badge=latest 58 | 59 | .. |video| image:: https://j.gifs.com/wp3BJM.gif 60 | :alt: Cartographer 3D SLAM Demo 61 | :scale: 100% 62 | :target: https://youtu.be/DM0dpHLhtX0 63 | -------------------------------------------------------------------------------- /cartographer-config.cmake.in: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # Usage from an external project: 16 | # In your CMakeLists.txt, add these lines: 17 | # 18 | # find_package(Cartographer REQUIRED ) 19 | # include_directories(${CARTOGRAPHER_INCLUDE_DIRS}) 20 | # target_link_libraries(MY_TARGET_NAME ${CARTOGRAPHER_LIBRARIES}) 21 | # 22 | # 23 | # This file will define the following variables: 24 | # - CARTOGRAPHER_LIBRARIES 25 | # - CARTOGRAPHER_LIBRARY_DIRS 26 | # - CARTOGRAPHER_INCLUDE_DIRS 27 | 28 | @PACKAGE_INIT@ 29 | 30 | set(CARTOGRAPHER_INCLUDE_DIRS "@PACKAGE_CARTOGRAPHER_INCLUDE_DIRS@") 31 | set(CARTOGRAPHER_LIBRARY_DIRS "@PACKAGE_CARTOGRAPHER_LIBRARY_DIRS@") 32 | 33 | set(CARTOGRAPHER_LIBRARIES 34 | "@CARTOGRAPHER_LIBRARIES@" 35 | ) 36 | 37 | set(CERES_DIR_HINTS @Ceres_DIR@) 38 | 39 | if (cartographer_FIND_QUIETLY) 40 | find_package(Ceres QUIET HINTS ${CERES_DIR_HINTS}) 41 | elseif (cartographer_FIND_REQUIRED) 42 | find_package(Ceres REQUIRED HINTS ${CERES_DIR_HINTS}) 43 | else () 44 | find_package(Ceres HINTS ${CERES_DIR_HINTS}) 45 | endif() 46 | -------------------------------------------------------------------------------- /cartographer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | add_subdirectory("common") 16 | add_subdirectory("kalman_filter") 17 | add_subdirectory("mapping") 18 | add_subdirectory("mapping_2d") 19 | add_subdirectory("mapping_3d") 20 | add_subdirectory("proto") 21 | add_subdirectory("sensor") 22 | add_subdirectory("transform") 23 | -------------------------------------------------------------------------------- /cartographer/common/ceres_solver_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/ceres_solver_options.h" 18 | 19 | namespace cartographer { 20 | namespace common { 21 | 22 | proto::CeresSolverOptions CreateCeresSolverOptionsProto( 23 | common::LuaParameterDictionary* parameter_dictionary) { 24 | proto::CeresSolverOptions proto; 25 | proto.set_use_nonmonotonic_steps( 26 | parameter_dictionary->GetBool("use_nonmonotonic_steps")); 27 | proto.set_max_num_iterations( 28 | parameter_dictionary->GetNonNegativeInt("max_num_iterations")); 29 | proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads")); 30 | CHECK_GT(proto.max_num_iterations(), 0); 31 | CHECK_GT(proto.num_threads(), 0); 32 | return proto; 33 | } 34 | 35 | ceres::Solver::Options CreateCeresSolverOptions( 36 | const proto::CeresSolverOptions& proto) { 37 | ceres::Solver::Options options; 38 | options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps(); 39 | options.max_num_iterations = proto.max_num_iterations(); 40 | options.num_threads = proto.num_threads(); 41 | return options; 42 | } 43 | 44 | } // namespace common 45 | } // namespace cartographer 46 | -------------------------------------------------------------------------------- /cartographer/common/ceres_solver_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 18 | #define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/common/proto/ceres_solver_options.pb.h" 22 | #include "ceres/ceres.h" 23 | 24 | namespace cartographer { 25 | namespace common { 26 | 27 | proto::CeresSolverOptions CreateCeresSolverOptionsProto( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | ceres::Solver::Options CreateCeresSolverOptions( 31 | const proto::CeresSolverOptions& proto); 32 | 33 | } // namespace common 34 | } // namespace cartographer 35 | 36 | #endif // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 37 | -------------------------------------------------------------------------------- /cartographer/common/config.h.cmake: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CONFIG_H_ 18 | #define CARTOGRAPHER_COMMON_CONFIG_H_ 19 | 20 | namespace cartographer { 21 | namespace common { 22 | 23 | constexpr char kConfigurationFilesDirectory[] = 24 | "@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@"; 25 | 26 | } // namespace common 27 | } // namespace cartographer 28 | 29 | #endif // CARTOGRAPHER_COMMON_CONFIG_H_ 30 | -------------------------------------------------------------------------------- /cartographer/common/configuration_file_resolver.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/configuration_file_resolver.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/config.h" 24 | #include "glog/logging.h" 25 | 26 | namespace cartographer { 27 | namespace common { 28 | 29 | ConfigurationFileResolver::ConfigurationFileResolver( 30 | const std::vector& configuration_files_directories) 31 | : configuration_files_directories_(configuration_files_directories) { 32 | configuration_files_directories_.push_back(kConfigurationFilesDirectory); 33 | } 34 | 35 | string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) { 36 | for (const auto& path : configuration_files_directories_) { 37 | const string filename = path + "/" + basename; 38 | std::ifstream stream(filename.c_str()); 39 | if (stream.good()) { 40 | LOG(INFO) << "Found '" << filename << "' for '" << basename << "'."; 41 | return filename; 42 | } 43 | } 44 | LOG(FATAL) << "File '" << basename << "' was not found."; 45 | } 46 | 47 | string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) { 48 | const string filename = GetFullPathOrDie(basename); 49 | std::ifstream stream(filename.c_str()); 50 | return string((std::istreambuf_iterator(stream)), 51 | std::istreambuf_iterator()); 52 | } 53 | 54 | } // namespace common 55 | } // namespace cartographer 56 | -------------------------------------------------------------------------------- /cartographer/common/configuration_file_resolver.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ 18 | #define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/common/lua_parameter_dictionary.h" 23 | #include "cartographer/common/port.h" 24 | 25 | namespace cartographer { 26 | namespace common { 27 | 28 | // A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk. 29 | // It searches the 'configuration_files_directories' in order to find the 30 | // requested filename. The last place searched is always the 31 | // 'configuration_files/' directory installed with Cartographer. It contains 32 | // reasonable configuration for the various Cartographer components which 33 | // provide a good starting ground for new platforms. 34 | class ConfigurationFileResolver : public FileResolver { 35 | public: 36 | explicit ConfigurationFileResolver( 37 | const std::vector& configuration_files_directories); 38 | 39 | string GetFullPathOrDie(const string& basename) override; 40 | string GetFileContentOrDie(const string& basename) override; 41 | 42 | private: 43 | std::vector configuration_files_directories_; 44 | }; 45 | 46 | } // namespace common 47 | } // namespace cartographer 48 | 49 | #endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ 50 | -------------------------------------------------------------------------------- /cartographer/common/fixed_ratio_sampler.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/fixed_ratio_sampler.h" 18 | 19 | namespace cartographer { 20 | namespace common { 21 | 22 | FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {} 23 | 24 | FixedRatioSampler::~FixedRatioSampler() {} 25 | 26 | bool FixedRatioSampler::Pulse() { 27 | ++num_pulses_; 28 | if (static_cast(num_samples_) / num_pulses_ < ratio_) { 29 | ++num_samples_; 30 | return true; 31 | } 32 | return false; 33 | } 34 | 35 | string FixedRatioSampler::DebugString() { 36 | return std::to_string(num_samples_) + " (" + 37 | std::to_string(100. * num_samples_ / num_pulses_) + "%)"; 38 | } 39 | 40 | } // namespace common 41 | } // namespace cartographer 42 | -------------------------------------------------------------------------------- /cartographer/common/fixed_ratio_sampler.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 18 | #define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/common/port.h" 23 | 24 | namespace cartographer { 25 | namespace common { 26 | 27 | // Signals when a sample should be taken from a stream of data to select a 28 | // uniformly distributed fraction of the data. 29 | class FixedRatioSampler { 30 | public: 31 | explicit FixedRatioSampler(double ratio); 32 | ~FixedRatioSampler(); 33 | 34 | FixedRatioSampler(const FixedRatioSampler&) = delete; 35 | FixedRatioSampler& operator=(const FixedRatioSampler&) = delete; 36 | 37 | // Returns true if this pulse should result in an sample. 38 | bool Pulse(); 39 | 40 | // Returns a debug string describing the current ratio of samples to pulses. 41 | string DebugString(); 42 | 43 | private: 44 | // Sampling occurs if the proportion of samples to pulses drops below this 45 | // number. 46 | const double ratio_; 47 | 48 | int64 num_pulses_ = 0; 49 | int64 num_samples_ = 0; 50 | }; 51 | 52 | } // namespace common 53 | } // namespace cartographer 54 | 55 | #endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 56 | -------------------------------------------------------------------------------- /cartographer/common/fixed_ratio_sampler_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/fixed_ratio_sampler.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace common { 23 | namespace { 24 | 25 | TEST(FixedRatioSamplerTest, AlwaysTrue) { 26 | FixedRatioSampler fixed_ratio_sampler(1.); 27 | for (int i = 0; i < 100; ++i) { 28 | EXPECT_TRUE(fixed_ratio_sampler.Pulse()); 29 | } 30 | } 31 | 32 | TEST(FixedRatioSamplerTest, AlwaysFalse) { 33 | FixedRatioSampler fixed_ratio_sampler(0.); 34 | for (int i = 0; i < 100; ++i) { 35 | EXPECT_FALSE(fixed_ratio_sampler.Pulse()); 36 | } 37 | } 38 | 39 | TEST(FixedRatioSamplerTest, SometimesTrue) { 40 | FixedRatioSampler fixed_ratio_sampler(0.5); 41 | for (int i = 0; i < 100; ++i) { 42 | EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse()); 43 | } 44 | } 45 | 46 | TEST(FixedRatioSamplerTest, FirstPulseIsTrue) { 47 | // Choose a very very small positive number for the ratio. 48 | FixedRatioSampler fixed_ratio_sampler(1e-20); 49 | EXPECT_TRUE(fixed_ratio_sampler.Pulse()); 50 | for (int i = 0; i < 100; ++i) { 51 | EXPECT_FALSE(fixed_ratio_sampler.Pulse()); 52 | } 53 | } 54 | 55 | } // namespace 56 | } // namespace common 57 | } // namespace cartographer 58 | -------------------------------------------------------------------------------- /cartographer/common/histogram.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_ 18 | #define CARTOGRAPHER_COMMON_HISTOGRAM_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "cartographer/common/port.h" 25 | 26 | namespace cartographer { 27 | namespace common { 28 | 29 | class BucketHistogram { 30 | public: 31 | void Hit(const string& bucket); 32 | string ToString() const; 33 | 34 | private: 35 | std::map buckets_; 36 | }; 37 | 38 | class Histogram { 39 | public: 40 | void Add(float value); 41 | string ToString(int buckets) const; 42 | 43 | private: 44 | std::vector values_; 45 | }; 46 | 47 | } // namespace common 48 | } // namespace cartographer 49 | 50 | #endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_ 51 | -------------------------------------------------------------------------------- /cartographer/common/lua.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_LUA_H_ 18 | #define CARTOGRAPHER_COMMON_LUA_H_ 19 | 20 | #include 21 | 22 | #endif // CARTOGRAPHER_COMMON_LUA_H_ 23 | -------------------------------------------------------------------------------- /cartographer/common/lua_parameter_dictionary_test_helpers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ 18 | #define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/lua_parameter_dictionary.h" 24 | #include "cartographer/common/make_unique.h" 25 | #include "cartographer/common/port.h" 26 | #include "glog/logging.h" 27 | 28 | namespace cartographer { 29 | namespace common { 30 | 31 | class DummyFileResolver : public FileResolver { 32 | public: 33 | DummyFileResolver() {} 34 | 35 | DummyFileResolver(const DummyFileResolver&) = delete; 36 | DummyFileResolver& operator=(const DummyFileResolver&) = delete; 37 | 38 | ~DummyFileResolver() override {} 39 | 40 | string GetFileContentOrDie(const string& unused_basename) override { 41 | LOG(FATAL) << "Not implemented"; 42 | } 43 | 44 | string GetFullPathOrDie(const string& unused_basename) override { 45 | LOG(FATAL) << "Not implemented"; 46 | } 47 | }; 48 | 49 | std::unique_ptr MakeDictionary(const string& code) { 50 | return common::make_unique( 51 | code, std::unique_ptr(new DummyFileResolver()), 52 | nullptr /* state_extension_function */); 53 | } 54 | 55 | } // namespace common 56 | } // namespace cartographer 57 | 58 | #endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ 59 | -------------------------------------------------------------------------------- /cartographer/common/make_unique.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 18 | #define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace cartographer { 26 | namespace common { 27 | 28 | // Implementation of c++14's std::make_unique, taken from 29 | // https://isocpp.org/files/papers/N3656.txt 30 | template 31 | struct _Unique_if { 32 | typedef std::unique_ptr _Single_object; 33 | }; 34 | 35 | template 36 | struct _Unique_if { 37 | typedef std::unique_ptr _Unknown_bound; 38 | }; 39 | 40 | template 41 | struct _Unique_if { 42 | typedef void _Known_bound; 43 | }; 44 | 45 | template 46 | typename _Unique_if::_Single_object make_unique(Args&&... args) { 47 | return std::unique_ptr(new T(std::forward(args)...)); 48 | } 49 | 50 | template 51 | typename _Unique_if::_Unknown_bound make_unique(size_t n) { 52 | typedef typename std::remove_extent::type U; 53 | return std::unique_ptr(new U[n]()); 54 | } 55 | 56 | template 57 | typename _Unique_if::_Known_bound make_unique(Args&&...) = delete; 58 | 59 | } // namespace common 60 | } // namespace cartographer 61 | 62 | #endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 63 | -------------------------------------------------------------------------------- /cartographer/common/math_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/math.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace common { 23 | namespace { 24 | 25 | TEST(MathTest, testPower) { 26 | EXPECT_EQ(0., Power(0, 42)); 27 | EXPECT_EQ(1., Power(0, 0)); 28 | EXPECT_EQ(1., Power(1, 0)); 29 | EXPECT_EQ(1., Power(1, 42)); 30 | EXPECT_EQ(4., Power(2, 2)); 31 | } 32 | 33 | TEST(MathTest, testPow2) { 34 | EXPECT_EQ(0., Pow2(0)); 35 | EXPECT_EQ(1., Pow2(1)); 36 | EXPECT_EQ(4., Pow2(2)); 37 | EXPECT_EQ(49., Pow2(7)); 38 | } 39 | 40 | TEST(MathTest, testDeg2rad) { 41 | EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9); 42 | EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6); 43 | } 44 | 45 | TEST(MathTest, testRad2deg) { 46 | EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9); 47 | EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6); 48 | } 49 | 50 | TEST(MathTest, testNormalizeAngleDifference) { 51 | EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9); 52 | EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9); 53 | EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9); 54 | EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9); 55 | EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9); 56 | EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9); 57 | } 58 | 59 | } // namespace 60 | } // namespace common 61 | } // namespace cartographer 62 | -------------------------------------------------------------------------------- /cartographer/common/port.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_PORT_H_ 18 | #define CARTOGRAPHER_COMMON_PORT_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | using int8 = int8_t; 29 | using int16 = int16_t; 30 | using int32 = int32_t; 31 | using int64 = int64_t; 32 | using uint8 = uint8_t; 33 | using uint16 = uint16_t; 34 | using uint32 = uint32_t; 35 | using uint64 = uint64_t; 36 | 37 | using std::string; 38 | 39 | namespace cartographer { 40 | namespace common { 41 | 42 | inline int RoundToInt(const float x) { return std::lround(x); } 43 | 44 | inline int RoundToInt(const double x) { return std::lround(x); } 45 | 46 | inline int64 RoundToInt64(const float x) { return std::lround(x); } 47 | 48 | inline int64 RoundToInt64(const double x) { return std::lround(x); } 49 | 50 | inline void FastGzipString(const string& uncompressed, string* compressed) { 51 | boost::iostreams::filtering_ostream out; 52 | out.push( 53 | boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed)); 54 | out.push(boost::iostreams::back_inserter(*compressed)); 55 | boost::iostreams::write(out, 56 | reinterpret_cast(uncompressed.data()), 57 | uncompressed.size()); 58 | } 59 | 60 | inline void FastGunzipString(const string& compressed, string* decompressed) { 61 | boost::iostreams::filtering_ostream out; 62 | out.push(boost::iostreams::gzip_decompressor()); 63 | out.push(boost::iostreams::back_inserter(*decompressed)); 64 | boost::iostreams::write(out, reinterpret_cast(compressed.data()), 65 | compressed.size()); 66 | } 67 | 68 | } // namespace common 69 | } // namespace cartographer 70 | 71 | #endif // CARTOGRAPHER_COMMON_PORT_H_ 72 | -------------------------------------------------------------------------------- /cartographer/common/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(common_proto_ceres_solver_options 16 | SRCS 17 | ceres_solver_options.proto 18 | ) 19 | -------------------------------------------------------------------------------- /cartographer/common/proto/ceres_solver_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.common.proto; 18 | 19 | message CeresSolverOptions { 20 | // Configure the Ceres solver. See the Ceres documentation for more 21 | // information: https://code.google.com/p/ceres-solver/ 22 | optional bool use_nonmonotonic_steps = 1; 23 | optional int32 max_num_iterations = 2; 24 | optional int32 num_threads = 3; 25 | } 26 | -------------------------------------------------------------------------------- /cartographer/common/rate_timer_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/rate_timer.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace common { 23 | namespace { 24 | 25 | TEST(RateTimerTest, ComputeRate) { 26 | RateTimer<> rate_timer(common::FromSeconds(1.)); 27 | common::Time time = common::FromUniversal(42); 28 | for (int i = 0; i < 100; ++i) { 29 | rate_timer.Pulse(time); 30 | time += common::FromSeconds(0.1); 31 | } 32 | EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3); 33 | } 34 | 35 | struct SimulatedClock { 36 | using rep = std::chrono::steady_clock::rep; 37 | using period = std::chrono::steady_clock::period; 38 | using duration = std::chrono::steady_clock::duration; 39 | using time_point = std::chrono::steady_clock::time_point; 40 | static constexpr bool is_steady = true; 41 | 42 | static time_point time; 43 | static time_point now() noexcept { return time; } 44 | }; 45 | 46 | SimulatedClock::time_point SimulatedClock::time; 47 | 48 | TEST(RateTimerTest, ComputeWallTimeRateRatio) { 49 | common::Time time = common::FromUniversal(42); 50 | RateTimer rate_timer(common::FromSeconds(1.)); 51 | for (int i = 0; i < 100; ++i) { 52 | rate_timer.Pulse(time); 53 | time += common::FromSeconds(0.1); 54 | SimulatedClock::time += 55 | std::chrono::duration_cast( 56 | std::chrono::duration(0.05)); 57 | } 58 | EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); 59 | } 60 | 61 | } // namespace 62 | } // namespace common 63 | } // namespace cartographer 64 | -------------------------------------------------------------------------------- /cartographer/common/thread_pool.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/thread_pool.h" 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "glog/logging.h" 25 | 26 | namespace cartographer { 27 | namespace common { 28 | 29 | ThreadPool::ThreadPool(int num_threads) { 30 | MutexLocker locker(&mutex_); 31 | for (int i = 0; i != num_threads; ++i) { 32 | pool_.emplace_back([this]() { ThreadPool::DoWork(); }); 33 | } 34 | } 35 | 36 | ThreadPool::~ThreadPool() { 37 | { 38 | MutexLocker locker(&mutex_); 39 | CHECK(running_); 40 | running_ = false; 41 | CHECK_EQ(work_queue_.size(), 0); 42 | } 43 | for (std::thread& thread : pool_) { 44 | thread.join(); 45 | } 46 | } 47 | 48 | void ThreadPool::Schedule(std::function work_item) { 49 | MutexLocker locker(&mutex_); 50 | CHECK(running_); 51 | work_queue_.push_back(work_item); 52 | } 53 | 54 | void ThreadPool::DoWork() { 55 | #ifdef __linux__ 56 | // This changes the per-thread nice level of the current thread on Linux. We 57 | // do this so that the background work done by the thread pool is not taking 58 | // away CPU resources from more important foreground threads. 59 | CHECK_NE(nice(10), -1); 60 | #endif 61 | for (;;) { 62 | std::function work_item; 63 | { 64 | MutexLocker locker(&mutex_); 65 | locker.Await([this]() REQUIRES(mutex_) { 66 | return !work_queue_.empty() || !running_; 67 | }); 68 | if (!work_queue_.empty()) { 69 | work_item = work_queue_.front(); 70 | work_queue_.pop_front(); 71 | } else if (!running_) { 72 | return; 73 | } 74 | } 75 | CHECK(work_item); 76 | work_item(); 77 | } 78 | } 79 | 80 | } // namespace common 81 | } // namespace cartographer 82 | -------------------------------------------------------------------------------- /cartographer/common/thread_pool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_ 18 | #define CARTOGRAPHER_COMMON_THREAD_POOL_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "cartographer/common/mutex.h" 26 | 27 | namespace cartographer { 28 | namespace common { 29 | 30 | // A fixed number of threads working on a work queue of work items. Adding a 31 | // new work item does not block, and will be executed by a background thread 32 | // eventually. The queue must be empty before calling the destructor. The thread 33 | // pool will then wait for the currently executing work items to finish and then 34 | // destroy the threads. 35 | class ThreadPool { 36 | public: 37 | explicit ThreadPool(int num_threads); 38 | ~ThreadPool(); 39 | 40 | ThreadPool(const ThreadPool&) = delete; 41 | ThreadPool& operator=(const ThreadPool&) = delete; 42 | 43 | void Schedule(std::function work_item); 44 | 45 | private: 46 | void DoWork(); 47 | 48 | Mutex mutex_; 49 | bool running_ GUARDED_BY(mutex_) = true; 50 | std::vector pool_ GUARDED_BY(mutex_); 51 | std::deque> work_queue_ GUARDED_BY(mutex_); 52 | }; 53 | 54 | } // namespace common 55 | } // namespace cartographer 56 | 57 | #endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_ 58 | -------------------------------------------------------------------------------- /cartographer/common/time.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/time.h" 18 | 19 | #include 20 | 21 | namespace cartographer { 22 | namespace common { 23 | 24 | Duration FromSeconds(const double seconds) { 25 | return std::chrono::duration_cast( 26 | std::chrono::duration(seconds)); 27 | } 28 | 29 | double ToSeconds(const Duration duration) { 30 | return std::chrono::duration_cast>(duration) 31 | .count(); 32 | } 33 | 34 | Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); } 35 | 36 | int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); } 37 | 38 | std::ostream& operator<<(std::ostream& os, const Time time) { 39 | os << std::to_string(ToUniversal(time)); 40 | return os; 41 | } 42 | 43 | common::Duration FromMilliseconds(const int64 milliseconds) { 44 | return std::chrono::duration_cast( 45 | std::chrono::milliseconds(milliseconds)); 46 | } 47 | 48 | } // namespace common 49 | } // namespace cartographer 50 | -------------------------------------------------------------------------------- /cartographer/common/time.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_TIME_H_ 18 | #define CARTOGRAPHER_COMMON_TIME_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "cartographer/common/port.h" 25 | 26 | namespace cartographer { 27 | namespace common { 28 | 29 | struct UniversalTimeScaleClock { 30 | using rep = int64; 31 | using period = std::ratio<1, 10000000>; 32 | using duration = std::chrono::duration; 33 | using time_point = std::chrono::time_point; 34 | static constexpr bool is_steady = true; 35 | }; 36 | 37 | // Represents Universal Time Scale durations and timestamps which are 64-bit 38 | // integers representing the 100 nanosecond ticks since the Epoch which is 39 | // January 1, 1 at the start of day in UTC. 40 | using Duration = UniversalTimeScaleClock::duration; 41 | using Time = UniversalTimeScaleClock::time_point; 42 | 43 | // Convenience functions to create common::Durations. 44 | Duration FromSeconds(double seconds); 45 | Duration FromMilliseconds(int64 milliseconds); 46 | 47 | // Returns the given duration in seconds. 48 | double ToSeconds(Duration duration); 49 | 50 | // Creates a time from a Universal Time Scale. 51 | Time FromUniversal(int64 ticks); 52 | 53 | // Outputs the Universal Time Scale timestamp for a given Time. 54 | int64 ToUniversal(Time time); 55 | 56 | // For logging and unit tests, outputs the timestamp integer. 57 | std::ostream& operator<<(std::ostream& os, Time time); 58 | 59 | } // namespace common 60 | } // namespace cartographer 61 | 62 | #endif // CARTOGRAPHER_COMMON_TIME_H_ 63 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | add_subdirectory("proto") 16 | 17 | google_library(kalman_filter_gaussian_distribution 18 | USES_EIGEN 19 | HDRS 20 | gaussian_distribution.h 21 | ) 22 | 23 | google_library(kalman_filter_odometry_state_tracker 24 | SRCS 25 | odometry_state_tracker.cc 26 | HDRS 27 | odometry_state_tracker.h 28 | DEPENDS 29 | common_time 30 | transform_rigid_transform 31 | ) 32 | 33 | google_library(kalman_filter_pose_tracker 34 | USES_CERES 35 | USES_EIGEN 36 | SRCS 37 | pose_tracker.cc 38 | HDRS 39 | pose_tracker.h 40 | DEPENDS 41 | common_lua_parameter_dictionary 42 | common_math 43 | common_port 44 | common_time 45 | kalman_filter_gaussian_distribution 46 | kalman_filter_odometry_state_tracker 47 | kalman_filter_proto_pose_tracker_options 48 | kalman_filter_unscented_kalman_filter 49 | sensor_proto_sensor 50 | transform_transform 51 | ) 52 | 53 | google_library(kalman_filter_unscented_kalman_filter 54 | USES_CERES 55 | USES_EIGEN 56 | HDRS 57 | unscented_kalman_filter.h 58 | DEPENDS 59 | kalman_filter_gaussian_distribution 60 | ) 61 | 62 | google_test(kalman_filter_gaussian_distribution_test 63 | SRCS 64 | gaussian_distribution_test.cc 65 | DEPENDS 66 | kalman_filter_gaussian_distribution 67 | ) 68 | 69 | google_test(kalman_filter_pose_tracker_test 70 | SRCS 71 | pose_tracker_test.cc 72 | DEPENDS 73 | common_lua_parameter_dictionary 74 | common_lua_parameter_dictionary_test_helpers 75 | common_make_unique 76 | kalman_filter_pose_tracker 77 | transform_rigid_transform 78 | transform_rigid_transform_test_helpers 79 | ) 80 | 81 | google_test(kalman_filter_unscented_kalman_filter_test 82 | SRCS 83 | unscented_kalman_filter_test.cc 84 | DEPENDS 85 | kalman_filter_gaussian_distribution 86 | kalman_filter_unscented_kalman_filter 87 | ) 88 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/gaussian_distribution.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ 18 | #define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ 19 | 20 | #include "Eigen/Cholesky" 21 | #include "Eigen/Core" 22 | #include "Eigen/Geometry" 23 | 24 | namespace cartographer { 25 | namespace kalman_filter { 26 | 27 | template 28 | class GaussianDistribution { 29 | public: 30 | GaussianDistribution(const Eigen::Matrix& mean, 31 | const Eigen::Matrix& covariance) 32 | : mean_(mean), covariance_(covariance) {} 33 | 34 | const Eigen::Matrix& GetMean() const { return mean_; } 35 | 36 | const Eigen::Matrix& GetCovariance() const { return covariance_; } 37 | 38 | private: 39 | Eigen::Matrix mean_; 40 | Eigen::Matrix covariance_; 41 | }; 42 | 43 | template 44 | GaussianDistribution operator+(const GaussianDistribution& lhs, 45 | const GaussianDistribution& rhs) { 46 | return GaussianDistribution(lhs.GetMean() + rhs.GetMean(), 47 | lhs.GetCovariance() + rhs.GetCovariance()); 48 | } 49 | 50 | template 51 | GaussianDistribution operator*(const Eigen::Matrix& lhs, 52 | const GaussianDistribution& rhs) { 53 | return GaussianDistribution( 54 | lhs * rhs.GetMean(), lhs * rhs.GetCovariance() * lhs.transpose()); 55 | } 56 | 57 | } // namespace kalman_filter 58 | } // namespace cartographer 59 | 60 | #endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ 61 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/gaussian_distribution_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/kalman_filter/gaussian_distribution.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace kalman_filter { 23 | namespace { 24 | 25 | TEST(GaussianDistributionTest, testConstructor) { 26 | Eigen::Matrix2d covariance; 27 | covariance << 1., 2., 3., 4.; 28 | GaussianDistribution distribution(Eigen::Vector2d(0., 1.), 29 | covariance); 30 | EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9); 31 | EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9); 32 | EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9); 33 | } 34 | 35 | } // namespace 36 | } // namespace kalman_filter 37 | } // namespace cartographer 38 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/odometry_state_tracker.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/kalman_filter/odometry_state_tracker.h" 18 | 19 | #include "cartographer/transform/rigid_transform.h" 20 | 21 | namespace cartographer { 22 | namespace kalman_filter { 23 | 24 | OdometryState::OdometryState(const common::Time time, 25 | const transform::Rigid3d& odometer_pose, 26 | const transform::Rigid3d& state_pose) 27 | : time(time), odometer_pose(odometer_pose), state_pose(state_pose) {} 28 | 29 | OdometryStateTracker::OdometryStateTracker(const int window_size) 30 | : window_size_(window_size) { 31 | CHECK_GT(window_size, 0); 32 | } 33 | 34 | const OdometryStateTracker::OdometryStates& 35 | OdometryStateTracker::odometry_states() const { 36 | return odometry_states_; 37 | } 38 | 39 | void OdometryStateTracker::AddOdometryState( 40 | const OdometryState& odometry_state) { 41 | odometry_states_.push_back(odometry_state); 42 | while (odometry_states_.size() > window_size_) { 43 | odometry_states_.pop_front(); 44 | } 45 | } 46 | 47 | bool OdometryStateTracker::empty() const { return odometry_states_.empty(); } 48 | 49 | const OdometryState& OdometryStateTracker::newest() const { 50 | return odometry_states_.back(); 51 | } 52 | 53 | } // namespace kalman_filter 54 | } // namespace cartographer 55 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/odometry_state_tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_ 18 | #define CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/common/time.h" 23 | #include "cartographer/transform/rigid_transform.h" 24 | 25 | namespace cartographer { 26 | namespace kalman_filter { 27 | 28 | struct OdometryState { 29 | OdometryState(common::Time time, const transform::Rigid3d& odometer_pose, 30 | const transform::Rigid3d& state_pose); 31 | OdometryState() {} 32 | 33 | common::Time time = common::Time::min(); 34 | transform::Rigid3d odometer_pose = transform::Rigid3d::Identity(); 35 | transform::Rigid3d state_pose = transform::Rigid3d::Identity(); 36 | }; 37 | 38 | // Keeps track of the odometry states by keeping sliding window over some 39 | // number of them. 40 | class OdometryStateTracker { 41 | public: 42 | using OdometryStates = std::deque; 43 | 44 | explicit OdometryStateTracker(int window_size); 45 | 46 | const OdometryStates& odometry_states() const; 47 | 48 | // Adds a new 'odometry_state' and makes sure the maximum number of previous 49 | // odometry states is not exceeded. 50 | void AddOdometryState(const OdometryState& odometry_state); 51 | 52 | // Returns true if no elements are present in the odometry queue. 53 | bool empty() const; 54 | 55 | // Retrieves the most recent OdometryState or an empty one if non yet present. 56 | const OdometryState& newest() const; 57 | 58 | private: 59 | OdometryStates odometry_states_; 60 | size_t window_size_; 61 | }; 62 | 63 | } // namespace kalman_filter 64 | } // namespace cartographer 65 | 66 | #endif // CARTOGRAPHER_KALMAN_FILTER_ODOMETRY_STATE_TRACKER_H_ 67 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(kalman_filter_proto_pose_tracker_options 16 | SRCS 17 | pose_tracker_options.proto 18 | ) 19 | -------------------------------------------------------------------------------- /cartographer/kalman_filter/proto/pose_tracker_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.kalman_filter.proto; 18 | 19 | message PoseTrackerOptions { 20 | // Model variances depend linearly on time. 21 | optional double position_model_variance = 1; 22 | optional double orientation_model_variance = 2; 23 | optional double velocity_model_variance = 3; 24 | 25 | // Time constant for the orientation moving average based on observed gravity 26 | // via linear acceleration. 27 | optional double imu_gravity_time_constant = 4; 28 | optional double imu_gravity_variance = 5; 29 | 30 | // Maximum number of previous odometry states to keep. 31 | optional int32 num_odometry_states = 6; 32 | } 33 | -------------------------------------------------------------------------------- /cartographer/mapping/global_trajectory_builder_interface.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/global_trajectory_builder_interface.h" 18 | 19 | namespace cartographer { 20 | namespace mapping { 21 | 22 | GlobalTrajectoryBuilderInterface::PoseEstimate::PoseEstimate( 23 | const common::Time time, const kalman_filter::PoseAndCovariance& prediction, 24 | const kalman_filter::PoseAndCovariance& observation, 25 | const kalman_filter::PoseAndCovariance& estimate, 26 | const transform::Rigid3d& pose, const sensor::PointCloud& point_cloud) 27 | : time(time), 28 | prediction(prediction), 29 | observation(observation), 30 | estimate(estimate), 31 | pose(pose), 32 | point_cloud(point_cloud) {} 33 | 34 | } // namespace mapping 35 | } // namespace cartographer 36 | -------------------------------------------------------------------------------- /cartographer/mapping/probability_values.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/probability_values.h" 18 | 19 | namespace cartographer { 20 | namespace mapping { 21 | 22 | namespace { 23 | 24 | // 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability]. 25 | float SlowValueToProbability(const uint16 value) { 26 | CHECK_GE(value, 0); 27 | CHECK_LE(value, 32767); 28 | if (value == kUnknownProbabilityValue) { 29 | // Unknown cells have kMinProbability. 30 | return kMinProbability; 31 | } 32 | const float kScale = (kMaxProbability - kMinProbability) / 32766.f; 33 | return value * kScale + (kMinProbability - kScale); 34 | } 35 | 36 | const std::vector* PrecomputeValueToProbability() { 37 | std::vector* result = new std::vector; 38 | // Repeat two times, so that both values with and without the update marker 39 | // can be converted to a probability. 40 | for (int repeat = 0; repeat != 2; ++repeat) { 41 | for (int value = 0; value != 32768; ++value) { 42 | result->push_back(SlowValueToProbability(value)); 43 | } 44 | } 45 | return result; 46 | } 47 | 48 | } // namespace 49 | 50 | const std::vector* const kValueToProbability = 51 | PrecomputeValueToProbability(); 52 | 53 | std::vector ComputeLookupTableToApplyOdds(const float odds) { 54 | std::vector result; 55 | result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + 56 | kUpdateMarker); 57 | for (int cell = 1; cell != 32768; ++cell) { 58 | result.push_back(ProbabilityToValue(ProbabilityFromOdds( 59 | odds * Odds((*kValueToProbability)[cell]))) + 60 | kUpdateMarker); 61 | } 62 | return result; 63 | } 64 | 65 | } // namespace mapping 66 | } // namespace cartographer 67 | -------------------------------------------------------------------------------- /cartographer/mapping/probability_values.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ 18 | #define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/math.h" 24 | #include "cartographer/common/port.h" 25 | #include "glog/logging.h" 26 | 27 | namespace cartographer { 28 | namespace mapping { 29 | 30 | inline float Odds(float probability) { 31 | return probability / (1.f - probability); 32 | } 33 | 34 | inline float ProbabilityFromOdds(const float odds) { 35 | return odds / (odds + 1.f); 36 | } 37 | 38 | constexpr float kMinProbability = 0.1f; 39 | constexpr float kMaxProbability = 1.f - kMinProbability; 40 | 41 | // Clamps probability to be in the range [kMinProbability, kMaxProbability]. 42 | inline float ClampProbability(const float probability) { 43 | return common::Clamp(probability, kMinProbability, kMaxProbability); 44 | } 45 | 46 | constexpr uint16 kUnknownProbabilityValue = 0; 47 | constexpr uint16 kUpdateMarker = 1u << 15; 48 | 49 | // Converts a probability to a uint16 in the [1, 32767] range. 50 | inline uint16 ProbabilityToValue(const float probability) { 51 | const int value = 52 | common::RoundToInt((ClampProbability(probability) - kMinProbability) * 53 | (32766.f / (kMaxProbability - kMinProbability))) + 54 | 1; 55 | // DCHECK for performance. 56 | DCHECK_GE(value, 1); 57 | DCHECK_LE(value, 32767); 58 | return value; 59 | } 60 | 61 | extern const std::vector* const kValueToProbability; 62 | 63 | // Converts a uint16 (which may or may not have the update marker set) to a 64 | // probability in the range [kMinProbability, kMaxProbability]. 65 | inline float ValueToProbability(const uint16 value) { 66 | return (*kValueToProbability)[value]; 67 | } 68 | 69 | std::vector ComputeLookupTableToApplyOdds(float odds); 70 | 71 | } // namespace mapping 72 | } // namespace cartographer 73 | 74 | #endif // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ 75 | -------------------------------------------------------------------------------- /cartographer/mapping/probability_values_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/probability_values.h" 18 | #include "gtest/gtest.h" 19 | 20 | namespace cartographer { 21 | namespace mapping { 22 | namespace { 23 | 24 | TEST(ProbabilityValuesTest, OddsConversions) { 25 | EXPECT_NEAR(ProbabilityFromOdds(Odds(kMinProbability)), kMinProbability, 26 | 1e-6); 27 | EXPECT_NEAR(ProbabilityFromOdds(Odds(kMaxProbability)), kMaxProbability, 28 | 1e-6); 29 | EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6); 30 | } 31 | 32 | } // namespace 33 | } // namespace mapping 34 | } // namespace cartographer 35 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_proto_map_builder_options 16 | SRCS 17 | map_builder_options.proto 18 | DEPENDS 19 | mapping_2d_proto_local_trajectory_builder_options 20 | mapping_3d_proto_local_trajectory_builder_options 21 | mapping_proto_sparse_pose_graph_options 22 | ) 23 | 24 | google_proto_library(mapping_proto_scan_matching_progress 25 | SRCS 26 | scan_matching_progress.proto 27 | ) 28 | 29 | google_proto_library(mapping_proto_sparse_pose_graph_options 30 | SRCS 31 | sparse_pose_graph_options.proto 32 | DEPENDS 33 | mapping_sparse_pose_graph_proto_constraint_builder_options 34 | mapping_sparse_pose_graph_proto_optimization_problem_options 35 | ) 36 | 37 | google_proto_library(mapping_proto_submaps 38 | SRCS 39 | submaps.proto 40 | DEPENDS 41 | transform_proto_transform 42 | ) 43 | 44 | google_proto_library(mapping_proto_trajectory_connectivity 45 | SRCS 46 | trajectory_connectivity.proto 47 | ) 48 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/map_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping/proto/sparse_pose_graph_options.proto"; 18 | import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto"; 19 | import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; 20 | 21 | package cartographer.mapping.proto; 22 | 23 | message MapBuilderOptions { 24 | optional bool use_trajectory_builder_2d = 1; 25 | optional mapping_2d.proto.LocalTrajectoryBuilderOptions 26 | trajectory_builder_2d_options = 2; 27 | optional bool use_trajectory_builder_3d = 3; 28 | optional mapping_3d.proto.LocalTrajectoryBuilderOptions 29 | trajectory_builder_3d_options = 4; 30 | 31 | // Number of threads to use for background computations. 32 | optional int32 num_background_threads = 5; 33 | optional SparsePoseGraphOptions sparse_pose_graph_options = 6; 34 | } 35 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/scan_matching_progress.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.proto; 18 | 19 | // This is how proto2 calls the outer class since there is already a message 20 | // with the same name in the file. 21 | option java_outer_classname = "ScanMatchingProgressOuterClass"; 22 | 23 | message ScanMatchingProgress { 24 | optional int64 num_scans_finished = 1; 25 | optional int64 num_scans_total = 2; 26 | } 27 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/sparse_pose_graph_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.proto; 18 | 19 | import "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto"; 20 | import "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto"; 21 | 22 | message SparsePoseGraphOptions { 23 | // Online loop closure: If positive, will run the loop closure while the map 24 | // is built. 25 | optional int32 optimize_every_n_scans = 1; 26 | 27 | // Options for the constraint builder. 28 | optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions 29 | constraint_builder_options = 3; 30 | 31 | // Options for the optimization problem. 32 | optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions 33 | optimization_problem_options = 4; 34 | 35 | // Number of iterations to use in 'optimization_problem_options' for the final 36 | // optimization. 37 | optional int32 max_num_final_iterations = 6; 38 | 39 | // Rate at which we sample a single trajectory's scans for global 40 | // localization. 41 | optional double global_sampling_ratio = 5; 42 | }; 43 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/submaps.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/transform/proto/transform.proto"; 18 | 19 | package cartographer.mapping.proto; 20 | 21 | message SubmapList { 22 | message SubmapEntry { 23 | optional int32 submap_version = 1; 24 | optional transform.proto.Rigid3d pose = 3; 25 | } 26 | 27 | message TrajectorySubmapList { 28 | repeated SubmapEntry submap = 1; 29 | } 30 | 31 | repeated TrajectorySubmapList trajectory = 2; 32 | } 33 | 34 | message SubmapQuery { 35 | message Request { 36 | // Index into 'SubmapList.trajectory(trajectory_id).submap'. 37 | optional int32 submap_id = 1; 38 | // Index into 'TrajectoryList.trajectory'. 39 | optional int32 trajectory_id = 2; 40 | } 41 | 42 | message Response { 43 | // ID of the Submap. 44 | optional int32 submap_id = 1; 45 | 46 | // Version of the given submap, higher means newer. 47 | optional int32 submap_version = 2; 48 | 49 | // GZipped map data, in row-major order, starting with (0,0). Each cell 50 | // consists of two bytes: value (premultiplied by alpha) and alpha. 51 | optional bytes cells = 3; 52 | 53 | // Dimensions of the grid in cells. 54 | optional int32 width = 4; 55 | optional int32 height = 5; 56 | 57 | // Size of one cell in meters. 58 | optional double resolution = 6; 59 | 60 | // Pose of the resolution*width x resolution*height rectangle in the submap 61 | // frame. 62 | optional transform.proto.Rigid3d slice_pose = 9; 63 | 64 | // Error message in response to malformed requests. 65 | optional string error_message = 8; 66 | } 67 | 68 | optional Request request = 1; 69 | optional Response response = 2; 70 | } 71 | -------------------------------------------------------------------------------- /cartographer/mapping/proto/trajectory_connectivity.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.proto; 18 | 19 | // This is how proto2 calls the outer class since there is already a message 20 | // with the same name in the file. 21 | option java_outer_classname = "TrajectoryConnectivityOuterClass"; 22 | 23 | // Connectivity structure between trajectories. 24 | message TrajectoryConnectivity { 25 | message ConnectedComponent { 26 | repeated int32 trajectory_id = 1; 27 | } 28 | 29 | repeated ConnectedComponent connected_component = 1; 30 | } 31 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | add_subdirectory("proto") 16 | 17 | google_library(mapping_sparse_pose_graph_constraint_builder 18 | SRCS 19 | constraint_builder.cc 20 | HDRS 21 | constraint_builder.h 22 | DEPENDS 23 | common_lua_parameter_dictionary 24 | mapping_2d_scan_matching_ceres_scan_matcher 25 | mapping_2d_scan_matching_fast_correlative_scan_matcher 26 | mapping_3d_scan_matching_ceres_scan_matcher 27 | mapping_3d_scan_matching_fast_correlative_scan_matcher 28 | mapping_sparse_pose_graph_proto_constraint_builder_options 29 | sensor_voxel_filter 30 | ) 31 | 32 | google_library(mapping_sparse_pose_graph_optimization_problem_options 33 | SRCS 34 | optimization_problem_options.cc 35 | HDRS 36 | optimization_problem_options.h 37 | DEPENDS 38 | common_ceres_solver_options 39 | common_lua_parameter_dictionary 40 | mapping_sparse_pose_graph_proto_optimization_problem_options 41 | ) 42 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/constraint_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 18 | #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping { 25 | namespace sparse_pose_graph { 26 | 27 | proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace sparse_pose_graph 31 | } // namespace mapping 32 | } // namespace cartographer 33 | 34 | #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 35 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/optimization_problem_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" 18 | 19 | #include "cartographer/common/ceres_solver_options.h" 20 | 21 | namespace cartographer { 22 | namespace mapping { 23 | namespace sparse_pose_graph { 24 | 25 | proto::OptimizationProblemOptions CreateOptimizationProblemOptions( 26 | common::LuaParameterDictionary* const parameter_dictionary) { 27 | proto::OptimizationProblemOptions options; 28 | options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); 29 | options.set_acceleration_scale( 30 | parameter_dictionary->GetDouble("acceleration_scale")); 31 | options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); 32 | options.set_consecutive_scan_translation_penalty_factor( 33 | parameter_dictionary->GetDouble( 34 | "consecutive_scan_translation_penalty_factor")); 35 | options.set_consecutive_scan_rotation_penalty_factor( 36 | parameter_dictionary->GetDouble( 37 | "consecutive_scan_rotation_penalty_factor")); 38 | options.set_log_solver_summary( 39 | parameter_dictionary->GetBool("log_solver_summary")); 40 | options.set_log_residual_histograms( 41 | parameter_dictionary->GetBool("log_residual_histograms")); 42 | *options.mutable_ceres_solver_options() = 43 | common::CreateCeresSolverOptionsProto( 44 | parameter_dictionary->GetDictionary("ceres_solver_options").get()); 45 | return options; 46 | } 47 | 48 | } // namespace sparse_pose_graph 49 | } // namespace mapping 50 | } // namespace cartographer 51 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/optimization_problem_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping { 25 | namespace sparse_pose_graph { 26 | 27 | proto::OptimizationProblemOptions CreateOptimizationProblemOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace sparse_pose_graph 31 | } // namespace mapping 32 | } // namespace cartographer 33 | 34 | #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 35 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_sparse_pose_graph_proto_constraint_builder_options 16 | SRCS 17 | constraint_builder_options.proto 18 | DEPENDS 19 | mapping_2d_scan_matching_proto_ceres_scan_matcher_options 20 | mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options 21 | mapping_3d_scan_matching_proto_ceres_scan_matcher_options 22 | mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options 23 | sensor_proto_adaptive_voxel_filter_options 24 | ) 25 | 26 | google_proto_library(mapping_sparse_pose_graph_proto_optimization_problem_options 27 | SRCS 28 | optimization_problem_options.proto 29 | DEPENDS 30 | common_proto_ceres_solver_options 31 | ) 32 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.sparse_pose_graph.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 11 22 | message OptimizationProblemOptions { 23 | // Scaling parameter for Huber loss function. 24 | optional double huber_scale = 1; 25 | 26 | // Scaling parameter for the IMU acceleration term. 27 | optional double acceleration_scale = 8; 28 | 29 | // Scaling parameter for the IMU rotation term. 30 | optional double rotation_scale = 9; 31 | 32 | // Penalty factors for changes to the relative pose between consecutive scans. 33 | optional double consecutive_scan_translation_penalty_factor = 2; 34 | optional double consecutive_scan_rotation_penalty_factor = 3; 35 | 36 | // If true, the Ceres solver summary will be logged for every optimization. 37 | optional bool log_solver_summary = 5; 38 | 39 | // If true, histograms of the final residual values after optimization will be 40 | // logged for every optimization. 41 | optional bool log_residual_histograms = 10; 42 | 43 | optional common.proto.CeresSolverOptions ceres_solver_options = 7; 44 | } 45 | -------------------------------------------------------------------------------- /cartographer/mapping/sparse_pose_graph_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/sparse_pose_graph.h" 18 | 19 | #include 20 | 21 | #include "glog/logging.h" 22 | #include "gmock/gmock.h" 23 | 24 | namespace cartographer { 25 | namespace mapping { 26 | namespace { 27 | 28 | class FakeSubmaps : public Submaps { 29 | public: 30 | const Submap* Get(int) const override { LOG(FATAL) << "Not implemented."; } 31 | 32 | int size() const override { LOG(FATAL) << "Not implemented."; } 33 | 34 | void SubmapToProto(int, const std::vector&, 35 | const transform::Rigid3d&, 36 | proto::SubmapQuery::Response*) override { 37 | LOG(FATAL) << "Not implemented."; 38 | } 39 | }; 40 | 41 | TEST(SparsePoseGraphTest, SplitTrajectoryNodes) { 42 | std::vector trajectory_nodes; 43 | const std::vector submaps(5); 44 | std::deque constant_data; 45 | constexpr int kNumTrajectoryNodes = 10; 46 | for (int j = 0; j < kNumTrajectoryNodes; ++j) { 47 | for (size_t i = 0; i < submaps.size(); ++i) { 48 | constant_data.push_back({}); 49 | constant_data.back().trajectory = &submaps[i]; 50 | TrajectoryNode node; 51 | node.constant_data = &constant_data.back(); 52 | trajectory_nodes.push_back(node); 53 | } 54 | } 55 | std::vector> split_trajectory_nodes = 56 | SplitTrajectoryNodes(trajectory_nodes); 57 | EXPECT_EQ(split_trajectory_nodes.size(), submaps.size()); 58 | for (size_t i = 0; i < submaps.size(); ++i) { 59 | EXPECT_EQ(split_trajectory_nodes[i].size(), kNumTrajectoryNodes); 60 | for (const auto& node : split_trajectory_nodes[i]) { 61 | EXPECT_EQ(node.constant_data->trajectory, &submaps[i]); 62 | } 63 | } 64 | } 65 | 66 | } // namespace 67 | } // namespace mapping 68 | } // namespace cartographer 69 | -------------------------------------------------------------------------------- /cartographer/mapping/submaps_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping/submaps.h" 18 | 19 | #include 20 | 21 | #include "gtest/gtest.h" 22 | 23 | namespace cartographer { 24 | namespace mapping { 25 | namespace { 26 | 27 | // Converts the given log odds to a probability. This function is known to be 28 | // very slow, because expf is incredibly slow. 29 | inline float Expit(float log_odds) { 30 | const float exp_log_odds = std::exp(log_odds); 31 | return exp_log_odds / (1.f + exp_log_odds); 32 | } 33 | 34 | TEST(SubmapsTest, LogOddsConversions) { 35 | EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6); 36 | EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6); 37 | EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6); 38 | } 39 | 40 | } // namespace 41 | } // namespace mapping 42 | } // namespace cartographer 43 | -------------------------------------------------------------------------------- /cartographer/mapping/trajectory_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 18 | #define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "Eigen/Core" 24 | #include "cartographer/common/time.h" 25 | #include "cartographer/sensor/laser.h" 26 | #include "cartographer/transform/rigid_transform.h" 27 | 28 | namespace cartographer { 29 | namespace mapping { 30 | 31 | struct Submaps; 32 | 33 | struct TrajectoryNode { 34 | struct ConstantData { 35 | common::Time time; 36 | 37 | // LaserFan in 'pose' frame. Only used in the 2D case. 38 | sensor::LaserFan laser_fan; 39 | 40 | // LaserFan in 'pose' frame. Only used in the 3D case. 41 | sensor::CompressedLaserFan3D laser_fan_3d; 42 | 43 | // Trajectory this node belongs to. 44 | // TODO(jmason): The naming here is confusing because 'trajectory' doesn't 45 | // seem like a good name for a Submaps*. Sort this out. 46 | const Submaps* trajectory; 47 | 48 | // Transform from the 3D 'tracking' frame to the 'pose' frame of the 49 | // laser, which contains roll, pitch and height for 2D. In 3D this is 50 | // always identity. 51 | transform::Rigid3d tracking_to_pose; 52 | }; 53 | 54 | common::Time time() const { return constant_data->time; } 55 | 56 | const ConstantData* constant_data; 57 | 58 | transform::Rigid3d pose; 59 | }; 60 | 61 | // Users will only be interested in 'trajectory_nodes'. But 'constant_data' 62 | // is referenced by 'trajectory_nodes'. This struct guarantees that their 63 | // lifetimes are bound. 64 | struct TrajectoryNodes { 65 | std::deque constant_data; 66 | std::vector trajectory_nodes; 67 | }; 68 | 69 | } // namespace mapping 70 | } // namespace cartographer 71 | 72 | #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 73 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/global_trajectory_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ 19 | 20 | #include "cartographer/mapping/global_trajectory_builder_interface.h" 21 | #include "cartographer/mapping_2d/local_trajectory_builder.h" 22 | #include "cartographer/mapping_2d/sparse_pose_graph.h" 23 | 24 | namespace cartographer { 25 | namespace mapping_2d { 26 | 27 | class GlobalTrajectoryBuilder 28 | : public mapping::GlobalTrajectoryBuilderInterface { 29 | public: 30 | GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, 31 | SparsePoseGraph* sparse_pose_graph); 32 | ~GlobalTrajectoryBuilder() override; 33 | 34 | GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; 35 | GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; 36 | 37 | const Submaps* submaps() const override; 38 | Submaps* submaps() override; 39 | kalman_filter::PoseTracker* pose_tracker() const override; 40 | const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() 41 | const override; 42 | 43 | void AddHorizontalLaserFan(common::Time time, 44 | const sensor::LaserFan3D& laser_fan) override; 45 | void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, 46 | const Eigen::Vector3d& angular_velocity) override; 47 | void AddOdometerPose( 48 | common::Time time, const transform::Rigid3d& pose, 49 | const kalman_filter::PoseCovariance& covariance) override; 50 | void AddLaserFan3D(common::Time, const sensor::LaserFan3D&) override { 51 | LOG(FATAL) << "Not implemented."; 52 | }; 53 | 54 | private: 55 | const proto::LocalTrajectoryBuilderOptions options_; 56 | SparsePoseGraph* const sparse_pose_graph_; 57 | LocalTrajectoryBuilder local_trajectory_builder_; 58 | }; 59 | 60 | } // namespace mapping_2d 61 | } // namespace cartographer 62 | 63 | #endif // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ 64 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/laser_fan_inserter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/lua_parameter_dictionary.h" 24 | #include "cartographer/common/port.h" 25 | #include "cartographer/mapping_2d/probability_grid.h" 26 | #include "cartographer/mapping_2d/proto/laser_fan_inserter_options.pb.h" 27 | #include "cartographer/mapping_2d/xy_index.h" 28 | #include "cartographer/sensor/laser.h" 29 | #include "cartographer/sensor/point_cloud.h" 30 | 31 | namespace cartographer { 32 | namespace mapping_2d { 33 | 34 | proto::LaserFanInserterOptions CreateLaserFanInserterOptions( 35 | common::LuaParameterDictionary* parameter_dictionary); 36 | 37 | class LaserFanInserter { 38 | public: 39 | explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); 40 | 41 | LaserFanInserter(const LaserFanInserter&) = delete; 42 | LaserFanInserter& operator=(const LaserFanInserter&) = delete; 43 | 44 | // Inserts 'laser_fan' into 'probability_grid'. 45 | void Insert(const sensor::LaserFan& laser_fan, 46 | ProbabilityGrid* probability_grid) const; 47 | 48 | const std::vector& hit_table() const { return hit_table_; } 49 | const std::vector& miss_table() const { return miss_table_; } 50 | 51 | private: 52 | const proto::LaserFanInserterOptions options_; 53 | const std::vector hit_table_; 54 | const std::vector miss_table_; 55 | }; 56 | 57 | } // namespace mapping_2d 58 | } // namespace cartographer 59 | 60 | #endif // CARTOGRAPHER_MAPPING_2D_LASER_FAN_INSERTER_H_ 61 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/map_limits_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_2d/map_limits.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace mapping_2d { 23 | namespace { 24 | 25 | TEST(MapLimitsTest, ConstructAndGet) { 26 | const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); 27 | 28 | const CellLimits& cell_limits = limits.cell_limits(); 29 | EXPECT_EQ(2, cell_limits.num_x_cells); 30 | EXPECT_EQ(3, cell_limits.num_y_cells); 31 | 32 | const Eigen::Vector2d& max = limits.max(); 33 | EXPECT_EQ(3., max.x()); 34 | EXPECT_EQ(0., max.y()); 35 | 36 | EXPECT_EQ(42., limits.resolution()); 37 | } 38 | 39 | TEST(MapLimitsTest, ComputeMapLimits) { 40 | const mapping::TrajectoryNode::ConstantData constant_data{ 41 | common::FromUniversal(52), 42 | sensor::LaserFan{ 43 | Eigen::Vector2f::Zero(), 44 | {Eigen::Vector2f(-30.f, 1.f), Eigen::Vector2f(50.f, -10.f)}, 45 | {}}, 46 | Compress(sensor::LaserFan3D{Eigen::Vector3f::Zero(), {}, {}, {}}), 47 | nullptr, transform::Rigid3d::Identity()}; 48 | const mapping::TrajectoryNode trajectory_node{&constant_data, 49 | transform::Rigid3d::Identity()}; 50 | constexpr double kResolution = 0.05; 51 | const MapLimits limits = 52 | MapLimits::ComputeMapLimits(kResolution, {trajectory_node}); 53 | constexpr float kPaddingAwareTolerance = 5 * kResolution; 54 | EXPECT_NEAR(50.f, limits.max().x(), kPaddingAwareTolerance); 55 | EXPECT_NEAR(1.f, limits.max().y(), kPaddingAwareTolerance); 56 | EXPECT_LT(200, limits.cell_limits().num_x_cells); 57 | EXPECT_LT(1600, limits.cell_limits().num_y_cells); 58 | EXPECT_GT(400, limits.cell_limits().num_x_cells); 59 | EXPECT_GT(2000, limits.cell_limits().num_y_cells); 60 | } 61 | 62 | } // namespace 63 | } // namespace mapping_2d 64 | } // namespace cartographer 65 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_2d_proto_laser_fan_inserter_options 16 | SRCS 17 | laser_fan_inserter_options.proto 18 | ) 19 | 20 | google_proto_library(mapping_2d_proto_local_trajectory_builder_options 21 | SRCS 22 | local_trajectory_builder_options.proto 23 | DEPENDS 24 | kalman_filter_proto_pose_tracker_options 25 | mapping_2d_proto_submaps_options 26 | mapping_2d_scan_matching_proto_ceres_scan_matcher_options 27 | mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options 28 | mapping_3d_proto_motion_filter_options 29 | sensor_proto_adaptive_voxel_filter_options 30 | ) 31 | 32 | google_proto_library(mapping_2d_proto_submaps_options 33 | SRCS 34 | submaps_options.proto 35 | DEPENDS 36 | mapping_2d_proto_laser_fan_inserter_options 37 | ) 38 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/proto/laser_fan_inserter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.proto; 18 | 19 | message LaserFanInserterOptions { 20 | // Probability change for a hit (this will be converted to odds and therefore 21 | // must be greater than 0.5). 22 | optional double hit_probability = 1; 23 | 24 | // Probability change for a miss (this will be converted to odds and therefore 25 | // must be less than 0.5). 26 | optional double miss_probability = 2; 27 | 28 | // If 'false', free space will not change the probabilities in the occupancy 29 | // grid. 30 | optional bool insert_free_space = 3; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/proto/local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.proto; 18 | 19 | import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; 20 | import "cartographer/mapping_3d/proto/motion_filter_options.proto"; 21 | import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; 22 | import "cartographer/mapping_2d/proto/submaps_options.proto"; 23 | import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; 24 | import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; 25 | 26 | message LocalTrajectoryBuilderOptions { 27 | // Cropping parameters for the horizontal laser fan. 28 | optional float horizontal_laser_min_z = 1; 29 | optional float horizontal_laser_max_z = 2; 30 | 31 | // Voxel filter that gets applied to the horizontal laser immediately after 32 | // cropping. 33 | optional float horizontal_laser_voxel_filter_size = 3; 34 | 35 | // Whether to solve the online scan matching first using the correlative scan 36 | // matcher to generate a good starting point for Ceres. 37 | optional bool use_online_correlative_scan_matching = 5; 38 | 39 | // Voxel filter used to compute a sparser point cloud for matching. 40 | optional sensor.proto.AdaptiveVoxelFilterOptions 41 | adaptive_voxel_filter_options = 6; 42 | 43 | optional scan_matching.proto.RealTimeCorrelativeScanMatcherOptions 44 | real_time_correlative_scan_matcher_options = 7; 45 | optional scan_matching.proto.CeresScanMatcherOptions 46 | ceres_scan_matcher_options = 8; 47 | optional mapping_3d.proto.MotionFilterOptions motion_filter_options = 13; 48 | optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 10; 49 | optional mapping_2d.proto.SubmapsOptions submaps_options = 11; 50 | 51 | // True if IMU data should be expected and used. 52 | optional bool use_imu_data = 12; 53 | } 54 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/proto/submaps_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_2d/proto/laser_fan_inserter_options.proto"; 18 | 19 | package cartographer.mapping_2d.proto; 20 | 21 | message SubmapsOptions { 22 | // Resolution of the map in meters. 23 | optional double resolution = 1; 24 | 25 | // Half the width/height of each submap, its "radius". 26 | optional double half_length = 2; 27 | 28 | // Number of scans before adding a new submap. Each submap will get twice the 29 | // number of scans inserted: First for initialization without being matched 30 | // against, then while being matched. 31 | optional int32 num_laser_fans = 3; 32 | 33 | // If enabled, submap%d.png images are written for debugging. 34 | optional bool output_debug_images = 4; 35 | 36 | optional LaserFanInserterOptions laser_fan_inserter_options = 5; 37 | } 38 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/ray_casting.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/mapping_2d/map_limits.h" 23 | #include "cartographer/mapping_2d/xy_index.h" 24 | #include "cartographer/sensor/laser.h" 25 | #include "cartographer/sensor/point_cloud.h" 26 | #include "cartographer/transform/transform.h" 27 | 28 | namespace cartographer { 29 | namespace mapping_2d { 30 | 31 | // For each ray in 'laser_fan', calls 'hit_visitor' and 'miss_visitor' on the 32 | // appropriate cells. Hits are handled before misses. 33 | void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits, 34 | const std::function& hit_visitor, 35 | const std::function& miss_visitor); 36 | 37 | } // namespace mapping_2d 38 | } // namespace cartographer 39 | 40 | #endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 41 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/fast_global_localizer.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h" 18 | 19 | #include "glog/logging.h" 20 | 21 | namespace cartographer { 22 | namespace mapping_2d { 23 | namespace scan_matching { 24 | 25 | bool PerformGlobalLocalization( 26 | const float cutoff, 27 | const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, 28 | const std::vector< 29 | cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& 30 | matchers, 31 | const cartographer::sensor::PointCloud2D& point_cloud, 32 | transform::Rigid2d* const best_pose_estimate, float* const best_score) { 33 | CHECK(best_pose_estimate != nullptr) 34 | << "Need a non-null output_pose_estimate!"; 35 | CHECK(best_score != nullptr) << "Need a non-null best_score!"; 36 | *best_score = cutoff; 37 | transform::Rigid2d pose_estimate; 38 | const sensor::PointCloud2D filtered_point_cloud = 39 | voxel_filter.Filter(point_cloud); 40 | bool success = false; 41 | if (matchers.size() == 0) { 42 | LOG(WARNING) << "Map not yet large enough to localize in!"; 43 | return false; 44 | } 45 | for (auto& matcher : matchers) { 46 | float score = -1; 47 | transform::Rigid2d pose_estimate; 48 | if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, 49 | &pose_estimate)) { 50 | CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; 51 | *best_score = score; 52 | *best_pose_estimate = pose_estimate; 53 | success = true; 54 | } 55 | } 56 | return success; 57 | } 58 | 59 | } // namespace scan_matching 60 | } // namespace mapping_2d 61 | } // namespace cartographer 62 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/fast_global_localizer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ 19 | 20 | #include 21 | 22 | #include "Eigen/Geometry" 23 | #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" 24 | #include "cartographer/sensor/voxel_filter.h" 25 | 26 | namespace cartographer { 27 | namespace mapping_2d { 28 | namespace scan_matching { 29 | 30 | // Perform global localization against the provided 'matchers'. The 'cutoff' 31 | // specifies the minimum correlation that will be accepted. 32 | // This function does not take ownership of the pointers passed in 33 | // 'matchers'; they are passed as a vector of raw pointers to give maximum 34 | // flexibility to callers. 35 | // 36 | // Returns true in the case of successful localization. The output parameters 37 | // should not be trusted if the function returns false. The 'cutoff' and 38 | // 'best_score' are in the range [0.0, 1.0]. 39 | bool PerformGlobalLocalization( 40 | float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, 41 | const std::vector< 42 | cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& 43 | matchers, 44 | const cartographer::sensor::PointCloud2D& point_cloud, 45 | transform::Rigid2d* best_pose_estimate, float* best_score); 46 | 47 | } // namespace scan_matching 48 | } // namespace mapping_2d 49 | } // namespace cartographer 50 | 51 | #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ 52 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_2d_scan_matching_proto_ceres_scan_matcher_options 16 | SRCS 17 | ceres_scan_matcher_options.proto 18 | DEPENDS 19 | common_proto_ceres_solver_options 20 | ) 21 | 22 | google_proto_library(mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options 23 | SRCS 24 | fast_correlative_scan_matcher_options.proto 25 | ) 26 | 27 | google_proto_library(mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options 28 | SRCS 29 | real_time_correlative_scan_matcher_options.proto 30 | ) 31 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 10 22 | message CeresScanMatcherOptions { 23 | // Scaling parameters for each cost functor. 24 | optional double occupied_space_cost_functor_weight = 1; 25 | optional double previous_pose_translation_delta_cost_functor_weight = 2; 26 | optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; 27 | 28 | // Scale applied to the covariance estimate from Ceres. 29 | optional double covariance_scale = 4; 30 | 31 | // Configure the Ceres solver. See the Ceres documentation for more 32 | // information: https://code.google.com/p/ceres-solver/ 33 | optional common.proto.CeresSolverOptions ceres_solver_options = 9; 34 | } 35 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | message FastCorrelativeScanMatcherOptions { 20 | // Minimum linear search window in which the best possible scan alignment 21 | // will be found. 22 | optional double linear_search_window = 3; 23 | 24 | // Minimum angular search window in which the best possible scan alignment 25 | // will be found. 26 | optional double angular_search_window = 4; 27 | 28 | // Number of precomputed grids to use. 29 | optional int32 branch_and_bound_depth = 2; 30 | }; 31 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | message RealTimeCorrelativeScanMatcherOptions { 20 | // Minimum linear search window in which the best possible scan alignment 21 | // will be found. 22 | optional double linear_search_window = 1; 23 | 24 | // Minimum angular search window in which the best possible scan alignment 25 | // will be found. 26 | optional double angular_search_window = 2; 27 | 28 | // Weights applied to each part of the score. 29 | optional double translation_delta_cost_weight = 3; 30 | optional double rotation_delta_cost_weight = 4; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ 19 | 20 | #include "Eigen/Core" 21 | #include "cartographer/transform/transform.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_2d { 25 | namespace scan_matching { 26 | 27 | // Computes the cost of rotating the initial pose estimate. Cost increases with 28 | // the solution's distance from the initial estimate. 29 | class RotationDeltaCostFunctor { 30 | public: 31 | // Constructs a new RotationDeltaCostFunctor for the given 'angle'. 32 | explicit RotationDeltaCostFunctor(const double scaling_factor, 33 | const double angle) 34 | : scaling_factor_(scaling_factor), angle_(angle) {} 35 | 36 | RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; 37 | RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; 38 | 39 | template 40 | bool operator()(const T* const pose, T* residual) const { 41 | residual[0] = scaling_factor_ * (pose[2] - angle_); 42 | return true; 43 | } 44 | 45 | private: 46 | const double scaling_factor_; 47 | const double angle_; 48 | }; 49 | 50 | } // namespace scan_matching 51 | } // namespace mapping_2d 52 | } // namespace cartographer 53 | 54 | #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ 55 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 19 | 20 | #include "Eigen/Core" 21 | 22 | namespace cartographer { 23 | namespace mapping_2d { 24 | namespace scan_matching { 25 | 26 | // Computes the cost of translating the initial pose estimate. Cost increases 27 | // with the solution's distance from the initial estimate. 28 | class TranslationDeltaCostFunctor { 29 | public: 30 | // Constructs a new TranslationDeltaCostFunctor from the given 31 | // 'initial_pose_estimate' (x, y, theta). 32 | explicit TranslationDeltaCostFunctor( 33 | const double scaling_factor, 34 | const transform::Rigid2d& initial_pose_estimate) 35 | : scaling_factor_(scaling_factor), 36 | x_(initial_pose_estimate.translation().x()), 37 | y_(initial_pose_estimate.translation().y()) {} 38 | 39 | TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; 40 | TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = 41 | delete; 42 | 43 | template 44 | bool operator()(const T* const pose, T* residual) const { 45 | residual[0] = scaling_factor_ * (pose[0] - x_); 46 | residual[1] = scaling_factor_ * (pose[1] - y_); 47 | return true; 48 | } 49 | 50 | private: 51 | const double scaling_factor_; 52 | const double x_; 53 | const double y_; 54 | }; 55 | 56 | } // namespace scan_matching 57 | } // namespace mapping_2d 58 | } // namespace cartographer 59 | 60 | #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 61 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_library(mapping_2d_sparse_pose_graph_constraint_builder 16 | USES_CERES 17 | USES_EIGEN 18 | SRCS 19 | constraint_builder.cc 20 | HDRS 21 | constraint_builder.h 22 | DEPENDS 23 | common_fixed_ratio_sampler 24 | common_histogram 25 | common_make_unique 26 | common_math 27 | common_mutex 28 | common_thread_pool 29 | kalman_filter_pose_tracker 30 | mapping_2d_scan_matching_ceres_scan_matcher 31 | mapping_2d_scan_matching_fast_correlative_scan_matcher 32 | mapping_2d_scan_matching_proto_ceres_scan_matcher_options 33 | mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options 34 | mapping_2d_sparse_pose_graph_optimization_problem 35 | mapping_2d_submaps 36 | mapping_3d_scan_matching_ceres_scan_matcher 37 | mapping_3d_scan_matching_fast_correlative_scan_matcher 38 | mapping_sparse_pose_graph_proto_constraint_builder_options 39 | mapping_trajectory_connectivity 40 | sensor_point_cloud 41 | sensor_voxel_filter 42 | transform_transform 43 | ) 44 | 45 | google_library(mapping_2d_sparse_pose_graph_optimization_problem 46 | USES_CERES 47 | USES_EIGEN 48 | SRCS 49 | optimization_problem.cc 50 | HDRS 51 | optimization_problem.h 52 | DEPENDS 53 | common_ceres_solver_options 54 | common_histogram 55 | common_lua_parameter_dictionary 56 | common_math 57 | common_port 58 | mapping_2d_submaps 59 | mapping_sparse_pose_graph 60 | mapping_sparse_pose_graph_proto_optimization_problem_options 61 | transform_transform 62 | ) 63 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/submaps_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_2d/submaps.h" 18 | 19 | #include 20 | #include 21 | 22 | #include "cartographer/common/lua_parameter_dictionary.h" 23 | #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" 24 | #include "cartographer/common/port.h" 25 | #include "cartographer/transform/transform.h" 26 | #include "gmock/gmock.h" 27 | 28 | namespace cartographer { 29 | namespace mapping_2d { 30 | namespace { 31 | 32 | TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { 33 | constexpr int kNumLaserFans = 10; 34 | auto parameter_dictionary = common::MakeDictionary( 35 | "return {" 36 | "resolution = 0.05, " 37 | "half_length = 10., " 38 | "num_laser_fans = " + 39 | std::to_string(kNumLaserFans) + 40 | ", " 41 | "output_debug_images = false, " 42 | "laser_fan_inserter = {" 43 | "insert_free_space = true, " 44 | "hit_probability = 0.53, " 45 | "miss_probability = 0.495, " 46 | "}," 47 | "}"); 48 | Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; 49 | auto num_inserted = [&submaps](const int i) { 50 | return submaps.Get(i)->end_laser_fan_index - 51 | submaps.Get(i)->begin_laser_fan_index; 52 | }; 53 | for (int i = 0; i != 1000; ++i) { 54 | submaps.InsertLaserFan({Eigen::Vector2f::Zero(), {}, {}}); 55 | const int matching = submaps.matching_index(); 56 | // Except for the first, maps should only be returned after enough scans. 57 | if (matching != 0) { 58 | EXPECT_LE(kNumLaserFans, num_inserted(matching)); 59 | } 60 | } 61 | for (int i = 0; i != submaps.size() - 2; ++i) { 62 | // Submaps should not be left without the right number of scans in them. 63 | EXPECT_EQ(kNumLaserFans * 2, num_inserted(i)); 64 | EXPECT_EQ(i * kNumLaserFans, submaps.Get(i)->begin_laser_fan_index); 65 | EXPECT_EQ((i + 2) * kNumLaserFans, submaps.Get(i)->end_laser_fan_index); 66 | } 67 | } 68 | 69 | } // namespace 70 | } // namespace mapping_2d 71 | } // namespace cartographer 72 | -------------------------------------------------------------------------------- /cartographer/mapping_2d/xy_index_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_2d/xy_index.h" 18 | 19 | #include "gtest/gtest.h" 20 | 21 | namespace cartographer { 22 | namespace mapping_2d { 23 | namespace { 24 | 25 | TEST(XYIndexTest, XYIndexRangeIterator) { 26 | const Eigen::Array2i min(1, 2); 27 | const Eigen::Array2i max(3, 4); 28 | XYIndexRangeIterator it(min, max); 29 | EXPECT_TRUE((min == *it.begin()).all()) << *it.begin(); 30 | EXPECT_TRUE((Eigen::Array2i(1, 5) == *it.end()).all()) << *it.end(); 31 | EXPECT_TRUE((min == *it).all()) << *it; 32 | int num_indices = 0; 33 | for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(min, max)) { 34 | LOG(INFO) << xy_index; 35 | EXPECT_TRUE((xy_index >= min).all()); 36 | EXPECT_TRUE((xy_index <= max).all()); 37 | ++num_indices; 38 | } 39 | EXPECT_EQ(9, num_indices); 40 | } 41 | 42 | } // namespace 43 | } // namespace mapping_2d 44 | } // namespace cartographer 45 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/ceres_pose.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/ceres_pose.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | CeresPose::CeresPose( 23 | const transform::Rigid3d& rigid, 24 | std::unique_ptr rotation_parametrization, 25 | ceres::Problem* problem) 26 | : translation_({{rigid.translation().x(), rigid.translation().y(), 27 | rigid.translation().z()}}), 28 | rotation_({{rigid.rotation().w(), rigid.rotation().x(), 29 | rigid.rotation().y(), rigid.rotation().z()}}) { 30 | problem->AddParameterBlock(translation_.data(), 3); 31 | problem->AddParameterBlock(rotation_.data(), 4, 32 | rotation_parametrization.release()); 33 | } 34 | 35 | const transform::Rigid3d CeresPose::ToRigid() const { 36 | return transform::Rigid3d( 37 | Eigen::Map(translation_.data()), 38 | Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], 39 | rotation_[3])); 40 | } 41 | 42 | } // namespace mapping_3d 43 | } // namespace cartographer 44 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/ceres_pose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "Eigen/Core" 24 | #include "cartographer/transform/rigid_transform.h" 25 | #include "ceres/ceres.h" 26 | 27 | namespace cartographer { 28 | namespace mapping_3d { 29 | 30 | class CeresPose { 31 | public: 32 | CeresPose( 33 | const transform::Rigid3d& rigid, 34 | std::unique_ptr rotation_parametrization, 35 | ceres::Problem* problem); 36 | 37 | CeresPose(const CeresPose&) = delete; 38 | CeresPose& operator=(const CeresPose&) = delete; 39 | 40 | const transform::Rigid3d ToRigid() const; 41 | 42 | double* translation() { return translation_.data(); } 43 | double* rotation() { return rotation_.data(); } 44 | 45 | private: 46 | std::array translation_; 47 | // Rotation quaternion as (w, x, y, z). 48 | std::array rotation_; 49 | }; 50 | 51 | } // namespace mapping_3d 52 | } // namespace cartographer 53 | 54 | #endif // CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 55 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/imu_integration.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/imu_integration.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | IntegrateImuResult IntegrateImu( 23 | const std::deque& imu_data, const common::Time start_time, 24 | const common::Time end_time, std::deque::const_iterator* it) { 25 | return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), 26 | Eigen::Affine3d::Identity(), start_time, end_time, 27 | it); 28 | } 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/kalman_local_trajectory_builder_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h" 18 | 19 | #include "cartographer/kalman_filter/pose_tracker.h" 20 | #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" 21 | 22 | namespace cartographer { 23 | namespace mapping_3d { 24 | 25 | proto::KalmanLocalTrajectoryBuilderOptions 26 | CreateKalmanLocalTrajectoryBuilderOptions( 27 | common::LuaParameterDictionary* const parameter_dictionary) { 28 | proto::KalmanLocalTrajectoryBuilderOptions options; 29 | options.set_use_online_correlative_scan_matching( 30 | parameter_dictionary->GetBool("use_online_correlative_scan_matching")); 31 | *options.mutable_real_time_correlative_scan_matcher_options() = 32 | mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( 33 | parameter_dictionary 34 | ->GetDictionary("real_time_correlative_scan_matcher") 35 | .get()); 36 | *options.mutable_pose_tracker_options() = 37 | kalman_filter::CreatePoseTrackerOptions( 38 | parameter_dictionary->GetDictionary("pose_tracker").get()); 39 | return options; 40 | } 41 | 42 | } // namespace mapping_3d 43 | } // namespace cartographer 44 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/kalman_local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::KalmanLocalTrajectoryBuilderOptions 27 | CreateKalmanLocalTrajectoryBuilderOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | 33 | #endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 34 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/laser_fan_inserter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ 19 | 20 | #include "cartographer/mapping_3d/hybrid_grid.h" 21 | #include "cartographer/mapping_3d/proto/laser_fan_inserter_options.pb.h" 22 | #include "cartographer/sensor/laser.h" 23 | #include "cartographer/sensor/point_cloud.h" 24 | 25 | namespace cartographer { 26 | namespace mapping_3d { 27 | 28 | proto::LaserFanInserterOptions CreateLaserFanInserterOptions( 29 | common::LuaParameterDictionary* parameter_dictionary); 30 | 31 | class LaserFanInserter { 32 | public: 33 | explicit LaserFanInserter(const proto::LaserFanInserterOptions& options); 34 | 35 | LaserFanInserter(const LaserFanInserter&) = delete; 36 | LaserFanInserter& operator=(const LaserFanInserter&) = delete; 37 | 38 | // Inserts 'laser_fan' into 'hybrid_grid'. 39 | void Insert(const sensor::LaserFan3D& laser_fan, 40 | HybridGrid* hybrid_grid) const; 41 | 42 | private: 43 | const proto::LaserFanInserterOptions options_; 44 | const std::vector hit_table_; 45 | const std::vector miss_table_; 46 | }; 47 | 48 | } // namespace mapping_3d 49 | } // namespace cartographer 50 | 51 | #endif // CARTOGRAPHER_MAPPING_3D_LASER_FAN_INSERTER_H_ 52 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/local_trajectory_builder.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/local_trajectory_builder.h" 18 | 19 | #include "cartographer/common/make_unique.h" 20 | #include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" 21 | #include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | std::unique_ptr CreateLocalTrajectoryBuilder( 27 | const proto::LocalTrajectoryBuilderOptions& 28 | local_trajectory_builder_options) { 29 | switch (local_trajectory_builder_options.use()) { 30 | case proto::LocalTrajectoryBuilderOptions::KALMAN: 31 | return common::make_unique( 32 | local_trajectory_builder_options); 33 | case proto::LocalTrajectoryBuilderOptions::OPTIMIZING: 34 | return common::make_unique( 35 | local_trajectory_builder_options); 36 | } 37 | LOG(FATAL); 38 | } 39 | 40 | } // namespace mapping_3d 41 | } // namespace cartographer 42 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/local_trajectory_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/mapping_3d/local_trajectory_builder_interface.h" 24 | #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" 25 | 26 | namespace cartographer { 27 | namespace mapping_3d { 28 | 29 | std::unique_ptr CreateLocalTrajectoryBuilder( 30 | const proto::LocalTrajectoryBuilderOptions& 31 | local_trajectory_builder_options); 32 | 33 | } // namespace mapping_3d 34 | } // namespace cartographer 35 | 36 | #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 37 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( 27 | common::LuaParameterDictionary* parameter_dictionary); 28 | 29 | } // namespace mapping_3d 30 | } // namespace cartographer 31 | 32 | #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 33 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/motion_filter.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/motion_filter.h" 18 | 19 | #include "cartographer/transform/transform.h" 20 | #include "glog/logging.h" 21 | 22 | namespace cartographer { 23 | namespace mapping_3d { 24 | 25 | proto::MotionFilterOptions CreateMotionFilterOptions( 26 | common::LuaParameterDictionary* const parameter_dictionary) { 27 | proto::MotionFilterOptions options; 28 | options.set_max_time_seconds( 29 | parameter_dictionary->GetDouble("max_time_seconds")); 30 | options.set_max_distance_meters( 31 | parameter_dictionary->GetDouble("max_distance_meters")); 32 | options.set_max_angle_radians( 33 | parameter_dictionary->GetDouble("max_angle_radians")); 34 | return options; 35 | } 36 | 37 | MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) 38 | : options_(options) {} 39 | 40 | bool MotionFilter::IsSimilar(const common::Time time, 41 | const transform::Rigid3d& pose) { 42 | LOG_EVERY_N(INFO, 500) << "Motion filter reduced the number of scans to " 43 | << 100. * num_different_ / num_total_ << "%."; 44 | ++num_total_; 45 | if (num_total_ > 1 && 46 | time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) && 47 | (pose.translation() - last_pose_.translation()).norm() <= 48 | options_.max_distance_meters() && 49 | transform::GetAngle(pose.inverse() * last_pose_) <= 50 | options_.max_angle_radians()) { 51 | return true; 52 | } 53 | last_time_ = time; 54 | last_pose_ = pose; 55 | ++num_different_; 56 | return false; 57 | } 58 | 59 | } // namespace mapping_3d 60 | } // namespace cartographer 61 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/motion_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/common/lua_parameter_dictionary.h" 23 | #include "cartographer/common/time.h" 24 | #include "cartographer/mapping_3d/proto/motion_filter_options.pb.h" 25 | #include "cartographer/transform/rigid_transform.h" 26 | 27 | namespace cartographer { 28 | namespace mapping_3d { 29 | 30 | proto::MotionFilterOptions CreateMotionFilterOptions( 31 | common::LuaParameterDictionary* parameter_dictionary); 32 | 33 | // Takes poses as input and filters them to get fewer poses. 34 | class MotionFilter { 35 | public: 36 | explicit MotionFilter(const proto::MotionFilterOptions& options); 37 | 38 | // If the accumulated motion (linear, rotational, or time) is above the 39 | // threshold, returns false. Otherwise the relative motion is accumulated and 40 | // true is returned. 41 | bool IsSimilar(common::Time time, const transform::Rigid3d& pose); 42 | 43 | private: 44 | const proto::MotionFilterOptions options_; 45 | int num_total_ = 0; 46 | int num_different_ = 0; 47 | common::Time last_time_; 48 | transform::Rigid3d last_pose_; 49 | }; 50 | 51 | } // namespace mapping_3d 52 | } // namespace cartographer 53 | 54 | #endif // CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ 55 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/optimizing_local_trajectory_builder_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | proto::OptimizingLocalTrajectoryBuilderOptions 23 | CreateOptimizingLocalTrajectoryBuilderOptions( 24 | common::LuaParameterDictionary* const parameter_dictionary) { 25 | proto::OptimizingLocalTrajectoryBuilderOptions options; 26 | options.set_high_resolution_grid_scale( 27 | parameter_dictionary->GetDouble("high_resolution_grid_scale")); 28 | options.set_low_resolution_grid_scale( 29 | parameter_dictionary->GetDouble("low_resolution_grid_scale")); 30 | options.set_velocity_scale(parameter_dictionary->GetDouble("velocity_scale")); 31 | options.set_translation_scale( 32 | parameter_dictionary->GetDouble("translation_scale")); 33 | options.set_rotation_scale(parameter_dictionary->GetDouble("rotation_scale")); 34 | options.set_odometry_translation_scale( 35 | parameter_dictionary->GetDouble("odometry_translation_scale")); 36 | options.set_odometry_rotation_scale( 37 | parameter_dictionary->GetDouble("odometry_rotation_scale")); 38 | return options; 39 | } 40 | 41 | } // namespace mapping_3d 42 | } // namespace cartographer 43 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::OptimizingLocalTrajectoryBuilderOptions 27 | CreateOptimizingLocalTrajectoryBuilderOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | 33 | #endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 34 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_3d_proto_kalman_local_trajectory_builder_options 16 | SRCS 17 | kalman_local_trajectory_builder_options.proto 18 | DEPENDS 19 | kalman_filter_proto_pose_tracker_options 20 | mapping_2d_scan_matching_proto_real_time_correlative_scan_matcher_options 21 | ) 22 | 23 | google_proto_library(mapping_3d_proto_laser_fan_inserter_options 24 | SRCS 25 | laser_fan_inserter_options.proto 26 | ) 27 | 28 | google_proto_library(mapping_3d_proto_local_trajectory_builder_options 29 | SRCS 30 | local_trajectory_builder_options.proto 31 | DEPENDS 32 | mapping_3d_proto_kalman_local_trajectory_builder_options 33 | mapping_3d_proto_motion_filter_options 34 | mapping_3d_proto_optimizing_local_trajectory_builder_options 35 | mapping_3d_proto_submaps_options 36 | mapping_3d_scan_matching_proto_ceres_scan_matcher_options 37 | sensor_proto_adaptive_voxel_filter_options 38 | ) 39 | 40 | google_proto_library(mapping_3d_proto_motion_filter_options 41 | SRCS 42 | motion_filter_options.proto 43 | ) 44 | 45 | google_proto_library(mapping_3d_proto_optimizing_local_trajectory_builder_options 46 | SRCS 47 | optimizing_local_trajectory_builder_options.proto 48 | ) 49 | 50 | google_proto_library(mapping_3d_proto_submaps_options 51 | SRCS 52 | submaps_options.proto 53 | DEPENDS 54 | mapping_3d_proto_laser_fan_inserter_options 55 | ) 56 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; 18 | import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; 19 | 20 | package cartographer.mapping_3d.proto; 21 | 22 | message KalmanLocalTrajectoryBuilderOptions { 23 | // Whether to solve the online scan matching first using the correlative scan 24 | // matcher to generate a good starting point for Ceres. 25 | optional bool use_online_correlative_scan_matching = 1; 26 | 27 | optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions 28 | real_time_correlative_scan_matcher_options = 2; 29 | optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; 30 | } 31 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/laser_fan_inserter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message LaserFanInserterOptions { 20 | // Probability change for a hit (this will be converted to odds and therefore 21 | // must be greater than 0.5). 22 | optional double hit_probability = 1; 23 | 24 | // Probability change for a miss (this will be converted to odds and therefore 25 | // must be less than 0.5). 26 | optional double miss_probability = 2; 27 | 28 | // Up to how many free space voxels are updated for scan matching. 29 | // 0 disables free space. 30 | optional int32 num_free_space_voxels = 3; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_3d/proto/motion_filter_options.proto"; 18 | import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; 19 | import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto"; 20 | import "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto"; 21 | import "cartographer/mapping_3d/proto/submaps_options.proto"; 22 | import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; 23 | 24 | package cartographer.mapping_3d.proto; 25 | 26 | message LocalTrajectoryBuilderOptions { 27 | enum Use { 28 | KALMAN = 0; 29 | OPTIMIZING = 1; 30 | } 31 | 32 | // Laser limits. 33 | optional float laser_min_range = 1; 34 | optional float laser_max_range = 2; 35 | 36 | // Number of scans to accumulate into one unwarped, combined scan to use for 37 | // scan matching. 38 | optional int32 scans_per_accumulation = 3; 39 | 40 | // Voxel filter that gets applied to the laser before doing anything with it. 41 | optional float laser_voxel_filter_size = 4; 42 | 43 | // Voxel filter used to compute a sparser point cloud for matching. 44 | optional sensor.proto.AdaptiveVoxelFilterOptions 45 | high_resolution_adaptive_voxel_filter_options = 5; 46 | optional sensor.proto.AdaptiveVoxelFilterOptions 47 | low_resolution_adaptive_voxel_filter_options = 12; 48 | 49 | optional scan_matching.proto.CeresScanMatcherOptions 50 | ceres_scan_matcher_options = 6; 51 | optional MotionFilterOptions motion_filter_options = 7; 52 | optional SubmapsOptions submaps_options = 8; 53 | 54 | // Which one of the implementation to instantiate and use. 55 | optional Use use = 9; 56 | optional KalmanLocalTrajectoryBuilderOptions 57 | kalman_local_trajectory_builder_options = 10; 58 | optional OptimizingLocalTrajectoryBuilderOptions 59 | optimizing_local_trajectory_builder_options = 11; 60 | } 61 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/motion_filter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message MotionFilterOptions { 20 | // Threshold above which a new scan is inserted based on time. 21 | optional double max_time_seconds = 1; 22 | 23 | // Threshold above which a new scan is inserted based on linear motion. 24 | optional double max_distance_meters = 2; 25 | 26 | // Threshold above which a new scan is inserted based on rotational motion. 27 | optional double max_angle_radians = 3; 28 | } 29 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message OptimizingLocalTrajectoryBuilderOptions { 20 | optional double high_resolution_grid_scale = 6; 21 | optional double low_resolution_grid_scale = 7; 22 | optional double velocity_scale = 1; 23 | optional double translation_scale = 2; 24 | optional double rotation_scale = 3; 25 | optional double odometry_translation_scale = 4; 26 | optional double odometry_rotation_scale = 5; 27 | } 28 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/proto/submaps_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_3d/proto/laser_fan_inserter_options.proto"; 18 | 19 | package cartographer.mapping_3d.proto; 20 | 21 | message SubmapsOptions { 22 | // Resolution of the 'high_resolution' map in meters used for local SLAM and 23 | // loop closure. 24 | optional double high_resolution = 1; 25 | 26 | // Maximum range to filter the point cloud to before insertion into the 27 | // 'high_resolution' map. 28 | optional double high_resolution_max_range = 4; 29 | 30 | // Resolution of the 'low_resolution' version of the map in meters used for 31 | // local SLAM only. 32 | optional double low_resolution = 5; 33 | 34 | // Number of scans before adding a new submap. Each submap will get twice the 35 | // number of scans inserted: First for initialization without being matched 36 | // against, then while being matched. 37 | optional int32 num_laser_fans = 2; 38 | 39 | optional LaserFanInserterOptions laser_fan_inserter_options = 3; 40 | } 41 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/rotation_cost_function.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ 19 | 20 | #include "Eigen/Core" 21 | #include "Eigen/Geometry" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | // Penalizes differences between IMU data and optimized orientations. 27 | class RotationCostFunction { 28 | public: 29 | RotationCostFunction(const double scaling_factor, 30 | const Eigen::Quaterniond& delta_rotation_imu_frame) 31 | : scaling_factor_(scaling_factor), 32 | delta_rotation_imu_frame_(delta_rotation_imu_frame) {} 33 | 34 | RotationCostFunction(const RotationCostFunction&) = delete; 35 | RotationCostFunction& operator=(const RotationCostFunction&) = delete; 36 | 37 | template 38 | bool operator()(const T* const start_rotation, const T* const end_rotation, 39 | T* residual) const { 40 | const Eigen::Quaternion start(start_rotation[0], start_rotation[1], 41 | start_rotation[2], start_rotation[3]); 42 | const Eigen::Quaternion end(end_rotation[0], end_rotation[1], 43 | end_rotation[2], end_rotation[3]); 44 | const Eigen::Quaternion error = 45 | end.conjugate() * start * delta_rotation_imu_frame_.cast(); 46 | residual[0] = scaling_factor_ * error.x(); 47 | residual[1] = scaling_factor_ * error.y(); 48 | residual[2] = scaling_factor_ * error.z(); 49 | return true; 50 | } 51 | 52 | private: 53 | const double scaling_factor_; 54 | const Eigen::Quaterniond delta_rotation_imu_frame_; 55 | }; 56 | 57 | } // namespace mapping_3d 58 | } // namespace cartographer 59 | 60 | #endif // CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ 61 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/precomputation_grid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ 19 | 20 | #include "cartographer/mapping_3d/hybrid_grid.h" 21 | 22 | namespace cartographer { 23 | namespace mapping_3d { 24 | namespace scan_matching { 25 | 26 | class PrecomputationGrid : public HybridGridBase { 27 | public: 28 | PrecomputationGrid(const float resolution, const Eigen::Vector3f& origin) 29 | : HybridGridBase(resolution, origin) {} 30 | 31 | // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. 32 | static float ToProbability(float value) { 33 | return mapping::kMinProbability + 34 | value * 35 | ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); 36 | } 37 | }; 38 | 39 | // Converts a HybridGrid to a PrecomputationGrid representing the same data, 40 | // but only using 8 bit instead of 2 x 16 bit. 41 | PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); 42 | 43 | // Returns a grid of the same resolution containing the maximum value of 44 | // original voxels in 'grid'. This maximum is over the 8 voxels that have 45 | // any combination of index components optionally increased by 'shift'. 46 | // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this 47 | // is using the precomputed grid of one depth before, this results in 48 | // precomputation grids analogous to the 2D case. 49 | PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, 50 | bool half_resolution, 51 | const Eigen::Array3i& shift); 52 | 53 | } // namespace scan_matching 54 | } // namespace mapping_3d 55 | } // namespace cartographer 56 | 57 | #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ 58 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(mapping_3d_scan_matching_proto_ceres_scan_matcher_options 16 | SRCS 17 | ceres_scan_matcher_options.proto 18 | DEPENDS 19 | common_proto_ceres_solver_options 20 | ) 21 | 22 | google_proto_library(mapping_3d_scan_matching_proto_fast_correlative_scan_matcher_options 23 | SRCS 24 | fast_correlative_scan_matcher_options.proto 25 | ) 26 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.scan_matching.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 7 22 | message CeresScanMatcherOptions { 23 | // Scaling parameters for each cost functor. 24 | repeated double occupied_space_cost_functor_weight = 1; 25 | optional double previous_pose_translation_delta_cost_functor_weight = 2; 26 | optional double initial_pose_estimate_rotation_delta_cost_functor_weight = 3; 27 | 28 | // Scale applied to the covariance estimate from Ceres. 29 | optional double covariance_scale = 4; 30 | 31 | // Whether only to allow changes to yaw, keeping roll/pitch constant. 32 | optional bool only_optimize_yaw = 5; 33 | 34 | // Configure the Ceres solver. See the Ceres documentation for more 35 | // information: https://code.google.com/p/ceres-solver/ 36 | optional common.proto.CeresSolverOptions ceres_solver_options = 6; 37 | } 38 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.scan_matching.proto; 18 | 19 | message FastCorrelativeScanMatcherOptions { 20 | // Number of precomputed grids to use. 21 | optional int32 branch_and_bound_depth = 2; 22 | 23 | // Number of full resolution grids to use, additional grids will reduce the 24 | // resolution by half each. 25 | optional int32 full_resolution_depth = 8; 26 | 27 | // Number of histogram buckets for the rotational scan matcher. 28 | optional int32 rotational_histogram_size = 3; 29 | 30 | // Minimum score for the rotational scan matcher. 31 | optional double min_rotational_score = 4; 32 | 33 | // Linear search window in the plane orthogonal to gravity in which the best 34 | // possible scan alignment will be found. 35 | optional double linear_xy_search_window = 5; 36 | 37 | // Linear search window in the gravity direction in which the best possible 38 | // scan alignment will be found. 39 | optional double linear_z_search_window = 6; 40 | 41 | // Minimum angular search window in which the best possible scan alignment 42 | // will be found. 43 | optional double angular_search_window = 7; 44 | } 45 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ 19 | 20 | #include 21 | 22 | #include "Eigen/Geometry" 23 | #include "cartographer/mapping/trajectory_node.h" 24 | #include "cartographer/sensor/point_cloud.h" 25 | 26 | namespace cartographer { 27 | namespace mapping_3d { 28 | namespace scan_matching { 29 | 30 | class RotationalScanMatcher { 31 | public: 32 | explicit RotationalScanMatcher( 33 | const std::vector& nodes, int histogram_size); 34 | 35 | RotationalScanMatcher(const RotationalScanMatcher&) = delete; 36 | RotationalScanMatcher& operator=(const RotationalScanMatcher&) = delete; 37 | 38 | // Scores how well a 'point_cloud' can be understood as rotated by certain 39 | // 'angles' relative to the 'nodes'. Each angle results in a score between 40 | // 0 (worst) and 1 (best). 41 | std::vector Match(const sensor::PointCloud& point_cloud, 42 | const std::vector& angles) const; 43 | 44 | private: 45 | float MatchHistogram(const Eigen::VectorXf& scan_histogram) const; 46 | 47 | Eigen::VectorXf histogram_; 48 | }; 49 | 50 | } // namespace scan_matching 51 | } // namespace mapping_3d 52 | } // namespace cartographer 53 | 54 | #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ 55 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 19 | 20 | #include "Eigen/Core" 21 | #include "cartographer/transform/rigid_transform.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | namespace scan_matching { 26 | 27 | // Computes the cost of translating the initial pose estimate. Cost increases 28 | // with the solution's distance from the initial estimate. 29 | class TranslationDeltaCostFunctor { 30 | public: 31 | // Constructs a new TranslationDeltaCostFunctor from the given 32 | // 'initial_pose_estimate'. 33 | explicit TranslationDeltaCostFunctor( 34 | const double scaling_factor, 35 | const transform::Rigid3d& initial_pose_estimate) 36 | : scaling_factor_(scaling_factor), 37 | x_(initial_pose_estimate.translation().x()), 38 | y_(initial_pose_estimate.translation().y()), 39 | z_(initial_pose_estimate.translation().z()) {} 40 | 41 | TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; 42 | TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = 43 | delete; 44 | 45 | template 46 | bool operator()(const T* const translation, T* residual) const { 47 | residual[0] = scaling_factor_ * (translation[0] - x_); 48 | residual[1] = scaling_factor_ * (translation[1] - y_); 49 | residual[2] = scaling_factor_ * (translation[2] - z_); 50 | return true; 51 | } 52 | 53 | private: 54 | const double scaling_factor_; 55 | const double x_; 56 | const double y_; 57 | const double z_; 58 | }; 59 | 60 | } // namespace scan_matching 61 | } // namespace mapping_3d 62 | } // namespace cartographer 63 | 64 | #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ 65 | -------------------------------------------------------------------------------- /cartographer/mapping_3d/translation_cost_function.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ 19 | 20 | #include "Eigen/Core" 21 | #include "Eigen/Geometry" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | // Penalizes differences between velocity and change in position. 27 | class TranslationCostFunction { 28 | public: 29 | TranslationCostFunction(const double scaling_factor, 30 | const double delta_time_seconds) 31 | : scaling_factor_(scaling_factor), 32 | delta_time_seconds_(delta_time_seconds) {} 33 | 34 | TranslationCostFunction(const TranslationCostFunction&) = delete; 35 | TranslationCostFunction& operator=(const TranslationCostFunction&) = delete; 36 | 37 | template 38 | bool operator()(const T* const start_translation, 39 | const T* const end_translation, const T* const velocity, 40 | T* residual) const { 41 | const T delta_x = end_translation[0] - start_translation[0]; 42 | const T delta_y = end_translation[1] - start_translation[1]; 43 | const T delta_z = end_translation[2] - start_translation[2]; 44 | 45 | residual[0] = 46 | scaling_factor_ * (delta_x - velocity[0] * delta_time_seconds_); 47 | residual[1] = 48 | scaling_factor_ * (delta_y - velocity[1] * delta_time_seconds_); 49 | residual[2] = 50 | scaling_factor_ * (delta_z - velocity[2] * delta_time_seconds_); 51 | return true; 52 | } 53 | 54 | private: 55 | const double scaling_factor_; 56 | const double delta_time_seconds_; 57 | }; 58 | 59 | } // namespace mapping_3d 60 | } // namespace cartographer 61 | 62 | #endif // CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ 63 | -------------------------------------------------------------------------------- /cartographer/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(proto_trajectory 16 | SRCS 17 | trajectory.proto 18 | DEPENDS 19 | transform_proto_transform 20 | ) 21 | -------------------------------------------------------------------------------- /cartographer/proto/trajectory.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/transform/proto/transform.proto"; 18 | 19 | package cartographer.proto; 20 | 21 | option java_outer_classname = "TrajectoryOuterClass"; 22 | 23 | message Trajectory { 24 | // NEXT_ID: 7 25 | message Node { 26 | optional int64 timestamp = 1; 27 | // 2D Pose in map frame. 28 | optional transform.proto.Rigid2d pose_2d = 2; 29 | // Transform from tracking to map frame. 30 | optional transform.proto.Rigid3d pose = 5; 31 | } 32 | 33 | // Time-ordered sequence of Nodes. 34 | repeated Node node = 1; 35 | } 36 | -------------------------------------------------------------------------------- /cartographer/sensor/configuration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 18 | #define CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 19 | 20 | #include "Eigen/Geometry" 21 | #include "cartographer/common/lua_parameter_dictionary.h" 22 | #include "cartographer/sensor/proto/configuration.pb.h" 23 | #include "cartographer/transform/rigid_transform.h" 24 | 25 | namespace cartographer { 26 | namespace sensor { 27 | 28 | // Creates the configuration for a singular sensor from 'parameter_dictionary'. 29 | proto::Configuration::Sensor CreateSensorConfiguration( 30 | common::LuaParameterDictionary* parameter_dictionary); 31 | 32 | // Creates the mapping from frame_id to Sensors defined in 33 | // 'parameter_dictionary'. 34 | proto::Configuration CreateConfiguration( 35 | common::LuaParameterDictionary* parameter_dictionary); 36 | 37 | // Returns true if 'frame_id' is mentioned in 'sensor_configuration'. 38 | bool IsEnabled(const string& frame_id, 39 | const sensor::proto::Configuration& sensor_configuration); 40 | 41 | // Returns the transform which takes data from the sensor frame to the 42 | // tracking frame. 43 | transform::Rigid3d GetTransformToTracking( 44 | const string& frame_id, 45 | const sensor::proto::Configuration& sensor_configuration); 46 | 47 | } // namespace sensor 48 | } // namespace cartographer 49 | 50 | #endif // CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 51 | -------------------------------------------------------------------------------- /cartographer/sensor/point_cloud.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 18 | #define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 19 | 20 | #include 21 | 22 | #include "Eigen/Core" 23 | #include "Eigen/Geometry" 24 | #include "cartographer/sensor/proto/sensor.pb.h" 25 | #include "cartographer/transform/rigid_transform.h" 26 | #include "glog/logging.h" 27 | 28 | namespace cartographer { 29 | namespace sensor { 30 | 31 | typedef std::vector PointCloud; 32 | typedef std::vector PointCloud2D; 33 | 34 | // Transforms 'point_cloud' according to 'transform'. 35 | PointCloud TransformPointCloud(const PointCloud& point_cloud, 36 | const transform::Rigid3f& transform); 37 | 38 | // Transforms 'point_cloud_2d' according to 'transform'. 39 | PointCloud2D TransformPointCloud2D(const PointCloud2D& point_cloud_2d, 40 | const transform::Rigid2f& transform); 41 | 42 | // Converts 'point_cloud_2d' to a 3D point cloud. 43 | PointCloud ToPointCloud(const PointCloud2D& point_cloud_2d); 44 | 45 | // Converts 'point_cloud' to a 2D point cloud by removing the z component. 46 | PointCloud2D ProjectToPointCloud2D(const PointCloud& point_cloud); 47 | 48 | // Returns a new point cloud without points that fall outside the axis-aligned 49 | // cuboid defined by 'min' and 'max'. 50 | PointCloud Crop(const PointCloud& point_cloud, const Eigen::Vector3f& min, 51 | const Eigen::Vector3f& max); 52 | 53 | // Converts 'point_cloud' to a proto::PointCloud. 54 | proto::PointCloud ToProto(const PointCloud& point_cloud); 55 | 56 | // Converts 'proto' to a PointCloud. 57 | PointCloud ToPointCloud(const proto::PointCloud& proto); 58 | 59 | } // namespace sensor 60 | } // namespace cartographer 61 | 62 | #endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 63 | -------------------------------------------------------------------------------- /cartographer/sensor/point_cloud_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/sensor/point_cloud.h" 18 | 19 | #include 20 | 21 | #include "gtest/gtest.h" 22 | 23 | namespace cartographer { 24 | namespace sensor { 25 | namespace { 26 | 27 | TEST(PointCloudTest, TransformPointCloud2D) { 28 | PointCloud2D point_cloud; 29 | point_cloud.emplace_back(0.5f, 0.5f); 30 | point_cloud.emplace_back(3.5f, 0.5f); 31 | const PointCloud2D transformed_point_cloud = 32 | TransformPointCloud2D(point_cloud, transform::Rigid2f::Rotation(M_PI_2)); 33 | EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); 34 | EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); 35 | EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); 36 | EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); 37 | } 38 | 39 | } // namespace 40 | } // namespace sensor 41 | } // namespace cartographer 42 | -------------------------------------------------------------------------------- /cartographer/sensor/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(sensor_proto_adaptive_voxel_filter_options 16 | SRCS 17 | adaptive_voxel_filter_options.proto 18 | ) 19 | 20 | google_proto_library(sensor_proto_configuration 21 | SRCS 22 | configuration.proto 23 | DEPENDS 24 | transform_proto_transform 25 | ) 26 | 27 | google_proto_library(sensor_proto_sensor 28 | SRCS 29 | sensor.proto 30 | DEPENDS 31 | transform_proto_transform 32 | ) 33 | -------------------------------------------------------------------------------- /cartographer/sensor/proto/adaptive_voxel_filter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.sensor.proto; 18 | 19 | message AdaptiveVoxelFilterOptions { 20 | // 'max_length' of a voxel edge. 21 | optional float max_length = 1; 22 | 23 | // If there are more points and not at least 'min_num_points' remain, the 24 | // voxel length is reduced trying to get this minimum number of points. 25 | optional float min_num_points = 2; 26 | 27 | // Points further away from the origin are removed. 28 | optional float max_range = 3; 29 | }; 30 | -------------------------------------------------------------------------------- /cartographer/sensor/proto/configuration.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.sensor.proto; 18 | 19 | import "cartographer/transform/proto/transform.proto"; 20 | 21 | // NEXT_ID: 16 22 | message Configuration { 23 | // NEXT_ID: 4 24 | message Sensor { 25 | // 'frame_id' of the sensor. 26 | optional string frame_id = 2; 27 | 28 | // Transformation from 'frame_id' to the tracking frame. 29 | optional transform.proto.Rigid3d transform = 3; 30 | } 31 | 32 | repeated Sensor sensor = 15; 33 | } 34 | -------------------------------------------------------------------------------- /cartographer/sensor/sensor_packet_period_histogram_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ 18 | #define CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/histogram.h" 24 | #include "cartographer/common/port.h" 25 | 26 | namespace cartographer { 27 | namespace sensor { 28 | 29 | class SensorPacketPeriodHistogramBuilder { 30 | public: 31 | void Add(int trajectory_id, int64 timestamp, const string& frame_id); 32 | void LogHistogramsAndClear(); 33 | 34 | private: 35 | using Key = std::pair; 36 | 37 | std::map last_timestamps_; 38 | std::unordered_map> 39 | histograms_; 40 | }; 41 | 42 | } // namespace sensor 43 | } // namespace cartographer 44 | 45 | #endif // CARTOGRAPHER_SENSOR_SENSOR_PACKET_PERIOD_HISTOGRAM_BUILDER_H_ 46 | -------------------------------------------------------------------------------- /cartographer/sensor/voxel_filter_test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/sensor/voxel_filter.h" 18 | 19 | #include 20 | 21 | #include "gmock/gmock.h" 22 | 23 | namespace cartographer { 24 | namespace sensor { 25 | namespace { 26 | 27 | using ::testing::ContainerEq; 28 | 29 | TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { 30 | PointCloud2D point_cloud = { 31 | {0.f, 0.f}, {0.1f, -0.1f}, {0.3f, -0.1f}, {0.f, 0.f}}; 32 | EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f), 33 | ContainerEq(PointCloud2D{point_cloud[0], point_cloud[2]})); 34 | } 35 | 36 | TEST(VoxelFilter3DTest, ReturnsTheFirstPointInEachVoxel) { 37 | PointCloud point_cloud = {{0.f, 0.f, 0.f}, 38 | {0.1f, -0.1f, 0.1f}, 39 | {0.3f, -0.1f, 0.f}, 40 | {0.f, 0.f, 0.1f}}; 41 | EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f), 42 | ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); 43 | } 44 | 45 | } // namespace 46 | } // namespace sensor 47 | } // namespace cartographer 48 | -------------------------------------------------------------------------------- /cartographer/transform/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | add_subdirectory("proto") 16 | 17 | google_library(transform_rigid_transform 18 | USES_CERES 19 | USES_EIGEN 20 | SRCS 21 | rigid_transform.cc 22 | HDRS 23 | rigid_transform.h 24 | DEPENDS 25 | common_lua_parameter_dictionary 26 | common_math 27 | common_port 28 | ) 29 | 30 | google_library(transform_rigid_transform_test_helpers 31 | USES_EIGEN 32 | HDRS 33 | rigid_transform_test_helpers.h 34 | DEPENDS 35 | common_port 36 | transform_rigid_transform 37 | ) 38 | 39 | google_library(transform_transform 40 | USES_EIGEN 41 | SRCS 42 | transform.cc 43 | HDRS 44 | transform.h 45 | DEPENDS 46 | common_math 47 | transform_proto_transform 48 | transform_rigid_transform 49 | ) 50 | 51 | google_library(transform_transform_interpolation_buffer 52 | USES_CERES 53 | USES_EIGEN 54 | SRCS 55 | transform_interpolation_buffer.cc 56 | HDRS 57 | transform_interpolation_buffer.h 58 | DEPENDS 59 | common_make_unique 60 | common_time 61 | proto_trajectory 62 | transform_rigid_transform 63 | transform_transform 64 | ) 65 | 66 | google_test(transform_transform_interpolation_buffer_test 67 | USES_EIGEN 68 | SRCS 69 | transform_interpolation_buffer_test.cc 70 | DEPENDS 71 | transform_rigid_transform 72 | transform_rigid_transform_test_helpers 73 | transform_transform_interpolation_buffer 74 | ) 75 | 76 | google_test(transform_transform_test 77 | SRCS 78 | transform_test.cc 79 | DEPENDS 80 | transform_rigid_transform 81 | transform_rigid_transform_test_helpers 82 | transform_transform 83 | ) 84 | -------------------------------------------------------------------------------- /cartographer/transform/proto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | google_proto_library(transform_proto_transform 16 | SRCS 17 | transform.proto 18 | ) 19 | -------------------------------------------------------------------------------- /cartographer/transform/proto/transform.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.transform.proto; 18 | 19 | // All coordinates are expressed in the right-handed Cartesian coordinate system 20 | // used by Cartographer (x forward, y left, z up). Message names are chosen to 21 | // mirror those used in the Eigen library. 22 | 23 | message Vector2d { 24 | optional double x = 1; 25 | optional double y = 2; 26 | } 27 | 28 | message Vector2f { 29 | optional float x = 1; 30 | optional float y = 2; 31 | } 32 | 33 | message Vector3d { 34 | optional double x = 1; 35 | optional double y = 2; 36 | optional double z = 3; 37 | } 38 | 39 | message Vector3f { 40 | optional float x = 1; 41 | optional float y = 2; 42 | optional float z = 3; 43 | } 44 | 45 | message Quaterniond { 46 | optional double x = 1; 47 | optional double y = 2; 48 | optional double z = 3; 49 | optional double w = 4; 50 | } 51 | 52 | message Quaternionf { 53 | optional float x = 1; 54 | optional float y = 2; 55 | optional float z = 3; 56 | optional float w = 4; 57 | } 58 | 59 | message Rigid2d { 60 | optional Vector2d translation = 1; 61 | optional double rotation = 2; 62 | } 63 | 64 | message Rigid2f { 65 | optional Vector2f translation = 1; 66 | optional float rotation = 2; 67 | } 68 | 69 | message Rigid3d { 70 | optional Vector3d translation = 1; 71 | optional Quaterniond rotation = 2; 72 | } 73 | 74 | message Rigid3f { 75 | optional Vector3f translation = 1; 76 | optional Quaternionf rotation = 2; 77 | } 78 | -------------------------------------------------------------------------------- /cartographer/transform/rigid_transform_test_helpers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 18 | #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 19 | 20 | #include 21 | 22 | #include "Eigen/Core" 23 | #include "Eigen/Geometry" 24 | #include "cartographer/common/port.h" 25 | #include "cartographer/transform/rigid_transform.h" 26 | #include "gmock/gmock.h" 27 | #include "gtest/gtest.h" 28 | 29 | namespace cartographer { 30 | namespace transform { 31 | 32 | template 33 | Eigen::Transform ToEigen(const Rigid2& rigid2) { 34 | return Eigen::Translation(rigid2.translation()) * rigid2.rotation(); 35 | } 36 | 37 | template 38 | Eigen::Transform ToEigen(const Rigid3& rigid3) { 39 | return Eigen::Translation(rigid3.translation()) * rigid3.rotation(); 40 | } 41 | 42 | MATCHER_P2(IsNearly, rigid, epsilon, 43 | string(string(negation ? "isn't" : "is", " nearly ") + 44 | rigid.DebugString())) { 45 | return ToEigen(arg).isApprox(ToEigen(rigid), epsilon); 46 | } 47 | 48 | } // namespace transform 49 | } // namespace cartographer 50 | 51 | #endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 52 | -------------------------------------------------------------------------------- /cartographer/transform/transform_interpolation_buffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ 18 | #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/common/time.h" 24 | #include "cartographer/proto/trajectory.pb.h" 25 | #include "cartographer/transform/rigid_transform.h" 26 | 27 | namespace cartographer { 28 | namespace transform { 29 | 30 | // A time-ordered buffer of transforms that supports interpolated lookups. 31 | class TransformInterpolationBuffer { 32 | public: 33 | static std::unique_ptr FromTrajectory( 34 | const cartographer::proto::Trajectory& trajectory); 35 | 36 | // Adds a new transform to the buffer and removes the oldest transform if the 37 | // buffer size limit is exceeded. 38 | void Push(common::Time time, const transform::Rigid3d& transform); 39 | 40 | // Returns true if an interpolated transfrom can be computed at 'time'. 41 | bool Has(common::Time time) const; 42 | 43 | // Returns an interpolated transform at 'time'. CHECK()s that a transform at 44 | // 'time' is available. 45 | transform::Rigid3d Lookup(common::Time time) const; 46 | 47 | // Returns the timestamp of the earliest transform in the buffer or 0 if the 48 | // buffer is empty. 49 | common::Time earliest_time() const; 50 | 51 | // Returns the timestamp of the earliest transform in the buffer or 0 if the 52 | // buffer is empty. 53 | common::Time latest_time() const; 54 | 55 | // Returns true if the buffer is empty. 56 | bool empty() const; 57 | 58 | private: 59 | struct TimestampedTransform { 60 | common::Time time; 61 | transform::Rigid3d transform; 62 | }; 63 | 64 | std::deque deque_; 65 | }; 66 | 67 | } // namespace transform 68 | } // namespace cartographer 69 | 70 | #endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ 71 | -------------------------------------------------------------------------------- /cmake/modules/FindSphinx.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # TODO(hrapp): Replace through the one of ceres 16 | 17 | find_program(SPHINX_EXECUTABLE 18 | NAMES sphinx-build 19 | PATHS 20 | /usr/bin 21 | DOC "Sphinx") 22 | 23 | include(FindPackageHandleStandardArgs) 24 | find_package_handle_standard_args(Sphinx DEFAULT_MSG SPHINX_EXECUTABLE) 25 | -------------------------------------------------------------------------------- /configuration_files/map_builder.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | include "trajectory_builder_2d.lua" 16 | include "trajectory_builder_3d.lua" 17 | include "sparse_pose_graph.lua" 18 | 19 | MAP_BUILDER = { 20 | use_trajectory_builder_2d = false, 21 | trajectory_builder_2d = TRAJECTORY_BUILDER_2D, 22 | use_trajectory_builder_3d = false, 23 | trajectory_builder_3d = TRAJECTORY_BUILDER_3D, 24 | num_background_threads = 4, 25 | sparse_pose_graph = SPARSE_POSE_GRAPH, 26 | } 27 | -------------------------------------------------------------------------------- /configuration_files/trajectory_builder_2d.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | TRAJECTORY_BUILDER_2D = { 16 | use_imu_data = true, 17 | horizontal_laser_min_z = -0.8, 18 | horizontal_laser_max_z = 2., 19 | horizontal_laser_voxel_filter_size = 0.025, 20 | 21 | use_online_correlative_scan_matching = false, 22 | adaptive_voxel_filter = { 23 | max_length = 0.5, 24 | min_num_points = 200, 25 | max_range = 50., 26 | }, 27 | 28 | real_time_correlative_scan_matcher = { 29 | linear_search_window = 0.1, 30 | angular_search_window = math.rad(20.), 31 | translation_delta_cost_weight = 1e-1, 32 | rotation_delta_cost_weight = 1e-1, 33 | }, 34 | 35 | ceres_scan_matcher = { 36 | occupied_space_cost_functor_weight = 20., 37 | previous_pose_translation_delta_cost_functor_weight = 1., 38 | initial_pose_estimate_rotation_delta_cost_functor_weight = 1e2, 39 | covariance_scale = 2.34e-4, 40 | ceres_solver_options = { 41 | use_nonmonotonic_steps = true, 42 | max_num_iterations = 50, 43 | num_threads = 1, 44 | }, 45 | }, 46 | 47 | motion_filter = { 48 | max_time_seconds = 5., 49 | max_distance_meters = 0.2, 50 | max_angle_radians = math.rad(1.), 51 | }, 52 | 53 | pose_tracker = { 54 | orientation_model_variance = 5e-4, 55 | position_model_variance = 0.000654766, 56 | velocity_model_variance = 0.053926, 57 | imu_gravity_time_constant = 10., 58 | imu_gravity_variance = 1e-6, 59 | num_odometry_states = 1000, 60 | }, 61 | 62 | submaps = { 63 | resolution = 0.05, 64 | half_length = 200., 65 | num_laser_fans = 90, 66 | output_debug_images = false, 67 | laser_fan_inserter = { 68 | insert_free_space = true, 69 | hit_probability = 0.55, 70 | miss_probability = 0.49, 71 | }, 72 | }, 73 | } 74 | -------------------------------------------------------------------------------- /docs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2016 The Cartographer Authors 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | set(OUTPUT_DIR "${CMAKE_BINARY_DIR}/docs/html") 16 | 17 | add_custom_target(build_doc ALL 18 | ${SPHINX_EXECUTABLE} -b html 19 | ${CMAKE_CURRENT_SOURCE_DIR}/source 20 | ${CMAKE_CURRENT_BINARY_DIR}/html 21 | COMMENT "Building documentation." 22 | ) 23 | 24 | # TODO(hrapp): Install documentation? 25 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. Copyright 2016 The Cartographer Authors 2 | 3 | .. Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | .. http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | .. Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | 15 | ============ 16 | Cartographer 17 | ============ 18 | 19 | .. toctree:: 20 | :maxdepth: 2 21 | :hidden: 22 | 23 | `Cartographer`_ is a system that provides real-time simultaneous localization 24 | and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor 25 | configurations. 26 | 27 | .. _Cartographer: https://github.com/googlecartographer/cartographer 28 | .. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping 29 | 30 | Getting started with ROS 31 | ======================== 32 | 33 | ROS integration is provided by the `Cartographer ROS repository`_. You will find 34 | complete documentation for using Cartographer with ROS at the 35 | `Cartographer ROS Read the Docs site`_. 36 | 37 | .. _Cartographer ROS repository: https://github.com/googlecartographer/cartographer_ros 38 | .. _Cartographer ROS Read the Docs site: https://google-cartographer-ros.readthedocs.io 39 | 40 | Getting started without ROS 41 | =========================== 42 | 43 | On Ubuntu 14.04 (Trusty): 44 | 45 | .. literalinclude:: ../../scripts/install_debs.sh 46 | :language: bash 47 | :linenos: 48 | :lines: 20- 49 | 50 | .. literalinclude:: ../../scripts/install_ceres.sh 51 | :language: bash 52 | :linenos: 53 | :lines: 20- 54 | 55 | .. literalinclude:: ../../scripts/install_cartographer.sh 56 | :language: bash 57 | :linenos: 58 | :lines: 20- 59 | 60 | How to cite us 61 | ============== 62 | 63 | Background about the algorithms developed for Cartographer can be found in the 64 | following publication. If you use Cartographer for your research, we would 65 | appreciate it if you cite our paper. 66 | 67 | W. Hess, D. Kohler, H. Rapp, and D. Andor, 68 | `Real-Time Loop Closure in 2D LIDAR SLAM`_, in 69 | *Robotics and Automation (ICRA), 2016 IEEE International Conference on*. 70 | IEEE, 2016. pp. 1271–1278. 71 | 72 | .. _Real-Time Loop Closure in 2D LIDAR SLAM: https://research.google.com/pubs/pub45466.html 73 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 17 | 18 | 19 | cartographer 20 | 1.0.0 21 | 22 | Cartographer is a system that provides real-time simultaneous localization 23 | and mapping (SLAM) in 2D and 3D across multiple platforms and sensor 24 | configurations. 25 | 26 | 27 | The Cartographer Authors 28 | 29 | Apache 2.0 30 | 31 | https://github.com/googlecartographer/cartographer 32 | 33 | catkin 34 | 35 | g++-static 36 | google-mock 37 | python-sphinx 38 | 39 | boost 40 | ceres_solver 41 | eigen 42 | libgflags-dev 43 | libgoogle-glog-dev 44 | libwebp-dev 45 | lua5.2-dev 46 | protobuf-dev 47 | 48 | 49 | cmake 50 | 51 | 52 | -------------------------------------------------------------------------------- /scripts/install_cartographer.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # Copyright 2016 The Cartographer Authors 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | set -o errexit 18 | set -o verbose 19 | 20 | # Build and install Cartographer. 21 | cd cartographer 22 | mkdir build 23 | cd build 24 | cmake .. -G Ninja 25 | ninja 26 | ninja test 27 | sudo ninja install 28 | -------------------------------------------------------------------------------- /scripts/install_ceres.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # Copyright 2016 The Cartographer Authors 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | set -o errexit 18 | set -o verbose 19 | 20 | # Build and install Ceres. 21 | git clone https://ceres-solver.googlesource.com/ceres-solver 22 | cd ceres-solver 23 | mkdir build 24 | cd build 25 | cmake .. -G Ninja 26 | ninja 27 | ninja test 28 | sudo ninja install 29 | -------------------------------------------------------------------------------- /scripts/install_debs.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # Copyright 2016 The Cartographer Authors 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | set -o errexit 18 | set -o verbose 19 | 20 | # Install the required libraries that are available as debs. 21 | sudo apt-get update 22 | sudo apt-get install -y \ 23 | cmake \ 24 | g++ \ 25 | git \ 26 | google-mock \ 27 | libboost-all-dev \ 28 | libeigen3-dev \ 29 | libgflags-dev \ 30 | libgoogle-glog-dev \ 31 | liblua5.2-dev \ 32 | libprotobuf-dev \ 33 | libsuitesparse-dev \ 34 | libwebp-dev \ 35 | ninja-build \ 36 | protobuf-compiler \ 37 | python-sphinx 38 | --------------------------------------------------------------------------------