Repository: slam-code/cartographer Branch: master Commit: 4332493a434d Files: 273 Total size: 954.9 KB Directory structure: gitextract_6vczu7ti/ ├── .gitignore ├── README.md ├── common/ │ ├── blocking_queue.h │ ├── blocking_queue_test.cc │ ├── ceres_solver_options.cc │ ├── ceres_solver_options.h │ ├── config.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 │ ├── make_unique_test.cc │ ├── math.h │ ├── math_test.cc │ ├── mutex.h │ ├── port.h │ ├── port_le_test.cpp │ ├── proto/ │ │ └── ceres_solver_options.proto │ ├── rate_timer.h │ ├── rate_timer_test.cc │ ├── thread_pool.cc │ ├── thread_pool.h │ ├── time.cc │ └── time.h ├── ground_truth/ │ └── compute_relations_metrics_main.cc ├── io/ │ ├── cairo_types.h │ ├── coloring_points_processor.cc │ ├── coloring_points_processor.h │ ├── counting_points_processor.cc │ ├── counting_points_processor.h │ ├── file_writer.cc │ ├── file_writer.h │ ├── fixed_ratio_sampling_points_processor.cc │ ├── fixed_ratio_sampling_points_processor.h │ ├── intensity_to_color_points_processor.cc │ ├── intensity_to_color_points_processor.h │ ├── min_max_range_filtering_points_processor.cc │ ├── min_max_range_filtering_points_processor.h │ ├── null_points_processor.h │ ├── outlier_removing_points_processor.cc │ ├── outlier_removing_points_processor.h │ ├── pcd_writing_points_processor.cc │ ├── pcd_writing_points_processor.h │ ├── ply_writing_points_processor.cc │ ├── ply_writing_points_processor.h │ ├── points_batch.cc │ ├── points_batch.h │ ├── points_processor.h │ ├── points_processor_pipeline_builder.cc │ ├── points_processor_pipeline_builder.h │ ├── proto_stream.cc │ ├── proto_stream.h │ ├── proto_stream_test.cc │ ├── xray_points_processor.cc │ ├── xray_points_processor.h │ ├── xyz_writing_points_processor.cc │ └── xyz_writing_points_processor.h ├── kalman_filter/ │ ├── gaussian_distribution.h │ ├── gaussian_distribution_test.cc │ ├── pose_tracker.cc │ ├── pose_tracker.h │ ├── pose_tracker_test.cc │ ├── proto/ │ │ └── pose_tracker_options.proto │ ├── unscented_kalman_filter.h │ └── unscented_kalman_filter_test.cc ├── mapping/ │ ├── collated_trajectory_builder.cc │ ├── collated_trajectory_builder.h │ ├── detect_floors.cc │ ├── detect_floors.h │ ├── global_trajectory_builder_interface.h │ ├── id.h │ ├── imu_tracker.cc │ ├── imu_tracker.h │ ├── map_builder.cc │ ├── map_builder.h │ ├── odometry_state_tracker.cc │ ├── odometry_state_tracker.h │ ├── probability_values.cc │ ├── probability_values.h │ ├── probability_values_test.cc │ ├── proto/ │ │ ├── map_builder_options.proto │ │ ├── sparse_pose_graph.proto │ │ ├── sparse_pose_graph_options.proto │ │ ├── submap_visualization.proto │ │ ├── trajectory.proto │ │ ├── trajectory_builder_options.proto │ │ └── trajectory_connectivity.proto │ ├── sparse_pose_graph/ │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem_options.cc │ │ ├── optimization_problem_options.h │ │ └── proto/ │ │ ├── constraint_builder_options.proto │ │ └── optimization_problem_options.proto │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── submaps.cc │ ├── submaps.h │ ├── submaps_test.cc │ ├── trajectory_builder.cc │ ├── trajectory_builder.h │ ├── trajectory_connectivity.cc │ ├── trajectory_connectivity.h │ ├── trajectory_connectivity_test.cc │ └── trajectory_node.h ├── mapping_2d/ │ ├── global_trajectory_builder.cc │ ├── global_trajectory_builder.h │ ├── local_trajectory_builder.cc │ ├── local_trajectory_builder.h │ ├── local_trajectory_builder_options.cc │ ├── local_trajectory_builder_options.h │ ├── map_limits.h │ ├── map_limits_test.cc │ ├── probability_grid.h │ ├── probability_grid_test.cc │ ├── proto/ │ │ ├── cell_limits.proto │ │ ├── local_trajectory_builder_options.proto │ │ ├── map_limits.proto │ │ ├── probability_grid.proto │ │ ├── range_data_inserter_options.proto │ │ └── submaps_options.proto │ ├── range_data_inserter.cc │ ├── range_data_inserter.h │ ├── range_data_inserter_test.cc │ ├── ray_casting.cc │ ├── ray_casting.h │ ├── scan_matching/ │ │ ├── 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/ │ │ │ ├── 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/ │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ ├── optimization_problem.h │ │ └── spa_cost_function.h │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph_test.cc │ ├── submaps.cc │ ├── submaps.h │ ├── submaps_test.cc │ ├── xy_index.h │ └── xy_index_test.cc ├── mapping_3d/ │ ├── 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 │ ├── 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/ │ │ ├── hybrid_grid.proto │ │ ├── kalman_local_trajectory_builder_options.proto │ │ ├── local_trajectory_builder_options.proto │ │ ├── motion_filter_options.proto │ │ ├── optimizing_local_trajectory_builder_options.proto │ │ ├── range_data_inserter_options.proto │ │ └── submaps_options.proto │ ├── range_data_inserter.cc │ ├── range_data_inserter.h │ ├── range_data_inserter_test.cc │ ├── rotation_cost_function.h │ ├── scan_matching/ │ │ ├── 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/ │ │ │ ├── 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/ │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ ├── optimization_problem.h │ │ ├── optimization_problem_test.cc │ │ └── spa_cost_function.h │ ├── sparse_pose_graph.cc │ ├── sparse_pose_graph.h │ ├── submaps.cc │ ├── submaps.h │ └── translation_cost_function.h ├── sensor/ │ ├── collator.cc │ ├── collator.h │ ├── collator_test.cc │ ├── compressed_point_cloud.cc │ ├── compressed_point_cloud.h │ ├── compressed_point_cloud_test.cc │ ├── configuration.cc │ ├── configuration.h │ ├── data.h │ ├── ordered_multi_queue.cc │ ├── ordered_multi_queue.h │ ├── ordered_multi_queue_le_test.cc │ ├── ordered_multi_queue_test.cc │ ├── point_cloud.cc │ ├── point_cloud.h │ ├── point_cloud_test.cc │ ├── proto/ │ │ ├── adaptive_voxel_filter_options.proto │ │ ├── configuration.proto │ │ └── sensor.proto │ ├── range_data.cc │ ├── range_data.h │ ├── range_data_test.cc │ ├── voxel_filter.cc │ ├── voxel_filter.h │ └── voxel_filter_test.cc └── transform/ ├── proto/ │ └── transform.proto ├── rigid_transform.cc ├── rigid_transform.h ├── rigid_transform_le2_grid2_test.cc ├── 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 ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ # Prerequisites *.d # Compiled Object files *.slo *.lo *.o *.obj # Precompiled Headers *.gch *.pch # Compiled Dynamic libraries *.so *.dylib *.dll # Fortran module files *.mod *.smod # Compiled Static libraries *.lai *.la *.a *.lib # Executables *.exe *.out *.app # 忽略*.o和*.a文件 *.[oa] *.[out] *build build #过滤整个build文件夹 devel *.class #过滤所有.class文件 .idea cartographer cartographer_ros ceres-solver Eigen src .zip orig backup pictures html note ================================================ FILE: README.md ================================================ # cartographer 关于谷歌slam地图库 [cartographer](https://github.com/googlecartographer/cartographer)的源码注释。(只含源码) 博客文章： * [知乎](https://zhuanlan.zhihu.com/learnmoreonce) (简略) * [CSDN](http://blog.csdn.net/learnmoreonce) (详细) 源码分析的markdown文件可在 [这里]( https://github.com/slam4code/SLAM/tree/master/cartographer-%E6%BA%90%E7%A0%81%E5%88%86%E6%9E%90 ) 查找。 我的微信公众号:slamcode
slamcode
================================================ FILE: common/blocking_queue.h ================================================ #ifndef CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ #define CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ #include #include #include #include "cartographer/common/mutex.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "glog/logging.h" /* * BlockingQueue是线程安全的阻塞队列,(生产者消费者模式) * 不可拷贝不可赋值 * 构造函数初始化队列大小,kInfiniteQueueSize=0默认不限制容量。queue_size限制容量：通过条件变量做到. * Push()添加元素,容量不够时,阻塞等待 * Pop()删除元素,没有元素时,阻塞等待 * Peek()返回下一个应该弹出的元素 * PushWithTimeout(),添加元素,若超时则返回false * PopWithTimeout(),删除元素，若超时则返回false * * */ namespace cartographer { namespace common { // A thread-safe blocking queue that is useful for producer/consumer patterns. // 'T' must be movable. template class BlockingQueue { public: static constexpr size_t kInfiniteQueueSize = 0; // Constructs a blocking queue with infinite queue size. BlockingQueue() : BlockingQueue(kInfiniteQueueSize) {} //默认不限制容量 BlockingQueue(const BlockingQueue&) = delete; BlockingQueue& operator=(const BlockingQueue&) = delete; // Constructs a blocking queue with a size of 'queue_size'.限制容量 explicit BlockingQueue(const size_t queue_size) : queue_size_(queue_size) {} // Pushes a value onto the queue. Blocks if the queue is full. void Push(T t) { MutexLocker lock(&mutex_);//加锁 lock.Await([this]() REQUIRES(mutex_) { return QueueNotFullCondition(); });//队列未满才能push deque_.push_back(std::move(t)); //移动而非拷贝 } // Like push, but returns false if 'timeout' is reached. bool PushWithTimeout(T t, const common::Duration timeout) { //最多等待timeout时间 MutexLocker lock(&mutex_); if (!lock.AwaitWithTimeout( [this]() REQUIRES(mutex_) { return QueueNotFullCondition(); }, timeout)) { return false; } deque_.push_back(std::move(t)); return true; } // Pops the next value from the queue. Blocks until a value is available. T Pop() { MutexLocker lock(&mutex_); //加锁 lock.Await([this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); });//队列不为空才能pop() T t = std::move(deque_.front()); deque_.pop_front(); return t; } // Like Pop, but can timeout. Returns nullptr in this case. T PopWithTimeout(const common::Duration timeout) { MutexLocker lock(&mutex_); if (!lock.AwaitWithTimeout( [this]() REQUIRES(mutex_) { return QueueNotEmptyCondition(); }, timeout)) { return nullptr; } T t = std::move(deque_.front()); deque_.pop_front(); return t; } // Returns the next value in the queue or nullptr if the queue is empty. // Maintains ownership. This assumes a member function get() that returns // a pointer to the given type R. template const R* Peek() { MutexLocker lock(&mutex_); if (deque_.empty()) { return nullptr; } return deque_.front().get(); } // Returns the number of items currently in the queue. size_t Size() { MutexLocker lock(&mutex_); return deque_.size(); } private: // Returns true iff the queue is not empty. bool QueueNotEmptyCondition() REQUIRES(mutex_) { return !deque_.empty(); } // Returns true iff the queue is not full. bool QueueNotFullCondition() REQUIRES(mutex_) { return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_; } Mutex mutex_; const size_t queue_size_ GUARDED_BY(mutex_); std::deque deque_ GUARDED_BY(mutex_); }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_BLOCKING_QUEUE_H_ ================================================ FILE: common/blocking_queue_test.cc ================================================ #include "cartographer/common/blocking_queue.h" #include #include #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "gtest/gtest.h" namespace cartographer { namespace common { namespace { TEST(BlockingQueueTest, testPushPeekPop) { BlockingQueue> blocking_queue;//元素是智能指针,不限容 blocking_queue.Push(common::make_unique(42)); //添加 ASSERT_EQ(1, blocking_queue.Size()); //size==1 blocking_queue.Push(common::make_unique(24)); //添加 ASSERT_EQ(2, blocking_queue.Size()); //size==2 EXPECT_EQ(42, *blocking_queue.Peek()); //顶元素==42 ASSERT_EQ(2, blocking_queue.Size()); //队列大小是2 EXPECT_EQ(42, *blocking_queue.Pop()); ASSERT_EQ(1, blocking_queue.Size()); EXPECT_EQ(24, *blocking_queue.Pop()); ASSERT_EQ(0, blocking_queue.Size()); EXPECT_EQ(nullptr, blocking_queue.Peek()); ASSERT_EQ(0, blocking_queue.Size()); } TEST(BlockingQueueTest, testPushPopSharedPtr) { BlockingQueue> blocking_queue; //不限容 blocking_queue.Push(std::make_shared(42)); blocking_queue.Push(std::make_shared(24)); EXPECT_EQ(42, *blocking_queue.Pop()); EXPECT_EQ(24, *blocking_queue.Pop()); } TEST(BlockingQueueTest, testPopWithTimeout) { BlockingQueue> blocking_queue; EXPECT_EQ(nullptr, //没有插入,所以是nullptr blocking_queue.PopWithTimeout(common::FromMilliseconds(150))); } TEST(BlockingQueueTest, testPushWithTimeout) { BlockingQueue> blocking_queue(1);//限容 EXPECT_EQ(true, //插入成功 blocking_queue.PushWithTimeout(common::make_unique(42), common::FromMilliseconds(150))); EXPECT_EQ(false, //失败,因为队列大小是1 blocking_queue.PushWithTimeout(common::make_unique(15), common::FromMilliseconds(150))); EXPECT_EQ(42, *blocking_queue.Pop()); EXPECT_EQ(0, blocking_queue.Size()); } TEST(BlockingQueueTest, testPushWithTimeoutInfinteQueue) { BlockingQueue> blocking_queue; //不限容量 EXPECT_EQ(true, blocking_queue.PushWithTimeout(common::make_unique(42), common::FromMilliseconds(150))); EXPECT_EQ(true, blocking_queue.PushWithTimeout(common::make_unique(45), common::FromMilliseconds(150))); EXPECT_EQ(42, *blocking_queue.Pop()); EXPECT_EQ(45, *blocking_queue.Pop()); EXPECT_EQ(0, blocking_queue.Size()); } TEST(BlockingQueueTest, testBlockingPop) { BlockingQueue> blocking_queue; ASSERT_EQ(0, blocking_queue.Size()); int pop = 0; //多线程测试 std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.Pop(); }); std::this_thread::sleep_for(common::FromMilliseconds(100)); blocking_queue.Push(common::make_unique(42)); thread.join(); ASSERT_EQ(0, blocking_queue.Size()); EXPECT_EQ(42, pop); } TEST(BlockingQueueTest, testBlockingPopWithTimeout) { BlockingQueue> blocking_queue; ASSERT_EQ(0, blocking_queue.Size()); int pop = 0; //多线程测试 std::thread thread([&blocking_queue, &pop] { pop = *blocking_queue.PopWithTimeout(common::FromMilliseconds(2500)); }); std::this_thread::sleep_for(common::FromMilliseconds(100)); blocking_queue.Push(common::make_unique(42)); thread.join(); ASSERT_EQ(0, blocking_queue.Size()); EXPECT_EQ(42, pop); } } // namespace } // namespace common } // namespace cartographer ================================================ FILE: common/ceres_solver_options.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/common/ceres_solver_options.h" namespace cartographer { namespace common { /* 根据lua文件设置 */ proto::CeresSolverOptions CreateCeresSolverOptionsProto( common::LuaParameterDictionary* parameter_dictionary) { proto::CeresSolverOptions proto; proto.set_use_nonmonotonic_steps( parameter_dictionary->GetBool("use_nonmonotonic_steps"));//是否设置使用nonmonotonic proto.set_max_num_iterations( parameter_dictionary->GetNonNegativeInt("max_num_iterations"));//迭代次数 proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads"));//线程数 CHECK_GT(proto.max_num_iterations(), 0); CHECK_GT(proto.num_threads(), 0); return proto; } /* 根据proto文件设置 */ ceres::Solver::Options CreateCeresSolverOptions( const proto::CeresSolverOptions& proto) { ceres::Solver::Options options;//建立临时对象 options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps();//为临时对象赋值 options.max_num_iterations = proto.max_num_iterations(); //赋值 options.num_threads = proto.num_threads(); //赋值 return options; } } // namespace common } // namespace cartographer ================================================ FILE: common/ceres_solver_options.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ #define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/proto/ceres_solver_options.pb.h" #include "ceres/ceres.h" namespace cartographer { namespace common { /* 配置加载类, 根据lua设置 CeresSolver的选项 */ proto::CeresSolverOptions CreateCeresSolverOptionsProto( common::LuaParameterDictionary* parameter_dictionary); //根据proto设置 CeresSolver的选项 ceres::Solver::Options CreateCeresSolverOptions( const proto::CeresSolverOptions& proto); } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ ================================================ FILE: common/config.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_CONFIG_H_ #define CARTOGRAPHER_COMMON_CONFIG_H_ namespace cartographer { namespace common { constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径 ""; constexpr char kSourceDirectory[] = "./"; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_CONFIG_H_ ================================================ FILE: common/config.h.cmake ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_CONFIG_H_ #define CARTOGRAPHER_COMMON_CONFIG_H_ namespace cartographer { namespace common { constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径 "@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@"; constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@"; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_CONFIG_H_ ================================================ FILE: common/configuration_file_resolver.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/common/configuration_file_resolver.h" #include #include #include #include "cartographer/common/config.h"//根据config.h.cmake生成 #include "glog/logging.h" namespace cartographer { namespace common { ConfigurationFileResolver::ConfigurationFileResolver( const std::vector& configuration_files_directories) : configuration_files_directories_(configuration_files_directories) { configuration_files_directories_.push_back(kConfigurationFilesDirectory);//配置文件夹路径 } string ConfigurationFileResolver::GetFullPathOrDie(const string& basename) { //根据文件 夹和basename生成文件名 for (const auto& path : configuration_files_directories_) { const string filename = path + "/" + basename; std::ifstream stream(filename.c_str()); if (stream.good()) { LOG(INFO) << "Found '" << filename << "' for '" << basename << "'."; return filename; } } LOG(FATAL) << "File '" << basename << "' was not found."; } string ConfigurationFileResolver::GetFileContentOrDie(const string& basename) {//根据文件名读取file内容 const string filename = GetFullPathOrDie(basename); std::ifstream stream(filename.c_str()); //文件输入流 return string((std::istreambuf_iterator(stream)),//使用迭代器初始化string对象 std::istreambuf_iterator()); } } // namespace common } // namespace cartographer ================================================ FILE: common/configuration_file_resolver.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ #define CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" namespace cartographer { namespace common { /* ConfigurationFileResolver是配置文件解析类,继承自FileResolver类 在给定的目录下搜索配置文件, */ // A 'FileResolver' for the 'LuaParameterDictionary' that reads files from disk. // It searches the 'configuration_files_directories' in order to find the // requested filename. The last place searched is always the // 'configuration_files/' directory installed with Cartographer. It contains // reasonable configuration for the various Cartographer components which // provide a good starting ground for new platforms. class ConfigurationFileResolver : public FileResolver { public: explicit ConfigurationFileResolver( const std::vector& configuration_files_directories); string GetFullPathOrDie(const string& basename) override;//获取文件的全部路径 string GetFileContentOrDie(const string& basename) override; //获取文件内容 private: std::vector configuration_files_directories_; }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_CONFIGURATION_FILE_RESOLVER_H_ ================================================ FILE: common/fixed_ratio_sampler.cc ================================================ #include "cartographer/common/fixed_ratio_sampler.h" namespace cartographer { namespace common { FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {} FixedRatioSampler::~FixedRatioSampler() {} bool FixedRatioSampler::Pulse() { //如果当前采样率 samples/ pulses低于固定采样率,此次事件计入采样. ++num_pulses_; if (static_cast(num_samples_) / num_pulses_ < ratio_) { ++num_samples_; return true; } return false; } string FixedRatioSampler::DebugString() { return std::to_string(num_samples_) + " (" + std::to_string(100. * num_samples_ / num_pulses_) + "%)"; } } // namespace common } // namespace cartographer ================================================ FILE: common/fixed_ratio_sampler.h ================================================ #ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ #define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ #include #include "cartographer/common/port.h" namespace cartographer { namespace common { /* FixedRatioSampler是频率固定的采样器类，目的是从数据流中均匀的按照固定频率采样数据 FixedRatioSampler不可拷贝,不可赋值. 成员函数提供2种操作: Pulse()产生一个事件pulses,并且:如果计入采样samples，返回true DebugString():以string形式输出采样率 */ // Signals when a sample should be taken from a stream of data to select a // uniformly distributed fraction of the data. class FixedRatioSampler { public: explicit FixedRatioSampler(double ratio); ~FixedRatioSampler(); FixedRatioSampler(const FixedRatioSampler&) = delete; FixedRatioSampler& operator=(const FixedRatioSampler&) = delete; // Returns true if this pulse should result in an sample. bool Pulse(); // Returns a debug string describing the current ratio of samples to pulses. string DebugString(); private: // Sampling occurs if the proportion of samples to pulses drops below this // number. const double ratio_; int64 num_pulses_ = 0; //产生的脉冲次数 int64 num_samples_ = 0;//记录的采样次数 }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ ================================================ FILE: common/fixed_ratio_sampler_test.cc ================================================ #include "cartographer/common/fixed_ratio_sampler.h" #include "gtest/gtest.h" namespace cartographer { namespace common { namespace { TEST(FixedRatioSamplerTest, AlwaysTrue) { FixedRatioSampler fixed_ratio_sampler(1.);//固定采样率是1hz，每次Pulse都采集samples for (int i = 0; i < 100; ++i) { EXPECT_TRUE(fixed_ratio_sampler.Pulse()); } } TEST(FixedRatioSamplerTest, AlwaysFalse) { FixedRatioSampler fixed_ratio_sampler(0.);//0 hz，不采集samples for (int i = 0; i < 100; ++i) { EXPECT_FALSE(fixed_ratio_sampler.Pulse()); } } TEST(FixedRatioSamplerTest, SometimesTrue) { FixedRatioSampler fixed_ratio_sampler(0.5); //0.5hz for (int i = 0; i < 100; ++i) { EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse());//每2次Pulse采集一次samples } } TEST(FixedRatioSamplerTest, FirstPulseIsTrue) { // Choose a very very small positive number for the ratio. FixedRatioSampler fixed_ratio_sampler(1e-20); //0.000000...001hz EXPECT_TRUE(fixed_ratio_sampler.Pulse()); for (int i = 0; i < 100; ++i) { EXPECT_FALSE(fixed_ratio_sampler.Pulse()); //每100000000次Pulse采集一次samples } } } // namespace } // namespace common } // namespace cartographer ================================================ FILE: common/histogram.cc ================================================ #include "cartographer/common/histogram.h" #include #include #include #include "cartographer/common/port.h" #include "glog/logging.h" namespace cartographer { namespace common { //添加元素 void Histogram::Add(const float value) { values_.push_back(value); } //以bin大小是buckets转成 string,输出 直方图信息. string Histogram::ToString(const int buckets) const { CHECK_GE(buckets, 1); //篮子个数必须>=1 if (values_.empty()) { return "Count: 0"; } const float min = *std::min_element(values_.begin(), values_.end()); const float max = *std::max_element(values_.begin(), values_.end()); const float mean = std::accumulate(values_.begin(), values_.end(), 0.f) / values_.size(); string result = "Count: " + std::to_string(values_.size()) + " Min: " + std::to_string(min) + " Max: " + std::to_string(max) + " Mean: " + std::to_string(mean); if (min == max) { //最大等于最小直接返回. return result; } CHECK_LT(min, max); float lower_bound = min; //bin区间下界 int total_count = 0; for (int i = 0; i != buckets; ++i) { //根据bin个数 const float upper_bound = //bin区间上界 (i + 1 == buckets) ? max : (max * (i + 1) / buckets + min * (buckets - i - 1) / buckets); int count = 0; for (const float value : values_) { //对每一个vector的值 if (lower_bound <= value && (i + 1 == buckets ? value <= upper_bound : value < upper_bound)) { ++count; } } total_count += count; result += "\n[" + std::to_string(lower_bound) + ", " + std::to_string(upper_bound) + ((i + 1 == buckets) ? "]" : ")"); constexpr int kMaxBarChars = 20; const int bar = (count * kMaxBarChars + values_.size() / 2) / values_.size(); result += "\t"; for (int i = 0; i != kMaxBarChars; ++i) { result += (i < (kMaxBarChars - bar)) ? " " : "#"; } result += "\tCount: " + std::to_string(count) + " (" + std::to_string(count * 1e2f / values_.size()) + "%)"; result += "\tTotal: " + std::to_string(total_count) + " (" + std::to_string(total_count * 1e2f / values_.size()) + "%)"; lower_bound = upper_bound; } return result; } } // namespace common } // namespace cartographer ================================================ FILE: common/histogram.h ================================================ #ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_ #define CARTOGRAPHER_COMMON_HISTOGRAM_H_ #include #include #include #include "cartographer/common/port.h" namespace cartographer { namespace common { /* Histogram直方图类, 提供2个操作: 1，Add()添加元素 2，ToString(buckets )以字符串的形式输出buckets个直方图信息,bin大小是篮子个数. Histogram只有一个数据成员，用vector表示 */ class Histogram { public: void Add(float value); //添加value,可乱序 string ToString(int buckets) const;//分为几块 private: std::vector values_; }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_ /*测试： 若bin为2:[2,1,2,4,5,5]: Count: 6 Min: 1.000000 Max: 5.000000 Mean: 3.166667 [1.000000, 3.000000) ########## Count: 3 (50.000000%) Total: 3 (50.000000%) [3.000000, 5.000000] ########## Count: 3 (50.000000%) Total: 6 (100.000000%) 若bin为3:[2,1,2,4,5,5,6,7,8]: Count: 10 Min: 1.000000 Max: 8.000000 Mean: 4.500000 [1.000000, 3.333333) ###### Count: 3 (30.000000%) Total: 3 (30.000000%) [3.333333, 5.666667) ######## Count: 4 (40.000000%) Total: 7 (70.000000%) [5.666667, 8.000000] ###### Count: 3 (30.000000%) Total: 10 (100.000000%) */ ================================================ FILE: common/lua.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_LUA_H_ #define CARTOGRAPHER_COMMON_LUA_H_ #include #endif // CARTOGRAPHER_COMMON_LUA_H_ ================================================ FILE: common/lua_parameter_dictionary.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ /*LuaParameterDictionary构造函数启动一个lua解释器,此解释器运行lua code, 并返回一个key/val键值表的lua Table */ // When a LuaParameterDictionary is constructed, a new Lua state (i.e. an // independent Lua interpreter) is fired up to evaluate the Lua code. The code // is expected to return a Lua table that contains key/value pairs that are the // key/value pairs of our parameter dictionary. // /* keep around:继续运行,lua table保持在栈上,c++引用这个table,而不是加载到c++ 的map中去. */ // We keep the Lua interpreter around and the table on the stack and reference // it in the Get* methods instead of moving its contents from Lua into a C++ map // since we can only know in the Get* methods what data type to expect in the // table. // // Some of the methods below documentation the current stack with the following // notation: S: ... /* 以下文档中的stack表示: .. lua中的 */ #include "cartographer/common/lua_parameter_dictionary.h" #include #include #include #include namespace cartographer { namespace common { namespace { /* lua_isstring()):当给定索引的值是一个字符串或是一个数字（数字总能转换成字符串）时，返回 1 ，否则返回 0 。 lua_gettop()返回栈顶元素的索引。因为索引是从 1 开始编号的，所以这个结果等于堆栈上的元素个数（因此返回 0 表示堆栈为空） */ // Replace the string at the top of the stack through a quoted version that Lua // can read back. void QuoteStringOnStack(lua_State* L) { //加""引号,方便lua回调 CHECK(lua_isstring(L, -1)) << "Top of stack is not a string value."; int current_index = lua_gettop(L); // S: ... string lua_pushglobaltable(L); // S: ... string globals lua_getfield(L, -1, "string"); // S: ... string globals lua_getfield(L, -1, "format"); // S: ... string globals format lua_pushstring(L, "%q"); // S: ... string globals format "%q" lua_pushvalue(L, current_index); // S: ... string globals // format "%q" string lua_call(L, 2, 1); // S: ... string globals quoted lua_replace(L, current_index); // S: ... quoted globals lua_pop(L, 2); // S: ... quoted } // Sets the given 'dictionary' as the value of the "this" key in Lua's registry // table. void SetDictionaryInRegistry(lua_State* L, LuaParameterDictionary* dictionary) { lua_pushstring(L, "this"); lua_pushlightuserdata(L, dictionary); lua_settable(L, LUA_REGISTRYINDEX); } // Gets the 'dictionary' from the "this" key in Lua's registry table. LuaParameterDictionary* GetDictionaryFromRegistry(lua_State* L) { lua_pushstring(L, "this"); lua_gettable(L, LUA_REGISTRYINDEX); void* return_value = lua_isnil(L, -1) ? nullptr : lua_touserdata(L, -1); lua_pop(L, 1); CHECK(return_value != nullptr); return reinterpret_cast(return_value); } // CHECK()s if a Lua method returned an error. void CheckForLuaErrors(lua_State* L, int status) { CHECK_EQ(status, 0) << lua_tostring(L, -1); } // Returns 'a' if 'condition' is true, else 'b'. int LuaChoose(lua_State* L) { CHECK_EQ(lua_gettop(L), 3) << "choose() takes (condition, a, b)."; CHECK(lua_isboolean(L, 1)) << "condition is not a boolean value."; const bool condition = lua_toboolean(L, 1); if (condition) { lua_pushvalue(L, 2); } else { lua_pushvalue(L, 3); } return 1; } // Pushes a value to the Lua stack. void PushValue(lua_State* L, const int key) { lua_pushinteger(L, key); } void PushValue(lua_State* L, const string& key) { lua_pushstring(L, key.c_str()); } // Reads the value with the given 'key' from the Lua dictionary and pushes it to // the top of the stack. template void GetValueFromLuaTable(lua_State* L, const T& key) { PushValue(L, key); lua_rawget(L, -2); } // CHECK() that the topmost parameter on the Lua stack is a table. void CheckTableIsAtTopOfStack(lua_State* L) { CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; } // Returns true if 'key' is in the table at the top of the Lua stack. template bool HasKeyOfType(lua_State* L, const T& key) { CheckTableIsAtTopOfStack(L); PushValue(L, key); lua_rawget(L, -2); const bool key_not_found = lua_isnil(L, -1); lua_pop(L, 1); // Pop the item again. return !key_not_found; } // Iterates over the integer keys of the table at the top of the stack of 'L• // and pushes the values one by one. 'pop_value' is expected to pop a value and // put them into a C++ container. void GetArrayValues(lua_State* L, const std::function& pop_value) { int idx = 1; while (true) { GetValueFromLuaTable(L, idx); if (lua_isnil(L, -1)) { lua_pop(L, 1); break; } pop_value(); ++idx; } } } // namespace std::unique_ptr LuaParameterDictionary::NonReferenceCounted( const string& code, std::unique_ptr file_resolver) { return std::unique_ptr(new LuaParameterDictionary( code, ReferenceCount::NO, std::move(file_resolver))); } LuaParameterDictionary::LuaParameterDictionary( const string& code, std::unique_ptr file_resolver) : LuaParameterDictionary(code, ReferenceCount::YES, std::move(file_resolver)) {} LuaParameterDictionary::LuaParameterDictionary( const string& code, ReferenceCount reference_count, std::unique_ptr file_resolver) : L_(luaL_newstate()), index_into_reference_table_(-1), file_resolver_(std::move(file_resolver)), reference_count_(reference_count) { CHECK_NOTNULL(L_); SetDictionaryInRegistry(L_, this); luaL_openlibs(L_); lua_register(L_, "choose", LuaChoose); lua_register(L_, "include", LuaInclude); lua_register(L_, "read", LuaRead); CheckForLuaErrors(L_, luaL_loadstring(L_, code.c_str())); CheckForLuaErrors(L_, lua_pcall(L_, 0, 1, 0)); CheckTableIsAtTopOfStack(L_); } LuaParameterDictionary::LuaParameterDictionary( lua_State* const L, ReferenceCount reference_count, std::shared_ptr file_resolver) : L_(lua_newthread(L)), file_resolver_(std::move(file_resolver)), reference_count_(reference_count) { CHECK_NOTNULL(L_); // Make sure this is never garbage collected. CHECK(lua_isthread(L, -1)); index_into_reference_table_ = luaL_ref(L, LUA_REGISTRYINDEX); CHECK(lua_istable(L, -1)) << "Topmost item on Lua stack is not a table!"; lua_xmove(L, L_, 1); // Moves the table and the coroutine over. CheckTableIsAtTopOfStack(L_); } LuaParameterDictionary::~LuaParameterDictionary() { if (reference_count_ == ReferenceCount::YES) { CheckAllKeysWereUsedExactlyOnceAndReset(); } if (index_into_reference_table_ > 0) { luaL_unref(L_, LUA_REGISTRYINDEX, index_into_reference_table_); } else { lua_close(L_); } } std::vector LuaParameterDictionary::GetKeys() const { CheckTableIsAtTopOfStack(L_); std::vector keys; lua_pushnil(L_); // Push the first key while (lua_next(L_, -2) != 0) { lua_pop(L_, 1); // Pop value, keep key. if (!lua_isnumber(L_, -1)) { keys.emplace_back(lua_tostring(L_, -1)); } } return keys; } bool LuaParameterDictionary::HasKey(const string& key) const { return HasKeyOfType(L_, key); } string LuaParameterDictionary::GetString(const string& key) { CheckHasKeyAndReference(key); GetValueFromLuaTable(L_, key); return PopString(Quoted::NO); } string LuaParameterDictionary::PopString(Quoted quoted) const { CHECK(lua_isstring(L_, -1)) << "Top of stack is not a string value."; if (quoted == Quoted::YES) { QuoteStringOnStack(L_); } const string value = lua_tostring(L_, -1); lua_pop(L_, 1); return value; } double LuaParameterDictionary::GetDouble(const string& key) { CheckHasKeyAndReference(key); GetValueFromLuaTable(L_, key); return PopDouble(); } double LuaParameterDictionary::PopDouble() const { CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; const double value = lua_tonumber(L_, -1); lua_pop(L_, 1); return value; } int LuaParameterDictionary::GetInt(const string& key) { CheckHasKeyAndReference(key); GetValueFromLuaTable(L_, key); return PopInt(); } int LuaParameterDictionary::PopInt() const { CHECK(lua_isnumber(L_, -1)) << "Top of stack is not a number value."; const int value = lua_tointeger(L_, -1); lua_pop(L_, 1); return value; } bool LuaParameterDictionary::GetBool(const string& key) { CheckHasKeyAndReference(key); GetValueFromLuaTable(L_, key); return PopBool(); } bool LuaParameterDictionary::PopBool() const { CHECK(lua_isboolean(L_, -1)) << "Top of stack is not a boolean value."; const bool value = lua_toboolean(L_, -1); lua_pop(L_, 1); return value; } std::unique_ptr LuaParameterDictionary::GetDictionary( const string& key) { CheckHasKeyAndReference(key); GetValueFromLuaTable(L_, key); return PopDictionary(reference_count_); } std::unique_ptr LuaParameterDictionary::PopDictionary( ReferenceCount reference_count) const { CheckTableIsAtTopOfStack(L_); std::unique_ptr value( new LuaParameterDictionary(L_, reference_count, file_resolver_)); // The constructor lua_xmove()s the value, no need to pop it. CheckTableIsAtTopOfStack(L_); return value; } string LuaParameterDictionary::DoToString(const string& indent) const { string result = "{"; bool dictionary_is_empty = true; const auto top_of_stack_to_string = [this, indent, &dictionary_is_empty]() -> string { dictionary_is_empty = false; const int value_type = lua_type(L_, -1); switch (value_type) { case LUA_TBOOLEAN: return PopBool() ? "true" : "false"; break; case LUA_TSTRING: return PopString(Quoted::YES); break; case LUA_TNUMBER: { const double value = PopDouble(); if (std::isinf(value)) { return value < 0 ? "-math.huge" : "math.huge"; } else { return std::to_string(value); } } break; case LUA_TTABLE: { std::unique_ptr subdict( PopDictionary(ReferenceCount::NO)); return subdict->DoToString(indent + " "); } break; default: LOG(FATAL) << "Unhandled type " << lua_typename(L_, value_type); } }; // Integer (array) keys. for (int i = 1; i; ++i) { GetValueFromLuaTable(L_, i); if (lua_isnil(L_, -1)) { lua_pop(L_, 1); break; } result.append("\n"); result.append(indent); result.append(" "); result.append(top_of_stack_to_string()); result.append(","); } // String keys. std::vector keys = GetKeys(); if (!keys.empty()) { std::sort(keys.begin(), keys.end()); for (const string& key : keys) { GetValueFromLuaTable(L_, key); result.append("\n"); result.append(indent); result.append(" "); result.append(key); result.append(" = "); result.append(top_of_stack_to_string()); result.append(","); } } result.append("\n"); result.append(indent); result.append("}"); if (dictionary_is_empty) { return "{}"; } return result; } string LuaParameterDictionary::ToString() const { return DoToString(""); } std::vector LuaParameterDictionary::GetArrayValuesAsDoubles() { std::vector values; GetArrayValues(L_, [&values, this] { values.push_back(PopDouble()); }); return values; } std::vector> LuaParameterDictionary::GetArrayValuesAsDictionaries() { std::vector> values; GetArrayValues(L_, [&values, this] { values.push_back(PopDictionary(reference_count_)); }); return values; } std::vector LuaParameterDictionary::GetArrayValuesAsStrings() { std::vector values; GetArrayValues(L_, [&values, this] { values.push_back(PopString(Quoted::NO)); }); return values; } void LuaParameterDictionary::CheckHasKey(const string& key) const { CHECK(HasKey(key)) << "Key '" << key << "' not in dictionary:\n" << ToString(); } void LuaParameterDictionary::CheckHasKeyAndReference(const string& key) { CheckHasKey(key); reference_counts_[key]++; } void LuaParameterDictionary::CheckAllKeysWereUsedExactlyOnceAndReset() { for (const auto& key : GetKeys()) { CHECK_EQ(1, reference_counts_.count(key)) << "Key '" << key << "' was used the wrong number of times."; CHECK_EQ(1, reference_counts_.at(key)) << "Key '" << key << "' was used the wrong number of times."; } reference_counts_.clear(); } int LuaParameterDictionary::GetNonNegativeInt(const string& key) { const int temp = GetInt(key); // Will increase reference count. CHECK_GE(temp, 0) << temp << " is negative."; return temp; } // Lua function to run a script in the current Lua context. Takes the filename // as its only argument. int LuaParameterDictionary::LuaInclude(lua_State* L) { CHECK_EQ(lua_gettop(L), 1); CHECK(lua_isstring(L, -1)) << "include takes a filename."; LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); const string basename = lua_tostring(L, -1); const string filename = parameter_dictionary->file_resolver_->GetFullPathOrDie(basename); if (std::find(parameter_dictionary->included_files_.begin(), parameter_dictionary->included_files_.end(), filename) != parameter_dictionary->included_files_.end()) { string error_msg = "Tried to include " + filename + " twice. Already included files in order of inclusion: "; for (const string& filename : parameter_dictionary->included_files_) { error_msg.append(filename); error_msg.append("\n"); } LOG(FATAL) << error_msg; } parameter_dictionary->included_files_.push_back(filename); lua_pop(L, 1); CHECK_EQ(lua_gettop(L), 0); const string content = parameter_dictionary->file_resolver_->GetFileContentOrDie(basename); CheckForLuaErrors( L, luaL_loadbuffer(L, content.c_str(), content.size(), filename.c_str())); CheckForLuaErrors(L, lua_pcall(L, 0, LUA_MULTRET, 0)); return lua_gettop(L); } // Lua function to read a file into a string. int LuaParameterDictionary::LuaRead(lua_State* L) { CHECK_EQ(lua_gettop(L), 1); CHECK(lua_isstring(L, -1)) << "read takes a filename."; LuaParameterDictionary* parameter_dictionary = GetDictionaryFromRegistry(L); const string file_content = parameter_dictionary->file_resolver_->GetFileContentOrDie( lua_tostring(L, -1)); lua_pushstring(L, file_content.c_str()); return 1; } } // namespace common } // namespace cartographer ================================================ FILE: common/lua_parameter_dictionary.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ #define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ #include #include #include #include #include "cartographer/common/lua.h" #include "cartographer/common/port.h" #include "glog/logging.h" namespace cartographer { namespace common { /* FileResolver是一个文件解析的虚基类,利用lua加载文件内容 */ // Resolves file paths and file content for the Lua 'read' and 'include' // functions. Use this to configure where those functions load other files from. class FileResolver { public: virtual ~FileResolver() {} virtual string GetFullPathOrDie(const string& basename) = 0; virtual string GetFileContentOrDie(const string& basename) = 0; }; /* LuaParameterDictionary类继承自FileResolver, 目的是从lua配置文件加载参数.该类不可拷贝不可赋值. 1,构造函数从file_resolver中加载一个带lua table的字典dictionary 2, code是以字符串代表的lua代码code */ // A parameter dictionary that gets loaded from Lua code. class LuaParameterDictionary { public: // Constructs the dictionary from a Lua Table specification. LuaParameterDictionary(const string& code, std::unique_ptr file_resolver); LuaParameterDictionary(const LuaParameterDictionary&) = delete; LuaParameterDictionary& operator=(const LuaParameterDictionary&) = delete; // Constructs a LuaParameterDictionary without reference counting. static std::unique_ptr NonReferenceCounted( const string& code, std::unique_ptr file_resolver); ~LuaParameterDictionary(); // Returns all available keys. std::vector GetKeys() const; // Returns true if the key is in this dictionary. bool HasKey(const string& key) const; // These methods CHECK() that the 'key' exists. string GetString(const string& key); double GetDouble(const string& key); int GetInt(const string& key); bool GetBool(const string& key); std::unique_ptr GetDictionary(const string& key); // Gets an int from the dictionary and CHECK()s that it is non-negative. int GetNonNegativeInt(const string& key); // Returns a string representation for this LuaParameterDictionary. string ToString() const; // Returns the values of the keys '1', '2', '3' as the given types. std::vector GetArrayValuesAsDoubles(); std::vector GetArrayValuesAsStrings(); std::vector> GetArrayValuesAsDictionaries(); private: enum class ReferenceCount { YES, NO }; LuaParameterDictionary(const string& code, ReferenceCount reference_count, std::unique_ptr file_resolver); // For GetDictionary(). LuaParameterDictionary(lua_State* L, ReferenceCount reference_count, std::shared_ptr file_resolver); // Function that recurses to keep track of indent for ToString(). string DoToString(const string& indent) const; // Pop the top of the stack and CHECKs that the type is correct. double PopDouble() const; int PopInt() const; bool PopBool() const; // Pop the top of the stack and CHECKs that it is a string. The returned value // is either quoted to be suitable to be read back by a Lua interpretor or // not. enum class Quoted { YES, NO }; string PopString(Quoted quoted) const; // Creates a LuaParameterDictionary from the Lua table at the top of the // stack, either with or without reference counting. std::unique_ptr PopDictionary( ReferenceCount reference_count) const; // CHECK() that 'key' is in the dictionary. void CheckHasKey(const string& key) const; // CHECK() that 'key' is in this dictionary and reference it as being used. void CheckHasKeyAndReference(const string& key); // If desired, this can be called in the destructor of a derived class. It // will CHECK() that all keys defined in the configuration have been used // exactly once and resets the reference counter. void CheckAllKeysWereUsedExactlyOnceAndReset(); // Reads a file into a Lua string. static int LuaRead(lua_State* L); // Handles inclusion of other Lua files and prevents double inclusion. static int LuaInclude(lua_State* L); lua_State* L_; // The name is by convention in the Lua World. int index_into_reference_table_; // This is shared with all the sub dictionaries. const std::shared_ptr file_resolver_; // If true will check that all keys were used on destruction. const ReferenceCount reference_count_; // This is modified with every call to Get* in order to verify that all // parameters are read exactly once. std::map reference_counts_; // List of all included files in order of inclusion. Used to prevent double // inclusion. std::vector included_files_; }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_H_ ================================================ FILE: common/lua_parameter_dictionary_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/common/lua_parameter_dictionary.h" #include #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "gtest/gtest.h" namespace cartographer { namespace common { namespace { /*返回一个智能指针,指向DummyFileResolver*/ std::unique_ptr MakeNonReferenceCounted( const string& code) { return LuaParameterDictionary::NonReferenceCounted( code, common::make_unique()); } class LuaParameterDictionaryTest : public ::testing::Test { protected: void ReferenceAllKeysAsIntegers(LuaParameterDictionary* dict) { for (const string& key : dict->GetKeys()) { dict->GetInt(key); } } }; TEST_F(LuaParameterDictionaryTest, GetInt) { auto dict = MakeDictionary("return { blah = 100 }"); ASSERT_EQ(dict->GetInt("blah"), 100); // blah==100 } TEST_F(LuaParameterDictionaryTest, GetString) { auto dict = MakeDictionary("return { blah = 'is_a_string' }\n"); ASSERT_EQ(dict->GetString("blah"), "is_a_string");//blah = 'is_a_string' } TEST_F(LuaParameterDictionaryTest, GetDouble) { auto dict = MakeDictionary("return { blah = 3.1415 }"); ASSERT_DOUBLE_EQ(dict->GetDouble("blah"), 3.1415); //blah = 3.1415 } TEST_F(LuaParameterDictionaryTest, GetBoolTrue) { auto dict = MakeDictionary("return { blah = true }"); ASSERT_TRUE(dict->GetBool("blah")); //blah = true } TEST_F(LuaParameterDictionaryTest, GetBoolFalse) { auto dict = MakeDictionary("return { blah = false }"); ASSERT_FALSE(dict->GetBool("blah")); // blah = false } TEST_F(LuaParameterDictionaryTest, GetDictionary) { auto dict = MakeDictionary("return { blah = { blue = 100, red = 200 }, fasel = 10 }"); //sub_dict 是一个子字典 std::unique_ptr sub_dict(dict->GetDictionary("blah")); std::vector keys = sub_dict->GetKeys();//blah下的key ASSERT_EQ(keys.size(), 2); //2个key,blue,red std::sort(keys.begin(), keys.end()); //顺序不定 ASSERT_EQ(keys[0], "blue"); ASSERT_EQ(keys[1], "red"); ASSERT_TRUE(sub_dict->HasKey("blue")); ASSERT_TRUE(sub_dict->HasKey("red")); ASSERT_EQ(sub_dict->GetInt("blue"), 100); //blue==100 ASSERT_EQ(sub_dict->GetInt("red"), 200); //red==10 ASSERT_EQ(dict->GetString("fasel"), "10"); } TEST_F(LuaParameterDictionaryTest, GetKeys) { auto dict = MakeDictionary("return { blah = 100, blah1 = 200}"); std::vector keys = dict->GetKeys(); //返回key组成的string ASSERT_EQ(keys.size(), 2); std::sort(keys.begin(), keys.end()); ASSERT_EQ(keys[0], "blah"); ASSERT_EQ(keys[1], "blah1"); ReferenceAllKeysAsIntegers(dict.get()); } TEST_F(LuaParameterDictionaryTest, NonExistingKey) { auto dict = MakeDictionary("return { blah = 100 }"); ReferenceAllKeysAsIntegers(dict.get()); ASSERT_DEATH(dict->GetInt("blah_fasel"), "Key.* not in dictionary.");//key不存在 } TEST_F(LuaParameterDictionaryTest, UintNegative) { auto dict = MakeDictionary("return { blah = -100}"); ASSERT_DEATH(dict->GetNonNegativeInt("blah"), ".*-100 is negative.");//key对应的value是负数 ReferenceAllKeysAsIntegers(dict.get()); } TEST_F(LuaParameterDictionaryTest, ToString) { auto dict = MakeDictionary(R"(return { ceta = { yolo = "hurray" }, fasel = 1234.456786, fasel1 = -math.huge, fasel2 = math.huge, blubber = 123, blub = 'hello', alpha = true, alpha1 = false, })"); const string golden = R"({ alpha = true, alpha1 = false, blub = "hello", blubber = 123.000000, ceta = { yolo = "hurray", }, fasel = 1234.456786, fasel1 = -math.huge, fasel2 = math.huge, })"; EXPECT_EQ(golden, dict->ToString()); //相等,dict 开始的return 不包含在内 auto dict1 = MakeDictionary("return " + dict->ToString()); EXPECT_EQ(dict1->GetBool("alpha"), true); EXPECT_EQ(dict1->GetBool("alpha1"), false); EXPECT_EQ(dict1->GetInt("blubber"), 123); //获取整数 EXPECT_EQ(dict1->GetString("blub"), "hello"); EXPECT_EQ(dict1->GetDictionary("ceta")->GetString("yolo"), "hurray"); EXPECT_NEAR(dict1->GetDouble("fasel"), 1234.456786, 1e-3); EXPECT_TRUE(std::isinf(-dict1->GetDouble("fasel1"))); EXPECT_TRUE(std::isinf(dict1->GetDouble("fasel2"))); EXPECT_EQ(dict->GetBool("alpha"), true); EXPECT_EQ(dict->GetBool("alpha1"), false); EXPECT_EQ(dict->GetInt("blubber"), 123); EXPECT_EQ(dict->GetString("blub"), "hello"); EXPECT_EQ(dict->GetDictionary("ceta")->GetString("yolo"), "hurray"); EXPECT_NEAR(dict->GetDouble("fasel"), 1234.456786, 1e-3); EXPECT_TRUE(std::isinf(-dict->GetDouble("fasel1"))); EXPECT_TRUE(std::isinf(dict->GetDouble("fasel2"))); } TEST_F(LuaParameterDictionaryTest, ToStringForArrays) { auto dict = MakeNonReferenceCounted( R"(return { "blub", 3, 3.1, foo = "ups", })"); const string golden = R"({ "blub", 3.000000, 3.100000, foo = "ups", })"; EXPECT_EQ(golden, dict->ToString()); } TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsStrings) { auto dict = MakeDictionary("return { 'a', 'b', 'c' }"); EXPECT_EQ(0, dict->GetKeys().size()); const std::vector values = dict->GetArrayValuesAsStrings(); EXPECT_EQ(3, values.size()); EXPECT_EQ("a", values[0]); EXPECT_EQ("b", values[1]); EXPECT_EQ("c", values[2]); } TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDoubles) { auto dict = MakeDictionary("return { 1., 2., 3. }"); EXPECT_EQ(0, dict->GetKeys().size()); const std::vector values = dict->GetArrayValuesAsDoubles(); EXPECT_EQ(3, values.size()); EXPECT_NEAR(1., values[0], 1e-7); EXPECT_NEAR(2., values[1], 1e-7); EXPECT_NEAR(3., values[2], 1e-7); } TEST_F(LuaParameterDictionaryTest, GetArrayValuesAsDictionaries) { auto dict = MakeDictionary("return { { a = 1 }, { b = 3 } }"); EXPECT_EQ(0, dict->GetKeys().size()); const std::vector> values = dict->GetArrayValuesAsDictionaries(); EXPECT_EQ(2, values.size()); EXPECT_EQ(1., values[0]->GetInt("a")); EXPECT_EQ(3., values[1]->GetInt("b")); } TEST_F(LuaParameterDictionaryTest, TestChooseTrue) { auto dict = MakeDictionary("return { a = choose(true, 1, 0) }"); EXPECT_EQ(1, dict->GetInt("a")); } TEST_F(LuaParameterDictionaryTest, TestChoseFalse) { auto dict = MakeDictionary("return { a = choose(false, 1, 0) }"); EXPECT_EQ(0, dict->GetInt("a")); } TEST_F(LuaParameterDictionaryTest, TestChooseInvalidArgument) { EXPECT_DEATH(MakeDictionary("return { a = choose('truish', 1, 0) }"), "condition is not a boolean value."); } } // namespace } // namespace common } // namespace cartographer ================================================ FILE: common/lua_parameter_dictionary_test_helpers.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ #define CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "glog/logging.h" namespace cartographer { namespace common { class DummyFileResolver : public FileResolver { public: DummyFileResolver() {} DummyFileResolver(const DummyFileResolver&) = delete; DummyFileResolver& operator=(const DummyFileResolver&) = delete; ~DummyFileResolver() override {} string GetFileContentOrDie(const string& unused_basename) override { LOG(FATAL) << "Not implemented"; } string GetFullPathOrDie(const string& unused_basename) override { LOG(FATAL) << "Not implemented"; } }; std::unique_ptr MakeDictionary(const string& code) { return common::make_unique( code, common::make_unique()); } } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_LUA_PARAMETER_DICTIONARY_TEST_HELPERS_H_ ================================================ FILE: common/make_unique.h ================================================ #ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ #define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ #include #include #include #include namespace cartographer { namespace common { /* make_unique.h在不支持c++14的环境下实现 std::make_unique的功能 实现细节:完美转发和移动语义 */ // Implementation of c++14's std::make_unique, taken from // https://isocpp.org/files/papers/N3656.txt template struct _Unique_if { typedef std::unique_ptr _Single_object; }; template struct _Unique_if { typedef std::unique_ptr _Unknown_bound; //不支持数组(不定长) }; template struct _Unique_if { //不支持数组(定长) typedef void _Known_bound; }; template typename _Unique_if::_Single_object make_unique(Args&&... args) { return std::unique_ptr(new T(std::forward(args)...)); //完美转发参数 } template typename _Unique_if::_Unknown_bound make_unique(size_t n) { typedef typename std::remove_extent::type U; return std::unique_ptr(new U[n]()); } template typename _Unique_if::_Known_bound make_unique(Args&&...) = delete;//不能使用定长数组 } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ /* 完美转发: 完美的传递函数参数而不修改参数类型，左值依然是左值，右值依然是右值。 */ ================================================ FILE: common/make_unique_test.cc ================================================ #include #include #include "cartographer/common/make_unique.h" #include "gtest/gtest.h" //using namespace std; using std::cout; using std::cin; using std::endl; using std::string; namespace cartographer { namespace common { TEST(MAKE_UNIQUE ,make_unique) { cout << *make_unique() << endl; cout << *make_unique(1729) << endl; cout << "\"" << *make_unique() << "\"" << endl; cout << "\"" << *make_unique("meow") << "\"" << endl; cout << "\"" << *make_unique(6, 'z') << "\"" << endl; auto up = make_unique(5); for (int i = 0; i < 5; ++i) { cout << up[i] << " "; } cout << endl; #if defined(ERROR1) auto up1 = make_unique("error"); #elif defined(ERROR2) auto up2 = make_unique(10, 20, 30, 40); #elif defined(ERROR3) auto up3 = make_unique(); #elif defined(ERROR4) auto up4 = make_unique(11, 22, 33, 44, 55); #endif } /* 输出: 0 1729 "" "meow" "zzzzzz" 0 0 0 0 0 */ } } ================================================ FILE: common/math.h ================================================ /* common/math.h文件主要实现数学计算，包括： 区间截断.求n次方.求平方.幅度角度转换.归一化.反正切值 */ #ifndef CARTOGRAPHER_COMMON_MATH_H_ #define CARTOGRAPHER_COMMON_MATH_H_ #include #include #include "Eigen/Core" #include "cartographer/common/port.h" #include "ceres/ceres.h" namespace cartographer { namespace common { // Clamps 'value' to be in the range ['min', 'max']. //将val截取到区间min至max中. template T Clamp(const T value, const T min, const T max) { if (value > max) { return max; } if (value < min) { return min; } return value; } // Calculates 'base'^'exponent'. 计算base的exp次方 template constexpr T Power(T base, int exponent) { return (exponent != 0) ? base * Power(base, exponent - 1) : T(1); } // Calculates a^2. 特化,求平方 template constexpr T Pow2(T a) { return Power(a, 2); } // Converts from degrees to radians.角度到弧度的转换. 60° -> pi/3 constexpr double DegToRad(double deg) { return M_PI * deg / 180.; } // Converts form radians to degrees.弧度到角度的转换, pi/3 -> 60° constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; } // Bring the 'difference' between two angles into [-pi; pi] //将角度差转换为[-pi;pi] template T NormalizeAngleDifference(T difference) { while (difference > M_PI) { difference -= T(2. * M_PI); } while (difference < -M_PI) { difference += T(2. * M_PI); } return difference; } /* atan2 返回原点至点(x,y)的方位角，即与 x 轴的夹角， 也可以理解为计算复数 x+yi 的辐角,范围是[-pi,pi] ATAN2(1,1) -> pi/4:以弧度表示点(1,1)的反正切值，即pi/4(0.785398) */ template T atan2(const Eigen::Matrix& vector) { //范围是[-pi,pi] return ceres::atan2(vector.y(), vector.x()); } } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_MATH_H_ ================================================ FILE: common/math_test.cc ================================================ #include "cartographer/common/math.h" #include "gtest/gtest.h" namespace cartographer { namespace common { namespace { TEST(MathTest, testPower) { EXPECT_EQ(0., Power(0, 42));// 0的42次方==0 EXPECT_EQ(1., Power(0, 0)); //0^0 ==0 EXPECT_EQ(1., Power(1, 0)); // 1^0 ==0 EXPECT_EQ(1., Power(1, 42));//1^42==1 EXPECT_EQ(4., Power(2, 2)); //2^2==4 } TEST(MathTest, testPow2) { EXPECT_EQ(0., Pow2(0)); //0^2==0 EXPECT_EQ(1., Pow2(1)); //1^2==1 EXPECT_EQ(4., Pow2(2)); //2^2==4 EXPECT_EQ(49., Pow2(7));//7^2==49 } TEST(MathTest, testDeg2rad) { EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9); // 180° ==pi EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6);//360° ==2pi } TEST(MathTest, testRad2deg) { EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9); //pi ==180° EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6);//2pi ==360° } TEST(MathTest, testNormalizeAngleDifference) { EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9); //0==0 EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9); //pi==oi EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9); //-pi==-pi EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9); //2pi==0 EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9); //5pi==pi EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9);//-5pi=-pi } } // namespace } // namespace common } // namespace cartographer ================================================ FILE: common/mutex.h ================================================ /* common/mutex.h主要是对c++11 的mutex的封装。 */ #ifndef CARTOGRAPHER_COMMON_MUTEX_H_ #define CARTOGRAPHER_COMMON_MUTEX_H_ #include #include #include "cartographer/common/time.h" namespace cartographer { namespace common { /*不使用clang时,以下宏定义全部为空操作.故看见宏定义时可暂时忽略*/ // Enable thread safety attributes only with clang. // The attributes can be safely erased when compiling with other compilers. #if defined(__SUPPORT_TS_ANNOTATION__) || defined(__clang__) #define THREAD_ANNOTATION_ATTRIBUTE__(x) __attribute__((x)) #else #define THREAD_ANNOTATION_ATTRIBUTE__(x) // no-op,空操作 #endif #define CAPABILITY(x) THREAD_ANNOTATION_ATTRIBUTE__(capability(x)) #define SCOPED_CAPABILITY THREAD_ANNOTATION_ATTRIBUTE__(scoped_lockable) #define GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(guarded_by(x)) #define PT_GUARDED_BY(x) THREAD_ANNOTATION_ATTRIBUTE__(pt_guarded_by(x)) #define REQUIRES(...) \ THREAD_ANNOTATION_ATTRIBUTE__(requires_capability(__VA_ARGS__)) #define ACQUIRE(...) \ THREAD_ANNOTATION_ATTRIBUTE__(acquire_capability(__VA_ARGS__)) #define RELEASE(...) \ THREAD_ANNOTATION_ATTRIBUTE__(release_capability(__VA_ARGS__)) #define EXCLUDES(...) THREAD_ANNOTATION_ATTRIBUTE__(locks_excluded(__VA_ARGS__)) #define NO_THREAD_SAFETY_ANALYSIS \ THREAD_ANNOTATION_ATTRIBUTE__(no_thread_safety_analysis) /* Mutex是c++11 mutex的封装， Mutex类有一个内部类Locker。 Locker 是一个RAII的类型. 在构造Locker时,对mutex上锁,在析构Locker时对mutex解锁. 本质是使用std::unique_lock和std::condition_variable实现的 Locker类提供2个成员函数Await()和AwaitWithTimeout() 功能是利用c++11的条件变量和unique_lock实现在谓词predicate为真的情况下对mutex解锁。 */ // Defines an annotated mutex that can only be locked through its scoped locker // implementation. class CAPABILITY("mutex") Mutex { public: // A RAII class that acquires a mutex in its constructor, and // releases it in its destructor. It also implements waiting functionality on // conditions that get checked whenever the mutex is released. class SCOPED_CAPABILITY Locker { public: Locker(Mutex* mutex) ACQUIRE(mutex) : mutex_(mutex), lock_(mutex->mutex_) {}//上锁 ~Locker() RELEASE() { lock_.unlock(); //解锁 mutex_->condition_.notify_all();//条件变量通知解锁 } template void Await(Predicate predicate) REQUIRES(this) { /*wait()实质是分3步： 1.对lock_解锁 2.等待predicate谓词为真，此时调用端阻塞。 3.对lock_ 重新上锁。 */ mutex_->condition_.wait(lock_, predicate); } template bool AwaitWithTimeout(Predicate predicate, common::Duration timeout) REQUIRES(this) { //等待谓词为真或者直到超时timeout返回。 return mutex_->condition_.wait_for(lock_, timeout, predicate); } private: Mutex* mutex_; std::unique_lock lock_; }; private: std::condition_variable condition_; std::mutex mutex_; }; using MutexLocker = Mutex::Locker; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_MUTEX_H_ /* 谓词.断言.Predicate: 一个用于比较的函数（或函数对象），输入两个T类型，返回bool类型。 */ ================================================ FILE: common/port.h ================================================ /* common-port.h文件主要实现2大功能： 1，使用std::lround对浮点数进行四舍五入取整运算 2，利用boost的iostreams/filter/gzip对字符串压缩与解压缩 */ #ifndef CARTOGRAPHER_COMMON_PORT_H_ #define CARTOGRAPHER_COMMON_PORT_H_ #include #include #include #include #include //包含多种解压与压缩算法 #include //配合filter实现流过滤 using int8 = int8_t; using int16 = int16_t; using int32 = int32_t; using int64 = int64_t; using uint8 = uint8_t; using uint16 = uint16_t; using uint32 = uint32_t; using uint64 = uint64_t; using std::string; namespace cartographer { namespace common { /* lround(+2.3) = 2 lround(+2.5) = 3 lround(+2.7) = 3 lround(-2.3) = -2 lround(-2.5) = -3 lround(-2.7) = -3 lround(-0.0) = 0 */ inline int RoundToInt(const float x) { return std::lround(x); }//四舍五入取整运算 inline int RoundToInt(const double x) { return std::lround(x); } inline int64 RoundToInt64(const float x) { return std::lround(x); } inline int64 RoundToInt64(const double x) { return std::lround(x); } /* 利用gzip_compressor对string进行压缩，第一参数是未压缩string，第二参数是完成压缩string */ inline void FastGzipString(const string& uncompressed, string* compressed) { boost::iostreams::filtering_ostream out;//创建过滤流 out.push( boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));//使用快速压缩算法 out.push(boost::iostreams::back_inserter(*compressed));//对compressed 使用后插迭代器 boost::iostreams::write(out, reinterpret_cast(uncompressed.data()), uncompressed.size());//压缩 char *,插入compressed } /* 利用gzip_decompressor解压缩string，第一参数是待解压的string，第二参数是解压后的string */ inline void FastGunzipString(const string& compressed, string* decompressed) { boost::iostreams::filtering_ostream out; //创建过滤流 out.push(boost::iostreams::gzip_decompressor());//指定解压缩算法 out.push(boost::iostreams::back_inserter(*decompressed));//对decompressed使用后插入迭代器 boost::iostreams::write(out, reinterpret_cast(compressed.data()), compressed.size());//解压缩char*，插入decompressed } } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_PORT_H_ /* c++11 已经支持back_inserter。 std::back_inserter执行push_back操作, 返回值back_insert_iterator, 并实现自增. 具体用法可见 port_le_test.cpp */ /* reinterpret_cast的转换格式：reinterpret_cast (expression) 允许将任何指针类型转换为指定的指针类型 */ /* boost::iostreams/filtering_ostream用法： 压缩时 filtering_ostream out; out.push(gzip_compressor()); //gzip OutputFilter out.push(bzip2_compressor());//bzip2 OutputFilter out.push(boost::iostreams::file_sink("test.txt"));//以file_sink device结束 将流的数据先按gzip压缩，然后再按bzip2压缩之后，才输出到text.txt文件中。 解压时 filtering_istream in; in.push(gzip_decompressor());/gzip InputFilter in.push(bzip2_decompressor());/bzip2 InputFilter in.push(file_source("test.txt")); 先将test.txt文件中数据读出，然后按bzip2解压，然后再按gzip解压，存入in流中，正好是压缩的逆序。 */ ================================================ FILE: common/port_le_test.cpp ================================================ #include #include #include /* c++11 已经支持back_inserter。 */ //#include using std::cout; using std::endl; int main(int argc, char *argv[]) { std::vector vec; for (int i = 0 ; i < 3; ++i) vec.push_back(i); std::vector ::iterator vIter; std::cout << "The initial vector vec is: ( "; for (vIter = vec.begin(); vIter != vec.end(); vIter++) std::cout << *vIter << " "; std::cout << ")." << std::endl; // Insertions can be done with template function std::back_insert_iterator > backiter(vec); *backiter = 30; backiter++; *backiter = 40; // Alternatively, insertions can be done with the // back_insert_iterator member function std::back_inserter(vec) = 500;//从vector后插入 std::back_inserter(vec) = 600; std::cout << "After the insertions, the vector vec is: ( "; for (vIter = vec.begin(); vIter != vec.end(); vIter++) std::cout << *vIter << " "; std::cout << ")." << std::endl<<"----\n\n"; std::vector firstvector, secondvector; for (int i=1; i<=5; i++) { firstvector.push_back(i); secondvector.push_back(i*10); } // 在firstvector向量后面加入secondvector的值. std::copy(secondvector.begin(), secondvector.end(), std::back_inserter(firstvector)); std::vector::iterator it; for ( it = firstvector.begin(); it!= firstvector.end(); ++it ) std::cout << *it << " "; std::cout << std::endl; return 0; } ================================================ FILE: common/proto/ceres_solver_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.common.proto; message CeresSolverOptions { // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ optional bool use_nonmonotonic_steps = 1; optional int32 max_num_iterations = 2; optional int32 num_threads = 3; } ================================================ FILE: common/rate_timer.h ================================================ #ifndef CARTOGRAPHER_COMMON_RATE_TIMER_H_ #define CARTOGRAPHER_COMMON_RATE_TIMER_H_ #include #include #include #include #include #include #include #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" /* * RateTimer是,脉冲频率计数类,计算在一段时间内的脉冲率 * RateTimer不可拷贝和赋值 * RateTimer只有一个构造函数,提供时间段Duration * ComputeRate()返回事件脉冲率,单位hz * ComputeWallTimeRateRatio()返回真实时间与墙上挂钟时间的比率 * 内部类Event封装了某个事件发生的时间点. * 调用一次Pulse()即产生了一次事件 * */ namespace cartographer { namespace common { // Computes the rate at which pulses come in.默认模板参数是steady_clock template class RateTimer { public: // Computes the rate at which pulses come in over 'window_duration' in wall // time. explicit RateTimer(const common::Duration window_duration) : window_duration_(window_duration) {} ~RateTimer() {} RateTimer(const RateTimer&) = delete; //不可拷贝/不可赋值 RateTimer& operator=(const RateTimer&) = delete; // Returns the pulse rate in Hz. double ComputeRate() const { //计算频率 if (events_.empty()) { return 0.; } //事件次数除以时间即为每秒钟发生多少次事件 return static_cast(events_.size() - 1) / common::ToSeconds((events_.back().time - events_.front().time)); //最晚发生的时间-最早发生的时间 (事件产生时的真实时间) } // Returns the ratio of the pulse rate (with supplied times) to the wall time // rate. For example, if a sensor produces pulses at 10 Hz, but we call Pulse // at 20 Hz wall time, this will return 2. double ComputeWallTimeRateRatio() const { //返回比率 if (events_.empty()) { return 0.; } return common::ToSeconds((events_.back().time - events_.front().time)) / //真实时间 //墙上挂钟时间,->调用Pulse时的系统的时间 std::chrono::duration_cast>( events_.back().wall_time - events_.front().wall_time) .count(); } // Records an event that will contribute to the computed rate. void Pulse(common::Time time) { //产生一个脉冲 events_.push_back(Event{time, ClockType::now()}); while (events_.size() > 2 && (events_.back().wall_time - events_.front().wall_time) > window_duration_) { events_.pop_front(); } } // Returns a debug string representation. string DebugString() const { if (events_.size() < 2) { return "unknown"; } std::ostringstream out; out << std::fixed << std::setprecision(2) << ComputeRate() << " Hz " << DeltasDebugString() << " (pulsed at " << ComputeWallTimeRateRatio() * 100. << "% real time)"; return out.str(); } private: struct Event { common::Time time; typename ClockType::time_point wall_time; }; // Computes all differences in seconds between consecutive pulses. std::vector ComputeDeltasInSeconds() const { CHECK_GT(events_.size(), 1); const size_t count = events_.size() - 1; std::vector result; result.reserve(count); for (size_t i = 0; i != count; ++i) { result.push_back( common::ToSeconds(events_[i + 1].time - events_[i].time)); } return result; } // Returns the average and standard deviation of the deltas. string DeltasDebugString() const { const auto deltas = ComputeDeltasInSeconds(); const double sum = std::accumulate(deltas.begin(), deltas.end(), 0.); const double mean = sum / deltas.size(); double squared_sum = 0.; for (const double x : deltas) { squared_sum += common::Pow2(x - mean); } const double sigma = std::sqrt(squared_sum / (deltas.size() - 1)); std::ostringstream out; out << std::scientific << std::setprecision(2) << mean << " s +/- " << sigma << " s"; return out.str(); } std::deque events_; const common::Duration window_duration_; }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_RATE_TIMER_H_ ================================================ FILE: common/rate_timer_test.cc ================================================ #include "cartographer/common/rate_timer.h" #include "gtest/gtest.h" namespace cartographer { namespace common { namespace { TEST(RateTimerTest, ComputeRate) { RateTimer<> rate_timer(common::FromSeconds(1.));//默认模板参数是steady_clock,设置时间段是1s common::Time time = common::FromUniversal(42); // 时间点,42us for (int i = 0; i < 100; ++i) { //每隔0.1s产生一次事件,产生100次 rate_timer.Pulse(time); time += common::FromSeconds(0.1); } EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3);//频率应该等于10次每s } struct SimulatedClock { /*模拟的时间clock类*/ using rep = std::chrono::steady_clock::rep; using period = std::chrono::steady_clock::period; using duration = std::chrono::steady_clock::duration; using time_point = std::chrono::steady_clock::time_point; static constexpr bool is_steady = true; static time_point time; static time_point now() noexcept { return time; } }; SimulatedClock::time_point SimulatedClock::time; TEST(RateTimerTest, ComputeWallTimeRateRatio) { common::Time time = common::FromUniversal(42); //时间点 42us RateTimer rate_timer(common::FromSeconds(1.)); //时间段是1s for (int i = 0; i < 100; ++i) { //100次事件,0.1s一次. rate_timer.Pulse(time); time += common::FromSeconds(0.1); //加0.1s,代表事件发生的时间(调用了Pulse) SimulatedClock::time += //模仿的时间,加0.05s,代表流失了0.05秒. std::chrono::duration_cast( std::chrono::duration(0.05)); } EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); //0.1/0.05==2 } } // namespace } // namespace common } // namespace cartographer ================================================ FILE: common/thread_pool.cc ================================================ #include "cartographer/common/thread_pool.h" #include #include #include #include #include "glog/logging.h" namespace cartographer { namespace common { /* pool_是vector,每个线程初始化时,执行DoWork()函数. */ ThreadPool::ThreadPool(int num_threads) { MutexLocker locker(&mutex_); for (int i = 0; i != num_threads; ++i) { pool_.emplace_back([this]() { ThreadPool::DoWork(); }); } } /* 只有线程池的work_queue_=0时,才能析构,此时所有的线程已经开始被pool执行, 只有等待pool结束所有的线程执行函数(join结束),ThreadPool才能析构完成 */ ThreadPool::~ThreadPool() { { MutexLocker locker(&mutex_); CHECK(running_); running_ = false; CHECK_EQ(work_queue_.size(), 0); } for (std::thread& thread : pool_) { thread.join(); } } /* 使想要线程池执行的work_item函数加入到等待队列中,然后排队等待空闲线程"领取"该函数. */ void ThreadPool::Schedule(std::function work_item) { MutexLocker locker(&mutex_); CHECK(running_); work_queue_.push_back(work_item); } /* 每个线程都要执行这个函数. ThreadPool利用条件变量通知此函数,work_queue_不为零时,说明有需要执行的函数,此时加入执行 */ void ThreadPool::DoWork() { #ifdef __linux__ // This changes the per-thread nice level of the current thread on Linux. We // do this so that the background work done by the thread pool is not taking // away CPU resources from more important foreground threads. CHECK_NE(nice(10), -1); #endif for (;;) { std::function work_item; { //加锁,同一时刻只能一个线程"领取"某个函数,何时执行这个函数呢? // 最后一行即为抢到的线程开始执行work_item MutexLocker locker(&mutex_); locker.Await([this]() REQUIRES(mutex_) { return !work_queue_.empty() || !running_; //注意,队列不为空或者不在running_状态,条件变量都需要通知该函数 }); if (!work_queue_.empty()) { work_item = work_queue_.front(); //领取 work_queue_.pop_front(); //删除 } else if (!running_) { return; } } CHECK(work_item); work_item(); } } } // namespace common } // namespace cartographer ================================================ FILE: common/thread_pool.h ================================================ #ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_ #define CARTOGRAPHER_COMMON_THREAD_POOL_H_ #include #include #include #include #include "cartographer/common/mutex.h" namespace cartographer { namespace common { /* ThreadPool 是对c++11 thread的封装. ThreadPool是线程数量固定的线程池，不可拷贝 和复制. 1，构造函数ThreadPool(int num_threads) 初始化一个线程数量固定的线程池。 2，Schedule(std::function work_item)添加想要ThreadPool执行的函数, std::thread会在线程后台依次排队执行相关函数. 3，数据成员pool_是具体的线程，work_queue_是待执行的函数队列。 */ // A fixed number of threads working on a work queue of work items. Adding a // new work item does not block, and will be executed by a background thread // eventually. The queue must be empty before calling the destructor. The thread // pool will then wait for the currently executing work items to finish and then // destroy the threads. class ThreadPool { public: explicit ThreadPool(int num_threads); ~ThreadPool(); ThreadPool(const ThreadPool&) = delete; ThreadPool& operator=(const ThreadPool&) = delete; void Schedule(std::function work_item); private: void DoWork(); Mutex mutex_; //互斥锁，保证线程安全 //running_只是一个监视哨,只有线程池在running_状态时,才能往work_queue_加入函数. bool running_ GUARDED_BY(mutex_) = true; std::vector pool_ GUARDED_BY(mutex_); std::deque> work_queue_ GUARDED_BY(mutex_); }; } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_ ================================================ FILE: common/time.cc ================================================ #include "cartographer/common/time.h" #include namespace cartographer { namespace common { //duration_cast是c++ 11的时间显式转换函数. Duration FromSeconds(const double seconds) { return std::chrono::duration_cast( //将double类型的秒数转化为duration对象 std::chrono::duration(seconds)); } double ToSeconds(const Duration duration) { //反转化,count()返回时钟周期数,ticks return std::chrono::duration_cast>(duration) .count(); } //先构造一个临时duration对象,再将其转化为time_point对象 //Duration(ticks)调用的是UniversalTimeScaleClock的构造函数 Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); } //count()返回time_point自epoch以来的时钟周期数 int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); } //先将Time转化为 int64 , 再转为字符串形式 std::ostream& operator<<(std::ostream& os, const Time time) { os << std::to_string(ToUniversal(time)); return os; } //mill是ms,micro是us,先将毫秒转为以毫秒计时的duration对象,再转化为以微妙计. common::Duration FromMilliseconds(const int64 milliseconds) { return std::chrono::duration_cast( std::chrono::milliseconds(milliseconds)); } } // namespace common } // namespace cartographer ================================================ FILE: common/time.h ================================================ /* 预备知识: c++11 提供了语言级别的时间函数.包括duration和time_point duration是时间段,指的是某单位时间上的一个明确的tick(片刻数)： 3分钟->"3个1分钟", 1.5个"1/3秒" :1.5是tick,1/3秒是时间单位 time_point是一个duration和一个epoch(起点)的组合： 2017年5月4日是"自1970,01,01"以来的126200000秒数 common/time.h主要功能是提供时间转换函数： */ #ifndef CARTOGRAPHER_COMMON_TIME_H_ #define CARTOGRAPHER_COMMON_TIME_H_ #include #include #include #include "cartographer/common/port.h" namespace cartographer { namespace common { //719162 是0001年1月1日到1970年1月1日所经历的天数 constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds = (719162ll * 24ll * 60ll * 60ll); struct UniversalTimeScaleClock { using rep = int64; using period = std::ratio<1, 10000000>; //百万分之一秒,1us --》错了，应该是0.1us. //以下涉及到us的均应该纠正为0.1us using duration = std::chrono::duration; using time_point = std::chrono::time_point; /*time_point的模板参数是UniversalTimeScaleClock, 那为何其可以做模板参数呢：？符合std::关于clock的类型定义和static成员*/ static constexpr bool is_steady = true; }; // Represents Universal Time Scale durations and timestamps which are 64-bit // integers representing the 100 nanosecond ticks since the Epoch which is // January 1, 1 at the start of day in UTC. using Duration = UniversalTimeScaleClock::duration;//微秒,us using Time = UniversalTimeScaleClock::time_point; //时间点 /*Time::min()是chrono自带的函数。返回一个低于1970.01.01的数。 编译运行cpp/cppstdlib_2nd/util/chrono1.cpp: epoch: Thu Jan 1 08:00:00 1970 now: Tue Jul 4 19:39:29 2017 min: Tue Sep 21 08:18:27 1677 max: Sat Apr 12 07:47:16 2262 */ // Convenience functions to create common::Durations. //将秒数seconds转为c++的duration实例对象 Duration FromSeconds(double seconds); Duration FromMilliseconds(int64 milliseconds); //毫秒 // Returns the given duration in seconds. //将的duration实例对象转为 秒数 double ToSeconds(Duration duration); // Creates a time from a Universal Time Scale. //将TUC时间(微秒)转化为c++的time_point对象 Time FromUniversal(int64 ticks); // Outputs the Universal Time Scale timestamp for a given Time. //将c++的time_point对象转为TUC时间,单位是us int64 ToUniversal(Time time); // For logging and unit tests, outputs the timestamp integer. //重载<<操作符,将time_point以string输出 std::ostream& operator<<(std::ostream& os, Time time); } // namespace common } // namespace cartographer #endif // CARTOGRAPHER_COMMON_TIME_H_ /* linux下关于time转换： http://blog.chinaunix.net/uid-20532339-id-1931780.html https://stackoverflow.com/questions/2883576/how-do-you-convert-epoch-time-in-c/7844741#7844741 https://www.epochconverter.com/batch*/ ================================================ FILE: ground_truth/compute_relations_metrics_main.cc ================================================ #include #include #include #include #include #include #include #include #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "cartographer/transform/transform_interpolation_buffer.h" #include "gflags/gflags.h" #include "glog/logging.h" DEFINE_string( trajectory_filename, "", "Proto containing the trajectory of which to assess the quality."); DEFINE_string(relations_filename, "", "Relations file containing the ground truth."); namespace cartographer { namespace ground_truth { namespace { transform::Rigid3d LookupPose(const transform::TransformInterpolationBuffer& transform_interpolation_buffer, const double time) { constexpr int64 kUtsTicksPerSecond = 10000000; common::Time common_time = common::FromUniversal(common::kUtsEpochOffsetFromUnixEpochInSeconds * kUtsTicksPerSecond) + common::FromSeconds(time); return transform_interpolation_buffer.Lookup(common_time); } struct Error { double translational_squared; double rotational_squared; }; Error ComputeError(const transform::Rigid3d& pose1, const transform::Rigid3d& pose2, const transform::Rigid3d& expected) { const transform::Rigid3d error = (pose1.inverse() * pose2) * expected.inverse(); return Error{error.translation().squaredNorm(), common::Pow2(transform::GetAngle(error))}; } string MeanAndStdDevString(const std::vector& values) { CHECK_GE(values.size(), 2); const double mean = std::accumulate(values.begin(), values.end(), 0.) / values.size(); double sum_of_squared_differences = 0.; for (const double value : values) { sum_of_squared_differences += common::Pow2(value - mean); } const double standard_deviation = std::sqrt(sum_of_squared_differences / (values.size() - 1)); std::ostringstream out; out << std::fixed << std::setprecision(5) << mean << " +/- " << standard_deviation; return string(out.str()); } string StatisticsString(const std::vector& errors) { std::vector translational_errors; std::vector squared_translational_errors; std::vector rotational_errors_degrees; std::vector squared_rotational_errors_degrees; for (const Error& error : errors) { translational_errors.push_back(std::sqrt(error.translational_squared)); squared_translational_errors.push_back(error.translational_squared); rotational_errors_degrees.push_back( common::RadToDeg(std::sqrt(error.rotational_squared))); squared_rotational_errors_degrees.push_back( common::Pow2(rotational_errors_degrees.back())); } return "Abs translational error " + MeanAndStdDevString(translational_errors) + " m\n" "Sqr translational error " + MeanAndStdDevString(squared_translational_errors) + " m^2\n" "Abs rotational error " + MeanAndStdDevString(rotational_errors_degrees) + " deg\n" "Sqr rotational error " + MeanAndStdDevString(squared_rotational_errors_degrees) + " deg^2\n"; } void Run(const string& trajectory_filename, const string& relations_filename) { LOG(INFO) << "Reading trajectory from '" << trajectory_filename << "'..."; mapping::proto::Trajectory trajectory_proto; { std::ifstream trajectory_stream(trajectory_filename.c_str(), std::ios::binary); CHECK(trajectory_proto.ParseFromIstream(&trajectory_stream)); } const auto transform_interpolation_buffer = transform::TransformInterpolationBuffer::FromTrajectory(trajectory_proto); LOG(INFO) << "Reading relations from '" << relations_filename << "'..."; std::vector errors; { std::ifstream relations_stream(relations_filename.c_str()); double time1, time2, x, y, z, roll, pitch, yaw; while (relations_stream >> time1 >> time2 >> x >> y >> z >> roll >> pitch >> yaw) { const auto pose1 = LookupPose(*transform_interpolation_buffer, time1); const auto pose2 = LookupPose(*transform_interpolation_buffer, time2); const transform::Rigid3d expected = transform::Rigid3d(transform::Rigid3d::Vector(x, y, z), transform::RollPitchYaw(roll, pitch, yaw)); errors.push_back(ComputeError(pose1, pose2, expected)); } CHECK(relations_stream.eof()); } LOG(INFO) << "Result:\n" << StatisticsString(errors); } } // namespace } // namespace ground_truth } // namespace cartographer int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); FLAGS_logtostderr = true; google::SetUsageMessage( "\n\n" "This program computes the relation based metric described in:\n" "R. Kuemmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti,\n" "C. Stachniss, and A. Kleiner, \"On measuring the accuracy of SLAM\n" "algorithms,\" Autonomous Robots, vol. 27, no. 4, pp. 387–407, 2009.\n" "\n" "Note: Timestamps in the relations_file are interpreted relative to\n" " the Unix epoch."); google::ParseCommandLineFlags(&argc, &argv, true); if (FLAGS_trajectory_filename.empty() || FLAGS_relations_filename.empty()) { google::ShowUsageWithFlagsRestrict(argv[0], "compute_relations_metrics"); return EXIT_FAILURE; } ::cartographer::ground_truth::Run(FLAGS_trajectory_filename, FLAGS_relations_filename); } ================================================ FILE: io/cairo_types.h ================================================ #ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_ #define CARTOGRAPHER_IO_CAIRO_TYPES_H_ #include #include "cairo/cairo.h" /* Cairo是一个2D图形库，支持多种输出设备。 cairo 是一个免费的矢量绘图软件库，它可以绘制多种输出格式。 https://www.ibm.com/developerworks/cn/linux/l-cairo/ http://liyanrui.is-programmer.com/2009/3/18/cairo-tutorial-05.7742.html http://blog.csdn.net/turingo/article/details/8131057 */ namespace cartographer { namespace io { namespace cairo { //智能指针管理对象生命期.在unique_ptr销毁时释放指向的对象资源. //对象类型是cairo_surface_t,自定义删除器deleter是函数指针. // std::unique_ptr for Cairo surfaces. The surface is destroyed when the // std::unique_ptr is reset or destroyed. using UniqueSurfacePtr = std::unique_ptr; //对象类型是cairo_t,自定义删除器deleter是函数指针. // std::unique_ptr for Cairo contexts. The context is destroyed when the // std::unique_ptr is reset or destroyed. using UniqueContextPtr = std::unique_ptr; //对象类型是cairo_path_t,自定义删除器deleter是函数指针. // std::unique_ptr for Cairo paths. The path is destroyed when the // std::unique_ptr is reset or destroyed. using UniquePathPtr = std::unique_ptr; } // namespace cairo } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_ ================================================ FILE: io/coloring_points_processor.cc ================================================ #include "cartographer/io/coloring_points_processor.h" #include "Eigen/Core" #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace io { /* 根据assets_writer_backpack_2d.lua配置:.lua文件概览: -- Now we recolor our points by frame and write another batch of X-Rays. It -- is visible in them what was seen by the horizontal and the vertical -- laser. { action = "color_points", frame_id = "horizontal_laser_link", color = { 255., 0., 0. }, }, { action = "color_points", frame_id = "vertical_laser_link", color = { 0., 255., 0. }, }, FromDictionary()根据.lua配置文件获取frame_id下color的{r,g,b} */ std::unique_ptr ColoringPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { /* frame_id:horizontal_laser_link或者vertical_laser_link color:{ 255., 0., 0. },或者{ 0., 255., 0. }, */ const string frame_id = dictionary->GetString("frame_id"); const std::vector color_values = dictionary->GetDictionary("color")->GetArrayValuesAsDoubles(); const Color color = {{static_cast(color_values[0]), static_cast(color_values[1]), static_cast(color_values[2])}}; return common::make_unique(color, frame_id, next); } ColoringPointsProcessor::ColoringPointsProcessor(const Color& color, const string& frame_id, PointsProcessor* const next) : color_(color), frame_id_(frame_id), next_(next) {} /* 只对相同的frame_id_处理：着色 */ void ColoringPointsProcessor::Process(std::unique_ptr batch) { if (batch->frame_id == frame_id_) { batch->colors.clear(); for (size_t i = 0; i < batch->points.size(); ++i) { batch->colors.push_back(color_);//用color 填充batch的colors } } next_->Process(std::move(batch)); } PointsProcessor::FlushResult ColoringPointsProcessor::Flush() { return next_->Flush(); } } // namespace io } // namespace cartographer ================================================ FILE: io/coloring_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* { action = "color_points", frame_id = "horizontal_laser_link", color = { 255., 0., 0. }, }, { action = "color_points", frame_id = "vertical_laser_link", color = { 0., 255., 0. }, }, ColoringPointsProcessor是PointsProcessor的第六子类(6). 功能: 用固定的Color填充PointsBatch的Color分量。 数据成员: 1),color_：rgb值 2),frame_id_:只有相同的id才填充Color，color一般为[255,0,0],[0,255,0] 2),next_下一阶段的PointsProcessor. */ // Colors points with a fixed color by frame_id. class ColoringPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "color_points"; ColoringPointsProcessor(const Color& color, const string& frame_id, PointsProcessor* next); static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~ColoringPointsProcessor() override{}; ColoringPointsProcessor(const ColoringPointsProcessor&) = delete; ColoringPointsProcessor& operator=(const ColoringPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: const Color color_; const string frame_id_; PointsProcessor* const next_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ ================================================ FILE: io/counting_points_processor.cc ================================================ #include "cartographer/io/counting_points_processor.h" #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace io { //初始化是num是0 CountingPointsProcessor::CountingPointsProcessor(PointsProcessor* next) : num_points_(0), next_(next) {} //与其他FromDictionary()函数统一写法 std::unique_ptr CountingPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique(next); } //不处理points,而是将num_points_加上batch.size(),，即统计点云数据。然后直接流水线到下一PointsProcessor void CountingPointsProcessor::Process(std::unique_ptr batch) { num_points_ += batch->points.size();//相加 next_->Process(std::move(batch)); } //依据下一阶段的状态重置本阶段的状态。 PointsProcessor::FlushResult CountingPointsProcessor::Flush() { switch (next_->Flush()) { case FlushResult::kFinished: LOG(INFO) << "Processed " << num_points_ << " and finishing."; return FlushResult::kFinished; case FlushResult::kRestartStream: LOG(INFO) << "Processed " << num_points_ << " and restarting stream."; num_points_ = 0; //重启,则置为0 return FlushResult::kRestartStream; } LOG(FATAL); } } // namespace io } // namespace cartographer ================================================ FILE: io/counting_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* PointsProcessor的第二个子类(2). 功能:记录有多少 point被输出处理 数据成员: 1),num_points_记录points数量 2),next_下一阶段的PointsProcessor. */ // Passes through points, but keeps track of how many points it saw and output // that on Flush. class CountingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "dump_num_points"; explicit CountingPointsProcessor(PointsProcessor* next); static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~CountingPointsProcessor() override {} CountingPointsProcessor(const CountingPointsProcessor&) = delete; CountingPointsProcessor& operator=(const CountingPointsProcessor&) = delete; void Process(std::unique_ptr points) override; FlushResult Flush() override; private: int64 num_points_; PointsProcessor* next_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ ================================================ FILE: io/file_writer.cc ================================================ #include "cartographer/io/file_writer.h" namespace cartographer { namespace io { //out:File open for writing: the internal stream buffer supports output operations. //binary:Operations are performed in binary mode rather than text. StreamFileWriter::StreamFileWriter(const string& filename) : out_(filename, std::ios::out | std::ios::binary) {}//打开,并以2进制方式写入 StreamFileWriter::~StreamFileWriter() {} //写入len个char bool StreamFileWriter::Write(const char* const data, const size_t len) { if (out_.bad()) { return false; } out_.write(data, len); return !out_.bad(); } bool StreamFileWriter::Close() { //关闭文件 if (out_.bad()) { return false; } out_.close(); return !out_.bad(); } bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) { if (out_.bad()) { return false; } out_.flush(); //清空buffer out_.seekp(0); //偏移量为0 return Write(data, len); } } // namespace io } // namespace cartographer ================================================ FILE: io/file_writer.h ================================================ #ifndef CARTOGRAPHER_IO_FILE_WRITER_H_ #define CARTOGRAPHER_IO_FILE_WRITER_H_ #include #include #include #include "cartographer/common/port.h" namespace cartographer { namespace io { /* FileWriter负责文件写入的虚基类,不可拷贝/赋值 没有数据成员.只提供一系列抽象接口. 1),WriteHeader(),写入文件head 2),Write(),写入数据 3),Close(),关闭文件 */ // Simple abstraction for a file. class FileWriter { public: FileWriter() {} FileWriter(const FileWriter&) = delete; FileWriter& operator=(const FileWriter&) = delete; virtual ~FileWriter() {} // Write 'data' to the beginning of the file. This is required to overwrite // fixed size headers which contain the number of points once we actually know // how many points there are. virtual bool WriteHeader(const char* data, size_t len) = 0; virtual bool Write(const char* data, size_t len) = 0; virtual bool Close() = 0; }; /* StreamFileWriter 文件流写入类,继承自FileWriter 数据成员只有一个std::ofstream out_负责将文件写入磁盘 */ // An Implementation of file using std::ofstream. class StreamFileWriter : public FileWriter { public: ~StreamFileWriter() override; StreamFileWriter(const string& filename); bool Write(const char* data, size_t len) override; bool WriteHeader(const char* data, size_t len) override; //从文件开始处写入,覆盖旧数据 bool Close() override; private: std::ofstream out_; }; /*工厂模式, 创建一个FileWriter对象,由智能指针管理生命期, 返回值是std::unique_ptr, 函数参数是string. */ using FileWriterFactory = std::function(const string& filename)>; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_FILE_WRITER_H_ ================================================ FILE: io/fixed_ratio_sampling_points_processor.cc ================================================ #include "cartographer/io/fixed_ratio_sampling_points_processor.h" #include "Eigen/Core" #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace io { /* 静态成员函数,利用参数构造一个智能指针指向FixedRatioSamplingPointsProcessor的对象 */ std::unique_ptr FixedRatioSamplingPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { const double sampling_ratio(dictionary->GetDouble("sampling_ratio")); //sparse_pose_graph.lua:0.3,0.03 CHECK_LT(0., sampling_ratio) << "Sampling ratio <= 0 makes no sense."; CHECK_LT(sampling_ratio, 1.) << "Sampling ratio >= 1 makes no sense."; return common::make_unique(sampling_ratio, next); } /* 构造函数用采样率 sampling_ratio和PointsProcessor* next初始化 */ FixedRatioSamplingPointsProcessor::FixedRatioSamplingPointsProcessor( const double sampling_ratio, PointsProcessor* next) : sampling_ratio_(sampling_ratio), next_(next), sampler_(new common::FixedRatioSampler(sampling_ratio_)) {} /* 该类的核心函数,有几个为了提高效率的地方: Process()传递智能指针,指向堆上的点集PointsBatch, 理论上该点集组成的vector是不断增加的.但是同时也有其他Process()在删除PointsBatch */ void FixedRatioSamplingPointsProcessor::Process( std::unique_ptr batch) { std::vector to_remove; //保存索引 for (size_t i = 0; i < batch->points.size(); ++i) { //points是vector if (!sampler_->Pulse()) { //调用一次Pulse()其实影响到了sampler_的内部状态. //模拟的传感器sampler_没有产生一个脉冲,则说明当前的batch[i]应该删除. to_remove.push_back(i); } } RemovePoints(to_remove, batch.get()); next_->Process(std::move(batch));//虚函数,调用子类的Process(),也就是这个函数本身 } PointsProcessor::FlushResult FixedRatioSamplingPointsProcessor::Flush() { switch (next_->Flush()) { case PointsProcessor::FlushResult::kFinished: //下一步已经完成,析构时则这一步也标记完成 return PointsProcessor::FlushResult::kFinished; case PointsProcessor::FlushResult::kRestartStream://重启采样 //为何要重新赋值?:重启,不能带有以前的状态,所以重新初始化. sampler_ = common::make_unique(sampling_ratio_); return PointsProcessor::FlushResult::kRestartStream; } LOG(FATAL); } } // namespace io } // namespace cartographer ================================================ FILE: io/fixed_ratio_sampling_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ #include #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* FixedRatioSamplingPointsProcessor是PointsProcessor的第七个子类(7). FixedRatioSamplingPointsProcessor是采样过滤器, 该class只让具有固定采样频率的点通过,其余全部 remove. sampling_ratio=1,即无任何操作,全通滤波. sparse_pose_graph.lua: sampling_ratio = 0.3, */ // Only let a fixed 'sampling_ratio' of points through. A 'sampling_ratio' of 1. // makes this filter a no-op. class FixedRatioSamplingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "fixed_ratio_sampler"; FixedRatioSamplingPointsProcessor(double sampling_ratio, PointsProcessor* next); static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~FixedRatioSamplingPointsProcessor() override{}; FixedRatioSamplingPointsProcessor(const FixedRatioSamplingPointsProcessor&) = delete; FixedRatioSamplingPointsProcessor& operator=( const FixedRatioSamplingPointsProcessor&) = delete; //根据采样率采集点云，不在采样点上的全部删除。 void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: const double sampling_ratio_;//采样率 PointsProcessor* const next_; std::unique_ptr sampler_;//具有固定采样率的采样函数 }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_FIXED_RATIO_SAMPLING_POINTS_PROCESSOR_H_ ================================================ FILE: io/intensity_to_color_points_processor.cc ================================================ #include "cartographer/io/intensity_to_color_points_processor.h" #include "Eigen/Core" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "glog/logging.h" namespace cartographer { namespace io { std::unique_ptr IntensityToColorPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { const string frame_id = dictionary->HasKey("frame_id") ? dictionary->GetString("frame_id") : ""; const float min_intensity = dictionary->GetDouble("min_intensity"); const float max_intensity = dictionary->GetDouble("max_intensity"); return common::make_unique( min_intensity, max_intensity, frame_id, next); } IntensityToColorPointsProcessor::IntensityToColorPointsProcessor( const float min_intensity, const float max_intensity, const string& frame_id, PointsProcessor* const next) : min_intensity_(min_intensity), max_intensity_(max_intensity), frame_id_(frame_id), next_(next) {} void IntensityToColorPointsProcessor::Process( std::unique_ptr batch) { if (!batch->intensities.empty() && (frame_id_.empty() || batch->frame_id == frame_id_)) { batch->colors.clear(); for (const float intensity : batch->intensities) { const uint8_t gray = cartographer::common::Clamp( (intensity - min_intensity_) / (max_intensity_ - min_intensity_), 0.f, 1.f) * 255; batch->colors.push_back(Color{{gray, gray, gray}}); } } next_->Process(std::move(batch)); } PointsProcessor::FlushResult IntensityToColorPointsProcessor::Flush() { return next_->Flush(); } } // namespace io } // namespace cartographer ================================================ FILE: io/intensity_to_color_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_batch.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* { action = "intensity_to_color", min_intensity = 0., max_intensity = 4095., }, PointsProcessor的第四个子类(4). 功能:将光强度转换为color Intensity,图像亮度 IntensityToColorPointsProcessor：处理强度到color point 的强度变换类 成员函数执行转换:('intensity' - min ) / (max - min) * 255 */ class IntensityToColorPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "intensity_to_color"; // Applies ('intensity' - min ) / (max - min) * 255 and color the point grey // with this value for each point that comes from the sensor with 'frame_id'. // If 'frame_id' is empty, this applies to all points. IntensityToColorPointsProcessor(float min_intensity, float max_intensity, const string& frame_id, PointsProcessor* next); static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~IntensityToColorPointsProcessor() override{}; IntensityToColorPointsProcessor(const IntensityToColorPointsProcessor&) = delete; IntensityToColorPointsProcessor& operator=( const IntensityToColorPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: const float min_intensity_; const float max_intensity_; const string frame_id_; PointsProcessor* const next_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_INTENSITY_TO_COLOR_POINTS_PROCESSOR_H_ ================================================ FILE: io/min_max_range_filtering_points_processor.cc ================================================ #include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { //从.lua文件加载min_range和max_range,如:0-30 std::unique_ptr MinMaxRangeFiteringPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), //trajectory_builder_2d.lua:0,30 //assets_writer_backpack_2d.lua 1-60 next); } /* 构造函数,传递min ,max2个范围参数和 PointsProcessor* next指针 */ MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( const double min_range, const double max_range, PointsProcessor* next) : min_range_(min_range), max_range_(max_range), next_(next) {} void MinMaxRangeFiteringPointsProcessor::Process( std::unique_ptr batch) { std::vector to_remove; //待移除的索引 for (size_t i = 0; i < batch->points.size(); ++i) { //计算sensor到远点的l2距离 const float range = (batch->points[i] - batch->origin).norm(); if (!(min_range_ <= range && range <= max_range_)) { //不在给定范围内,加入移除队列. to_remove.push_back(i); } } RemovePoints(to_remove, batch.get()); next_->Process(std::move(batch));//完成本轮过滤,接下来进行下一次过滤.即模拟流水线操作. } PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() { return next_->Flush(); } } // namespace io } // namespace cartographer ================================================ FILE: io/min_max_range_filtering_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* PointsProcessor的第一个子类(1). 功能:继承自PointsProcessor点云虚基类.距离过滤器,L2 距离不在Min-Max范围内的都过滤掉. 数据成员: 1),min_range_ 最小范围 2),max_range_ 最大范围 3),PointsProcessor* const next_:完成本轮Processor,接下来进行下一次Processor. */ // Filters all points that are farther away from their 'origin' as 'max_range' // or closer than 'min_range'. class MinMaxRangeFiteringPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "min_max_range_filter"; MinMaxRangeFiteringPointsProcessor(double min_range, double max_range, PointsProcessor* next); //从.lua文件加载min_range和max_range,如:0-30 static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~MinMaxRangeFiteringPointsProcessor() override {}//覆盖 MinMaxRangeFiteringPointsProcessor( const MinMaxRangeFiteringPointsProcessor&) = delete; MinMaxRangeFiteringPointsProcessor& operator=( const MinMaxRangeFiteringPointsProcessor&) = delete; //点云处理,删除[min,max]以外的point，并把数据传递到下一轮Processor处理。 void Process(std::unique_ptr batch) override; FlushResult Flush() override; //调用next_->Flush();父类调用Flush(),但在运行时才会决定是否调用子类的Flush() private: const double min_range_; const double max_range_; PointsProcessor* const next_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_MIN_MAX_RANGE_FILTERING_POINTS_PROCESSOR_H_ ================================================ FILE: io/null_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* NullPointsProcessor是PointsProcessor的第十个子类(10) 空操作类。通常用于pipline的尾端，丢弃所有的点云，表示整个处理流程已经完毕。 */ // A points processor that just drops all points. The end of a pipeline usually. class NullPointsProcessor : public PointsProcessor { public: NullPointsProcessor() {} ~NullPointsProcessor() override {} //不作任何事情 void Process(std::unique_ptr points_batch) override {} //返回"kFinished"状态，此操作会传递给上一个流水线。 FlushResult Flush() override { return FlushResult::kFinished; } }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ ================================================ FILE: io/outlier_removing_points_processor.cc ================================================ #include "cartographer/io/outlier_removing_points_processor.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace io { /* VOXEL_SIZE = 5e-2 */ std::unique_ptr OutlierRemovingPointsProcessor::FromDictionary( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( dictionary->GetDouble("voxel_size"), next); //构造一个对象,返回一个智能指针 } /* 构造函数,传递一个 voxel_size 和 PointsProcessor* next */ OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor( const double voxel_size, PointsProcessor* next) : voxel_size_(voxel_size), next_(next), state_(State::kPhase1), voxels_(voxel_size_) { LOG(INFO) << "Marking hits..."; } /* 根据3个不同的state分别处理 points */ void OutlierRemovingPointsProcessor::Process( std::unique_ptr points) { switch (state_) { case State::kPhase1: ProcessInPhaseOne(*points); break; case State::kPhase2: ProcessInPhaseTwo(*points); break; case State::kPhase3: ProcessInPhaseThree(std::move(points)); break; } } /* 更新state,并返回 FlushResult结果 */ PointsProcessor::FlushResult OutlierRemovingPointsProcessor::Flush() { switch (state_) { case State::kPhase1: LOG(INFO) << "Counting rays..."; state_ = State::kPhase2; return FlushResult::kRestartStream; case State::kPhase2: LOG(INFO) << "Filtering outliers..."; state_ = State::kPhase3; return FlushResult::kRestartStream; case State::kPhase3: CHECK(next_->Flush() == FlushResult::kFinished) << "Voxel filtering and outlier removal must be configured to occur " "after any stages that require multiple passes."; // multiple passes：多次传输。 return FlushResult::kFinished; } LOG(FATAL); } /* state 1，统计光线的数量。 */ void OutlierRemovingPointsProcessor::ProcessInPhaseOne( const PointsBatch& batch) { for (size_t i = 0; i < batch.points.size(); ++i) { ++voxels_.mutable_value(voxels_.GetCellIndex(batch.points[i]))->hits; } } /* */ void OutlierRemovingPointsProcessor::ProcessInPhaseTwo( const PointsBatch& batch) { // TODO(whess): This samples every 'voxel_size' distance and could be improved // by better ray casting, and also by marking the hits of the current range // data to be excluded. for (size_t i = 0; i < batch.points.size(); ++i) { const Eigen::Vector3f delta = batch.points[i] - batch.origin; const float length = delta.norm(); for (float x = 0; x < length; x += voxel_size_) { const auto index = voxels_.GetCellIndex(batch.origin + (x / length) * delta); if (voxels_.value(index).hits > 0) { ++voxels_.mutable_value(index)->rays; } } } } void OutlierRemovingPointsProcessor::ProcessInPhaseThree( std::unique_ptr batch) { constexpr double kMissPerHitLimit = 3; std::vector to_remove; for (size_t i = 0; i < batch->points.size(); ++i) { const auto voxel = voxels_.value(voxels_.GetCellIndex(batch->points[i])); if (!(voxel.rays < kMissPerHitLimit * voxel.hits)) { to_remove.push_back(i); } } RemovePoints(to_remove, batch.get()); next_->Process(std::move(batch)); } } // namespace io } // namespace cartographer ================================================ FILE: io/outlier_removing_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace io { /* OutlierRemovingPointsProcessor是PointsProcessor的第八个子类(8). OutlierRemovingPointsProcessor： 作用：体素过滤器,Remove有移动痕迹的体素。只保留没有移动的体素 数据成员： const double voxel_size_; PointsProcessor* const next_; State state_; mapping_3d::HybridGridBase voxels_; */ // Voxel filters the data and only passes on points that we believe are on // non-moving objects. class OutlierRemovingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "voxel_filter_and_remove_moving_objects"; OutlierRemovingPointsProcessor(double voxel_size, PointsProcessor* next); static std::unique_ptr FromDictionary( common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~OutlierRemovingPointsProcessor() override {} OutlierRemovingPointsProcessor(const OutlierRemovingPointsProcessor&) = delete; OutlierRemovingPointsProcessor& operator=( const OutlierRemovingPointsProcessor&) = delete; //删除移动的体素。 void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: // To reduce memory consumption by not having to keep all rays in memory, we // filter outliers in three phases each going over all data: First we compute // all voxels containing any hits, then we compute the rays passing through // each of these voxels, and finally we output all hits in voxels that are // considered obstructed. struct VoxelData { int hits = 0; int rays = 0; }; enum class State { kPhase1, kPhase2, kPhase3, }; // First phase counts the number of hits per voxel. void ProcessInPhaseOne(const PointsBatch& batch); // Second phase counts how many rays pass through each voxel. This is only // done for voxels that contain hits. This is to reduce memory consumption by // not adding data to free voxels. void ProcessInPhaseTwo(const PointsBatch& batch); // Third phase produces the output containing all inliers. We consider each // hit an inlier if it is inside a voxel that has a sufficiently high // hit-to-ray ratio. void ProcessInPhaseThree(std::unique_ptr batch); const double voxel_size_; //体素大小 PointsProcessor* const next_; State state_; mapping_3d::HybridGridBase voxels_; //包含多个体素的网格Grid。 }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_OUTLIER_REMOVING_POINTS_PROCESSOR_H_ ================================================ FILE: io/pcd_writing_points_processor.cc ================================================ #include "cartographer/io/pcd_writing_points_processor.h" #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/io/points_batch.h" #include "glog/logging.h" namespace cartographer { namespace io { namespace { /* PCD文件由文件头和数据域组成，文件头存储了点云数据的字段格式信息，如： ======================================== # .PCD v0.7 - Point Cloud Data file format VERSION 0.7 FIELDS x y z rgba SIZE 4 4 4 4 TYPE F F F U COUNT 1 1 1 1 WIDTH 640 HEIGHT 480 VIEWPOINT 0 0 0 0 1 0 0 POINTS 307200 DATA binary //下面是点云数据段 123.1 21.1 64.0 ... ======================================== */ // Writes the PCD header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPcdHeader(const bool has_color, const int64 num_points, FileWriter* const file_writer) { string color_header_field = !has_color ? "" : " rgb"; string color_header_type = !has_color ? "" : " U"; string color_header_size = !has_color ? "" : " 4"; string color_header_count = !has_color ? "" : " 1"; std::ostringstream stream; stream << "# generated by Cartographer\n" << "VERSION .7\n" << "FIELDS x y z" << color_header_field << "\n" << "SIZE 4 4 4" << color_header_size << "\n" << "TYPE F F F" << color_header_type << "\n" << "COUNT 1 1 1" << color_header_count << "\n" << "WIDTH " << std::setw(15) << std::setfill('0') << num_points << "\n" << "HEIGHT 1\n" << "VIEWPOINT 0 0 0 1 0 0 0\n" << "POINTS " << std::setw(15) << std::setfill('0') << num_points << "\n" << "DATA binary\n"; const string out = stream.str(); file_writer->WriteHeader(out.data(), out.size()); } //{x,y,z}写入文件 void WriteBinaryPcdPointCoordinate(const Eigen::Vector3f& point, FileWriter* const file_writer) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); //64机,float:4位 memcpy(buffer + 8, &point[2], sizeof(float)); CHECK(file_writer->Write(buffer, 12)); } void WriteBinaryPcdPointColor(const Color& color, FileWriter* const file_writer) { char buffer[4]; buffer[0] = color[2]; buffer[1] = color[1]; buffer[2] = color[0]; buffer[3] = 0; CHECK(file_writer->Write(buffer, 4)); } } // namespace std::unique_ptr PcdWritingPointsProcessor::FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( file_writer_factory(dictionary->GetString("filename")), next); } PcdWritingPointsProcessor::PcdWritingPointsProcessor( std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), file_writer_(std::move(file_writer)) {} PointsProcessor::FlushResult PcdWritingPointsProcessor::Flush() { WriteBinaryPcdHeader(has_colors_, num_points_, file_writer_.get()); //调用Flush()才写入head信息:此时才知道num_points_的值 CHECK(file_writer_->Close()); switch (next_->Flush()) { case FlushResult::kFinished: return FlushResult::kFinished; case FlushResult::kRestartStream://不能多次传递同一数据. LOG(FATAL) << "PCD generation must be configured to occur after any " "stages that require multiple passes."; } LOG(FATAL); } /* 核心函数,按照{x,y,z}写入文件 */ void PcdWritingPointsProcessor::Process(std::unique_ptr batch) { if (batch->points.empty()) { next_->Process(std::move(batch)); return; } if (num_points_ == 0) { has_colors_ = !batch->colors.empty(); WriteBinaryPcdHeader(has_colors_, 0, file_writer_.get()); } for (size_t i = 0; i < batch->points.size(); ++i) { WriteBinaryPcdPointCoordinate(batch->points[i], file_writer_.get()); if (!batch->colors.empty()) { WriteBinaryPcdPointColor(batch->colors[i], file_writer_.get()); } ++num_points_;//统计处理了多少points } next_->Process(std::move(batch)); } } // namespace io } // namespace cartographer ================================================ FILE: io/pcd_writing_points_processor.h ================================================ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" /* 点云是目标表面特性的海量点集合。 根据激光测量原理得到的点云，包括三维坐标（XYZ）和激光反射强度（Intensity）。 根据摄影测量原理得到的点云，包括三维坐标（XYZ）和颜色信息（RGB）。 结合激光测量和摄影测量原理得到点云，包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）。 在获取物体表面每个采样点的空间坐标后，得到的是一个点的集合，称之为“点云”(Point Cloud)。 pcd文件存储了每个点的x,y,z坐标以及r,g,b,a颜色信息 */ namespace cartographer { namespace io { /* PcdWritingPointsProcessor是PointsProcessor的第九个子类(9) . PcdWritingPointsProcessor类将点云按照pcd格式存储在pcd文件中. 1,构造函数初始化一个文件类名, 2,成员函数FromDictionary 从配置文件读取 文件名 3,Process()和Flush()负责写入文件 4,数据成员next_指向 下一阶段的PointsProcessor点云处理类 */ // Streams a PCD file to disk. The header is written in 'Flush'. class PcdWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_pcd"; PcdWritingPointsProcessor(std::unique_ptr file_writer, PointsProcessor* next); static std::unique_ptr FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PcdWritingPointsProcessor() override {} PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete; PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: PointsProcessor* const next_; int64 num_points_; bool has_colors_; std::unique_ptr file_writer_; }; } // namespace io } // namespace cartographer ================================================ FILE: io/ply_writing_points_processor.cc ================================================ #include "cartographer/io/ply_writing_points_processor.h" #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/io/points_batch.h" #include "glog/logging.h" namespace cartographer { namespace io { namespace { /* 按照ply格式写入ply的head信息.此函数在最后写入. why?num_points_只有最后才知道.之前尚未统计完成. */ // Writes the PLY header claiming 'num_points' will follow it into // 'output_file'. void WriteBinaryPlyHeader(const bool has_color, const int64 num_points, FileWriter* const file_writer) { string color_header = !has_color ? "" : "property uchar red\n" "property uchar green\n" "property uchar blue\n"; std::ostringstream stream; stream << "ply\n" << "format binary_little_endian 1.0\n" << "comment generated by Cartographer\n" << "element vertex " << std::setw(15) << std::setfill('0') << num_points << "\n" << "property float x\n" << "property float y\n" << "property float z\n" << color_header << "end_header\n"; const string out = stream.str(); CHECK(file_writer->WriteHeader(out.data(), out.size())); } /* 往file_writer写入某一point对应的{x,y,z} */ void WriteBinaryPlyPointCoordinate(const Eigen::Vector3f& point, FileWriter* const file_writer) { char buffer[12]; memcpy(buffer, &point[0], sizeof(float)); memcpy(buffer + 4, &point[1], sizeof(float)); memcpy(buffer + 8, &point[2], sizeof(float)); CHECK(file_writer->Write(buffer, 12)); } /* 往file_writer写入rgb值 */ void WriteBinaryPlyPointColor(const Color& color, FileWriter* const file_writer) { CHECK(file_writer->Write(reinterpret_cast(color.data()), color.size())); } } // namespace /* 根据.lua文件的filename创建file_writer_factory */ std::unique_ptr PlyWritingPointsProcessor::FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( file_writer_factory(dictionary->GetString("filename")), next); } /* 构造函数,默认点云数是0,无rgb值 */ PlyWritingPointsProcessor::PlyWritingPointsProcessor( std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), num_points_(0), has_colors_(false), file_(std::move(file_writer)) {} /* 在最后阶段才写入head,但不是在文件最后写入head,而是out_.seekp(0); 偏移到0处 */ PointsProcessor::FlushResult PlyWritingPointsProcessor::Flush() { WriteBinaryPlyHeader(has_colors_, num_points_, file_.get()); CHECK(file_->Close()) << "Closing PLY file_writer failed."; switch (next_->Flush()) { case FlushResult::kFinished: return FlushResult::kFinished; case FlushResult::kRestartStream: LOG(FATAL) << "PLY generation must be configured to occur after any " "stages that require multiple passes."; } LOG(FATAL); } /* 真正的写入点云PointsBatch,整个类的核心在这个函数. */ void PlyWritingPointsProcessor::Process(std::unique_ptr batch) { if (batch->points.empty()) { //点云为空,则不写入磁盘,直接进行下一Process环节. next_->Process(std::move(batch)); return; } if (num_points_ == 0) {//点云num为0则写入rgb,和head has_colors_ = !batch->colors.empty(); WriteBinaryPlyHeader(has_colors_, 0, file_.get()); //file_.get()获取指针指针对应的raw指针 } if (has_colors_) { //rgb的vector长度和point的vecor长度必须一致. CHECK_EQ(batch->points.size(), batch->colors.size()) << "First PointsBatch had colors, but encountered one without. " "frame_id: " << batch->frame_id; } //函数核心,开始写入每一个point. for (size_t i = 0; i < batch->points.size(); ++i) { WriteBinaryPlyPointCoordinate(batch->points[i], file_.get()); if (has_colors_) { WriteBinaryPlyPointColor(batch->colors[i], file_.get()); } ++num_points_; } next_->Process(std::move(batch)); //继续流水线操作. } } // namespace io } // namespace cartographer ================================================ FILE: io/ply_writing_points_processor.h ================================================ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* PlyWritingPointsProcessor是PointsProcessor的第五个子类(5). PlyWritingPointsProcessor负责将点云point以PLY的格式写入磁盘. */ // Streams a PLY file to disk. The header is written in 'Flush'. class PlyWritingPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_ply"; // 指定文件写入的工厂和流水线next指针。 PlyWritingPointsProcessor(std::unique_ptr file, PointsProcessor* next); static std::unique_ptr FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~PlyWritingPointsProcessor() override {} PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete; PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) = delete; //按照ply格式写点云信息 。然后直接流水线到下一PointsProcessor void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: PointsProcessor* const next_; int64 num_points_; bool has_colors_;//是否是RGB std::unique_ptr file_; }; } // namespace io } // namespace cartographer ================================================ FILE: io/points_batch.cc ================================================ #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { /* 先对to_remove做降序排列,然后按照to_remove依次移除某些point. batch->points是vector,index 是要移除的vector元素的索引. 时间复杂度:o(m*n),m是to_remove的大小,n是to_remove[0]到batch的end()的距离. 注:erase()的复杂度是: Linear on the number of elements erased (destructions)* the number of elements after the last element deleted (moving). 返回值是一个迭代器，指向删除元素下一个元素; 使用erase()要注意迭代器失效的问题. 使用降序排序,即从end开始erase()可避免失效 */ void RemovePoints(std::vector to_remove, PointsBatch* batch) { std::sort(to_remove.begin(), to_remove.end(), std::greater());//降序排列 for (const int index : to_remove) { batch->points.erase(batch->points.begin() + index); //移除point if (!batch->colors.empty()) { batch->colors.erase(batch->colors.begin() + index); //point对应的rgb } if (!batch->intensities.empty()) { //移除光强度 batch->intensities.erase(batch->intensities.begin() + index); } } } } // namespace io } // namespace cartographer ================================================ FILE: io/points_batch.h ================================================ #ifndef CARTOGRAPHER_IO_POINTS_BATCH_H_ #define CARTOGRAPHER_IO_POINTS_BATCH_H_ #include #include #include #include "Eigen/Core" #include "cartographer/common/time.h" namespace cartographer { namespace io { // A point's color. using Color = std::array; /* PointsBatch类是对多个点云point的抽象.这些point在由“同一时刻”,“同一机器人坐标地点”的传感器采集而得。 数据成员主要描述了point的特性. 1,time:point采集时间. 2,origin:sensor的世界坐标 3,frame_id:帧id 4,trajectory_index:轨迹线id 5,points:几何参数,vector<{x,y,z}> 6,intensities:光强 7,colors:point的rgb值 */ // A number of points, captured around the same 'time' and by a // sensor at the same 'origin'. struct PointsBatch { PointsBatch() { origin = Eigen::Vector3f::Zero(); trajectory_index = 0; } // Time at which this batch has been acquired. point被采集的时间点 common::Time time; // Origin of the data, i.e. the location of the sensor in the world at // 'time'. 传感器位姿 Eigen::Vector3f origin; // Sensor that generated this data's 'frame_id' or empty if this information // is unknown. 关键帧的id string frame_id; // Trajectory index that produced this point. 轨迹线 int trajectory_index; // Geometry of the points in a metric frame. 公制，point的几何参数 std::vector points; // Intensities are optional and may be unspecified. The meaning of these // intensity values varies by device. For example, the VLP16 provides values // in the range [0, 100] for non-specular return values and values up to 255 // for specular returns. On the other hand, Hokuyo lasers provide a 16-bit // value that rarely peaks above 4096. std::vector intensities; //光强度，可选项 // Colors are optional. If set, they are RGB values.彩色rgb数字 std::vector colors; }; // Removes the indices in 'to_remove' from 'batch'. // 按照to_temove中的索引,在batch中移除某些point. void RemovePoints(std::vector to_remove, PointsBatch* batch); } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_POINTS_BATCH_H_ ================================================ FILE: io/points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ #include #include "cartographer/io/points_batch.h" namespace cartographer { namespace io { /* PointsProcessor 点云虚基类,提供一种批量处理points的抽象接口,不可拷贝/赋值 2个抽象接口: 1),Process()负责对PointsBatch进行处理 2),Flush()刷新. 在assets_writer_backpack_2d.lua文件中有各个pipeline的处理流程. */ // A processor in a pipeline. It processes a 'points_batch' and hands it to the // next processor in the pipeline. class PointsProcessor { public: enum class FlushResult { kRestartStream, kFinished, }; PointsProcessor() {} virtual ~PointsProcessor() {} //必须为虚函数,不然子类无法正确析构. PointsProcessor(const PointsProcessor&) = delete; PointsProcessor& operator=(const PointsProcessor&) = delete; // Receive a 'points_batch', process it and pass it on. virtual void Process(std::unique_ptr points_batch) = 0;//纯虚函数 // Some implementations will perform expensive computations and others that do // multiple passes over the data might ask for restarting the stream. virtual FlushResult Flush() = 0; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ ================================================ FILE: io/points_processor_pipeline_builder.cc ================================================ #include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/common/make_unique.h" #include "cartographer/io/coloring_points_processor.h" #include "cartographer/io/counting_points_processor.h" #include "cartographer/io/fixed_ratio_sampling_points_processor.h" #include "cartographer/io/intensity_to_color_points_processor.h" #include "cartographer/io/min_max_range_filtering_points_processor.h" #include "cartographer/io/null_points_processor.h" #include "cartographer/io/outlier_removing_points_processor.h" #include "cartographer/io/pcd_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/xray_points_processor.h" #include "cartographer/io/xyz_writing_points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { namespace io { /* 注册内置的points Processor处理类, 只有1个参数，类型是PointsProcessorPipelineBuilder。 具体做法是调用Builder的成员函数 Register(const std::string& name, FactoryFunction factory); 第一参数是kConfigurationFileActionName 第二参数是FactoryFunction,即为函数unique_ptr<> f(dictionary,next), 返回值是调用每个Processor的成员函数FromDictionary() 得到unique_ptr. */ template void RegisterPlainPointsProcessor( PointsProcessorPipelineBuilder* const builder) { builder->Register( PointsProcessorType::kConfigurationFileActionName, [](common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) -> std::unique_ptr//返回值是unique_ptr { return PointsProcessorType::FromDictionary(dictionary, next); }); } /* RegisterFileWritingPointsProcessor负责注册文件写入的Processor类。 用于将points写入到文件。 第一参数：管理文件写入的FileWriterFactory 第二参数：PointsProcessorPipelineBuilder* */ template void RegisterFileWritingPointsProcessor( FileWriterFactory file_writer_factory, PointsProcessorPipelineBuilder* const builder) { builder->Register( PointsProcessorType::kConfigurationFileActionName, [file_writer_factory]( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) -> std::unique_ptr //返回值是unique_ptr { return PointsProcessorType::FromDictionary(file_writer_factory, dictionary, next); }); } /*.h文件对外的接口,主要的代码逻辑 作用：依次注册所有的内置Processor类。 */ void RegisterBuiltInPointsProcessors( const mapping::proto::Trajectory& trajectory, FileWriterFactory file_writer_factory, PointsProcessorPipelineBuilder* builder) { //注册多个非文件相关的Processor实例对象 RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); RegisterPlainPointsProcessor(builder); //注册多个file Processor实例对象 RegisterFileWritingPointsProcessor( file_writer_factory, builder); RegisterFileWritingPointsProcessor( file_writer_factory, builder); RegisterFileWritingPointsProcessor( file_writer_factory, builder); //特殊：轨迹线传引用&id，用于区别不同建图楼层。 // X-Ray is an odd ball(古怪的，特殊的) since it requires the trajectory to figure out the // different building levels we walked on to separate the images. builder->Register( XRayPointsProcessor::kConfigurationFileActionName, [&trajectory, file_writer_factory] ( common::LuaParameterDictionary* const dictionary, PointsProcessor* const next ) -> std::unique_ptr { return XRayPointsProcessor::FromDictionary( trajectory, file_writer_factory, dictionary, next); }); } //将用name标识的class添加到hash表中 void PointsProcessorPipelineBuilder::Register(const std::string& name, FactoryFunction factory) { CHECK(factories_.count(name) == 0) << "A points processor named '" << name << "' has already been registered."; factories_[name] = factory; } PointsProcessorPipelineBuilder::PointsProcessorPipelineBuilder() {} /* 1）整个sensor文件夹下最核心的代码。根据.lua文件 用于创建pipeline流水线，登记所有的PointsProcessor类。 返回值pipeline 是points 处理类的集合。内含所有的class集合。 2）pipeline是vector，元素类型是unique_ptr,指向处理point的class。 3）第一个是空指针NullPointsProcessor类,最后调用,用于丢弃所有的points, 其余的按照逆序添加到pipeline中 */ std::vector> PointsProcessorPipelineBuilder::CreatePipeline( common::LuaParameterDictionary* const dictionary) const { std::vector> pipeline; // The last consumer in the pipeline must exist, so that the one created after // it (and being before it in the pipeline) has a valid 'next' to point to. // The last consumer will just drop all points. pipeline.emplace_back(common::make_unique()); std::vector> configurations = dictionary->GetArrayValuesAsDictionaries(); //按action逆序，rbegin()，从尾部添加元素到pipeline中。 // We construct the pipeline starting at the back. for (auto it = configurations.rbegin(); it != configurations.rend(); it++) { const string action = (*it)->GetString("action");//min_max_range_filter auto factory_it = factories_.find(action); //类型是pair<,>，second是min_max对应的FactoryFunction //只有在hash表中的action在可被添加：否则找不到函数实现。 CHECK(factory_it != factories_.end()) << "Unknown action '" << action << "'. Did you register the correspoinding PointsProcessor?"; pipeline.push_back(factory_it->second(it->get(), pipeline.back().get())); /*注意： 上述代码是vector.push_back( f(dict,next)); 内部是一个函数，返回unique_ptr,然后插入vector. vector.back()总是next的Processor。 */ } return pipeline; } } // namespace io } // namespace cartographer /* pipeline = { { action = "min_max_range_filter",--1 min_range = 1., max_range = 60., }, { action = "dump_num_points", --2 }, -- Gray X-Rays. These only use geometry to color pixels. { action = "write_xray_image", --3 voxel_size = VOXEL_SIZE, filename = "xray_yz_all", transform = YZ_TRANSFORM, }, .... } */ ================================================ FILE: io/points_processor_pipeline_builder.h ================================================ #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ #define CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/proto/trajectory.pb.h" namespace cartographer { namespace io { /* 使用Builder模式创建PointsProcessor对象： PointsProcessorPipelineBuilder类用于创建各个ProcessorPiont类对象 调用RegisterBuiltInPointsProcessors() 函数可用于创建所有cartographer内置的PointsProcessor类. */ // Builder to create a points processor pipeline out of a Lua configuration. // You can register all built-in PointsProcessors using // 'RegisterBuiltInPointsProcessors'. Non-built-in PointsProcessors must define // a name and a factory method for building itself from a // LuaParameterDictionary. See the various built-in PointsProcessors for // examples. class PointsProcessorPipelineBuilder { public: //FactoryFunction是函数f(dictionary,next); //返回值为std::unique_ptr using FactoryFunction = std::function ( common::LuaParameterDictionary*, PointsProcessor* next) >; PointsProcessorPipelineBuilder(); PointsProcessorPipelineBuilder(const PointsProcessorPipelineBuilder&) = delete; PointsProcessorPipelineBuilder& operator=( const PointsProcessorPipelineBuilder&) = delete; // Register a new PointsProcessor type uniquly identified by 'name' which will // be created using 'factory'. void Register(const std::string& name, FactoryFunction factory); std::vector> CreatePipeline( common::LuaParameterDictionary* dictionary) const; private: //hash表,记录已有代码实现的points processor及其对应的function //key是每个class都有的name。 std::unordered_map factories_; }; // Register all 'PointsProcessor' that ship with Cartographer with this // 'builder'. void RegisterBuiltInPointsProcessors( const mapping::proto::Trajectory& trajectory, FileWriterFactory file_writer_factory, PointsProcessorPipelineBuilder* builder); } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_PIPELINE_BUILDER_H_ ================================================ FILE: io/proto_stream.cc ================================================ #include "cartographer/io/proto_stream.h" namespace cartographer { namespace io { namespace { // First eight bytes to identify our proto stream format. const size_t kMagic = 0x7b1d1f7b5bf501db;//文件头标识符 void WriteSizeAsLittleEndian(size_t size, std::ostream* out) { for (int i = 0; i != 8; ++i) { out->put(size & 0xff); size >>= 8; } } bool ReadSizeAsLittleEndian(std::istream* in, size_t* size) { *size = 0; for (int i = 0; i != 8; ++i) { *size >>= 8; *size += static_cast(in->get()) << 56; } return !in->fail(); } } // namespace ProtoStreamWriter::ProtoStreamWriter(const string& filename) : out_(filename, std::ios::out | std::ios::binary) { WriteSizeAsLittleEndian(kMagic, &out_); } ProtoStreamWriter::~ProtoStreamWriter() {} void ProtoStreamWriter::Write(const string& uncompressed_data) { string compressed_data; common::FastGzipString(uncompressed_data, &compressed_data); WriteSizeAsLittleEndian(compressed_data.size(), &out_); out_.write(compressed_data.data(), compressed_data.size()); } bool ProtoStreamWriter::Close() { out_.close(); return !out_.fail(); } ProtoStreamReader::ProtoStreamReader(const string& filename) : in_(filename, std::ios::in | std::ios::binary) { size_t magic; if (!ReadSizeAsLittleEndian(&in_, &magic) || magic != kMagic) { in_.setstate(std::ios::failbit); } } ProtoStreamReader::~ProtoStreamReader() {} bool ProtoStreamReader::Read(string* decompressed_data) { size_t compressed_size; if (!ReadSizeAsLittleEndian(&in_, &compressed_size)) { return false; } string compressed_data(compressed_size, '\0'); if (!in_.read(&compressed_data.front(), compressed_size)) { return false; } common::FastGunzipString(compressed_data, decompressed_data); return true; } } // namespace io } // namespace cartographer ================================================ FILE: io/proto_stream.h ================================================ #ifndef CARTOGRAPHER_IO_PROTO_STREAM_H_ #define CARTOGRAPHER_IO_PROTO_STREAM_H_ #include #include "cartographer/common/port.h" namespace cartographer { namespace io { /* ProtoStreamWriter类用于将压缩的序列化数据protocol messages 写入文件 ProtoStreamReader类用于读取上述已经compressed的文件 */ // A simple writer of a compressed sequence of protocol buffer messages to a // file. The format is not intended to be compatible with any other format used // outside of Cartographer. // // TODO(whess): Compress the file instead of individual messages for better // compression performance? Should we use LZ4? class ProtoStreamWriter { public: ProtoStreamWriter(const string& filename); ~ProtoStreamWriter(); ProtoStreamWriter(const ProtoStreamWriter&) = delete; ProtoStreamWriter& operator=(const ProtoStreamWriter&) = delete; // Serializes, compressed and writes the 'proto' to the file. template void WriteProto(const MessageType& proto) { string uncompressed_data; proto.SerializeToString(&uncompressed_data); Write(uncompressed_data); //Write()函数执行压缩过程。 } // This should be called to check whether writing was successful. bool Close(); private: void Write(const string& uncompressed_data); std::ofstream out_; }; // A reader of the format produced by ProtoStreamWriter. class ProtoStreamReader { public: ProtoStreamReader(const string& filename); ~ProtoStreamReader(); ProtoStreamReader(const ProtoStreamReader&) = delete; ProtoStreamReader& operator=(const ProtoStreamReader&) = delete; template bool ReadProto(MessageType* proto) { string decompressed_data; return Read(&decompressed_data) && proto->ParseFromString(decompressed_data); } private: bool Read(string* decompressed_data); std::ifstream in_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_PROTO_STREAM_H_ ================================================ FILE: io/proto_stream_test.cc ================================================ #include "cartographer/io/proto_stream.h" #include #include #include #include #include "cartographer/common/port.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "gtest/gtest.h" namespace cartographer { namespace io { namespace { class ProtoStreamTest : public ::testing::Test { protected: void SetUp() override { const string tmpdir = P_tmpdir; test_directory_ = tmpdir + "/proto_stream_test_XXXXXX"; ASSERT_NE(mkdtemp(&test_directory_[0]), nullptr) << strerror(errno); } void TearDown() override { remove(test_directory_.c_str()); } string test_directory_; }; TEST_F(ProtoStreamTest, WriteAndReadBack) { const string test_file = test_directory_ + "/test_trajectory.pbstream"; { ProtoStreamWriter writer(test_file); for (int i = 0; i != 10; ++i) { mapping::proto::Trajectory trajectory; trajectory.add_node()->set_timestamp(i); writer.WriteProto(trajectory); } ASSERT_TRUE(writer.Close()); } { ProtoStreamReader reader(test_file); for (int i = 0; i != 10; ++i) { mapping::proto::Trajectory trajectory; ASSERT_TRUE(reader.ReadProto(&trajectory)); ASSERT_EQ(1, trajectory.node_size()); EXPECT_EQ(i, trajectory.node(0).timestamp()); } mapping::proto::Trajectory trajectory; EXPECT_FALSE(reader.ReadProto(&trajectory)); } remove(test_file.c_str()); } } // namespace } // namespace io } // namespace cartographer ================================================ FILE: io/xray_points_processor.cc ================================================ #include "cartographer/io/xray_points_processor.h" #include #include #include "Eigen/Core" #include "cairo/cairo.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/io/cairo_types.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace io { namespace { struct PixelData { size_t num_occupied_cells_in_column = 0; double mean_r = 0.; double mean_g = 0.; double mean_b = 0.; }; using PixelDataMatrix = Eigen::Matrix; double Mix(const double a, const double b, const double t) { return a * (1. - t) + t * b; } //cairo回调函数，将data写入FileWriter cairo_status_t CairoWriteCallback(void* const closure, const unsigned char* data, const unsigned int length) { if (static_cast(closure)->Write( reinterpret_cast(data), length)) { return CAIRO_STATUS_SUCCESS; } return CAIRO_STATUS_WRITE_ERROR; } /* 使用cairo库将mat以png文件写入filename */ // Write 'mat' as a pleasing-to-look-at PNG into 'filename' void WritePng(const PixelDataMatrix& mat, FileWriter* const file_writer) { const int stride = cairo_format_stride_for_width(CAIRO_FORMAT_ARGB32, mat.cols()); CHECK_EQ(stride % 4, 0); std::vector pixels(stride / 4 * mat.rows(), 0.); float max = std::numeric_limits::min(); for (int y = 0; y < mat.rows(); ++y) { for (int x = 0; x < mat.cols(); ++x) { const PixelData& cell = mat(y, x); if (cell.num_occupied_cells_in_column == 0.) { continue; } max = std::max(max, std::log(cell.num_occupied_cells_in_column)); } } for (int y = 0; y < mat.rows(); ++y) { for (int x = 0; x < mat.cols(); ++x) { const PixelData& cell = mat(y, x); if (cell.num_occupied_cells_in_column == 0.) { pixels[y * stride / 4 + x] = (255 << 24) | (255 << 16) | (255 << 8) | 255; continue; } // We use a logarithmic weighting for how saturated a pixel will be. The // basic idea here was that walls (full height) are fully saturated, but // details like chairs and tables are still well visible. const float saturation = std::log(cell.num_occupied_cells_in_column) / max; double mean_r_in_column = (cell.mean_r / 255.); double mean_g_in_column = (cell.mean_g / 255.); double mean_b_in_column = (cell.mean_b / 255.); double mix_r = Mix(1., mean_r_in_column, saturation); double mix_g = Mix(1., mean_g_in_column, saturation); double mix_b = Mix(1., mean_b_in_column, saturation); const int r = common::RoundToInt(mix_r * 255.); const int g = common::RoundToInt(mix_g * 255.); const int b = common::RoundToInt(mix_b * 255.); pixels[y * stride / 4 + x] = (255 << 24) | (r << 16) | (g << 8) | b; } } // TODO(hrapp): cairo_image_surface_create_for_data does not take ownership of // the data until the surface is finalized. Once it is finalized though, // cairo_surface_write_to_png fails, complaining that the surface is already // finalized. This makes it pretty hard to pass back ownership of the image to // the caller. cairo::UniqueSurfacePtr surface( cairo_image_surface_create_for_data( reinterpret_cast(pixels.data()), CAIRO_FORMAT_ARGB32, mat.cols(), mat.rows(), stride), cairo_surface_destroy); CHECK_EQ(cairo_surface_status(surface.get()), CAIRO_STATUS_SUCCESS); CHECK_EQ(cairo_surface_write_to_png_stream(surface.get(), &CairoWriteCallback, file_writer), CAIRO_STATUS_SUCCESS); CHECK(file_writer->Close()); } bool ContainedIn(const common::Time& time, const std::vector& timespans) { for (const mapping::Timespan& timespan : timespans) { if (timespan.start <= time && time <= timespan.end) { return true; } } return false; } } // namespace XRayPointsProcessor::XRayPointsProcessor( const double voxel_size, const transform::Rigid3f& transform, const std::vector& floors, const string& output_filename, FileWriterFactory file_writer_factory, PointsProcessor* const next) : next_(next), file_writer_factory_(file_writer_factory), floors_(floors), output_filename_(output_filename), transform_(transform) { for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) { aggregations_.emplace_back( Aggregation{mapping_3d::HybridGridBase(voxel_size), {}}); } } std::unique_ptr XRayPointsProcessor::FromDictionary( const mapping::proto::Trajectory& trajectory, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { std::vector floors; if (dictionary->HasKey("separate_floors") && dictionary->GetBool("separate_floors")) { floors = mapping::DetectFloors(trajectory); } return common::make_unique( dictionary->GetDouble("voxel_size"), transform::FromDictionary(dictionary->GetDictionary("transform").get()) .cast(), floors, dictionary->GetString("filename"), file_writer_factory, next); } void XRayPointsProcessor::WriteVoxels(const Aggregation& aggregation, FileWriter* const file_writer) { if (bounding_box_.isEmpty()) { LOG(WARNING) << "Not writing output: bounding box is empty."; return; } // Returns the (x, y) pixel of the given 'index'. const auto voxel_index_to_pixel = [this](const Eigen::Array3i& index) { // We flip the y axis, since matrices rows are counted from the top. return Eigen::Array2i(bounding_box_.max()[1] - index[1], bounding_box_.max()[2] - index[2]); }; // Hybrid grid uses X: forward, Y: left, Z: up. // For the screen we are using. X: right, Y: up const int xsize = bounding_box_.sizes()[1] + 1; const int ysize = bounding_box_.sizes()[2] + 1; PixelDataMatrix image = PixelDataMatrix(ysize, xsize); for (mapping_3d::HybridGridBase::Iterator it(aggregation.voxels); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); const Eigen::Array2i pixel = voxel_index_to_pixel(cell_index); PixelData& pixel_data = image(pixel.y(), pixel.x()); const auto& column_data = aggregation.column_data.at( std::make_pair(cell_index[1], cell_index[2])); pixel_data.mean_r = column_data.sum_r / column_data.count; pixel_data.mean_g = column_data.sum_g / column_data.count; pixel_data.mean_b = column_data.sum_b / column_data.count; ++pixel_data.num_occupied_cells_in_column; } WritePng(image, file_writer); } void XRayPointsProcessor::Insert(const PointsBatch& batch, const transform::Rigid3f& transform, Aggregation* const aggregation) { constexpr Color kDefaultColor = {{0, 0, 0}}; for (size_t i = 0; i < batch.points.size(); ++i) { const Eigen::Vector3f camera_point = transform * batch.points[i]; const Eigen::Array3i cell_index = aggregation->voxels.GetCellIndex(camera_point); *aggregation->voxels.mutable_value(cell_index) = true; bounding_box_.extend(cell_index.matrix()); ColumnData& column_data = aggregation->column_data[std::make_pair(cell_index[1], cell_index[2])]; const auto& color = batch.colors.empty() ? kDefaultColor : batch.colors.at(i); column_data.sum_r += color[0]; column_data.sum_g += color[1]; column_data.sum_b += color[2]; ++column_data.count; } } void XRayPointsProcessor::Process(std::unique_ptr batch) { if (floors_.empty()) { CHECK_EQ(aggregations_.size(), 1); Insert(*batch, transform_, &aggregations_[0]); } else { for (size_t i = 0; i < floors_.size(); ++i) { if (!ContainedIn(batch->time, floors_[i].timespans)) { continue; } Insert(*batch, transform_, &aggregations_[i]); } } next_->Process(std::move(batch)); } PointsProcessor::FlushResult XRayPointsProcessor::Flush() { if (floors_.empty()) { CHECK_EQ(aggregations_.size(), 1); WriteVoxels(aggregations_[0], file_writer_factory_(output_filename_ + ".png").get()); } else { for (size_t i = 0; i < floors_.size(); ++i) { WriteVoxels( aggregations_[i], file_writer_factory_(output_filename_ + std::to_string(i) + ".png") .get()); } } switch (next_->Flush()) { case FlushResult::kRestartStream: LOG(FATAL) << "X-Ray generation must be configured to occur after any " "stages that require multiple passes."; case FlushResult::kFinished: return FlushResult::kFinished; } LOG(FATAL); } } // namespace io } // namespace cartographer ================================================ FILE: io/xray_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" #include "cartographer/mapping/detect_floors.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/transform/rigid_transform.h" /*配置文件概览： VOXEL_SIZE = 5e-2 YZ_TRANSFORM = { translation = { 0., 0., 0. }, rotation = { 0. , 0., math.pi, }, } { action = "write_xray_image", voxel_size = VOXEL_SIZE, filename = "xray_yz_all_intensity", transform = YZ_TRANSFORM, }, */ namespace cartographer { namespace io { /* xray_points_processor.h是PointsProcessor的第三个子类(3). 功能:将x射线以体素为voxel_size写入image。 数据成员: 1), 2), */ // Creates X-ray cuts through the points with pixels being 'voxel_size' big. class XRayPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_xray_image"; XRayPointsProcessor(double voxel_size, const transform::Rigid3f& transform, const std::vector& floors, const string& output_filename, FileWriterFactory file_writer_factory, PointsProcessor* next); static std::unique_ptr FromDictionary( const mapping::proto::Trajectory& trajectory, FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XRayPointsProcessor() override {} //将点云写入.png文件，然后流水到下一处理器 void Process(std::unique_ptr batch) override; FlushResult Flush() override; Eigen::AlignedBox3i bounding_box() const { return bounding_box_; } private: struct ColumnData { double sum_r = 0.; double sum_g = 0.; double sum_b = 0.; uint32_t count = 0; }; struct Aggregation { mapping_3d::HybridGridBase voxels; std::map, ColumnData> column_data; }; void WriteVoxels(const Aggregation& aggregation, FileWriter* const file_writer); void Insert(const PointsBatch& batch, const transform::Rigid3f& transform, Aggregation* aggregation); PointsProcessor* const next_; FileWriterFactory file_writer_factory_; //负责文件写入的工厂 // If empty, we do not separate into floors. std::vector floors_; //楼层 const string output_filename_; //文件名称 const transform::Rigid3f transform_; //点云涉及的变换 // Only has one entry if we do not separate into floors. std::vector aggregations_; //聚集数据 // Bounding box containing all cells with data in all 'aggregations_'. Eigen::AlignedBox3i bounding_box_; //边界框 }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_XRAY_POINTS_PROCESSOR_H_ ================================================ FILE: io/xyz_writing_points_processor.cc ================================================ #include "cartographer/io/xyz_writing_points_processor.h" #include #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace io { namespace { /* 将x,y,x以字符串形式写入FileWriter */ void WriteXyzPoint(const Eigen::Vector3f& point, FileWriter* const file_writer) { std::ostringstream stream; stream << std::setprecision(6); stream << point.x() << " " << point.y() << " " << point.z() << "\n"; const string out = stream.str(); CHECK(file_writer->Write(out.data(), out.size())); } } // namespace /* 构造函数初始化 */ XyzWriterPointsProcessor::XyzWriterPointsProcessor( std::unique_ptr file_writer, PointsProcessor* const next) : next_(next), file_writer_(std::move(file_writer)) {} /* 根据filename返回指向文件名的file_writer_factory */ std::unique_ptr XyzWriterPointsProcessor::FromDictionary ( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* const dictionary, PointsProcessor* const next) { return common::make_unique( file_writer_factory(dictionary->GetString("filename")), next); } /*刷新缓冲*/ PointsProcessor::FlushResult XyzWriterPointsProcessor::Flush() { CHECK(file_writer_->Close()) << "Closing XYZ file failed."; switch (next_->Flush()) { case FlushResult::kFinished: return FlushResult::kFinished; case FlushResult::kRestartStream: LOG(FATAL) << "XYZ generation must be configured to occur after any " "stages that require multiple passes."; } LOG(FATAL); } /* 主逻辑,以{x,y,z}写入文件 */ void XyzWriterPointsProcessor::Process(std::unique_ptr batch) { for (const Eigen::Vector3f& point : batch->points) { WriteXyzPoint(point, file_writer_.get()); } next_->Process(std::move(batch)); } } // namespace io } // namespace cartographer ================================================ FILE: io/xyz_writing_points_processor.h ================================================ #ifndef CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ #define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/points_processor.h" namespace cartographer { namespace io { /* XyzWriterPointsProcessor将points以{x,y,z}形式写入 FileWriter */ // Writes ASCII xyz points. class XyzWriterPointsProcessor : public PointsProcessor { public: constexpr static const char* kConfigurationFileActionName = "write_xyz"; XyzWriterPointsProcessor(std::unique_ptr, PointsProcessor* next); static std::unique_ptr FromDictionary( FileWriterFactory file_writer_factory, common::LuaParameterDictionary* dictionary, PointsProcessor* next); ~XyzWriterPointsProcessor() override {} XyzWriterPointsProcessor(const XyzWriterPointsProcessor&) = delete; XyzWriterPointsProcessor& operator=(const XyzWriterPointsProcessor&) = delete; void Process(std::unique_ptr batch) override; FlushResult Flush() override; private: PointsProcessor* const next_; std::unique_ptr file_writer_; }; } // namespace io } // namespace cartographer #endif // CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ ================================================ FILE: kalman_filter/gaussian_distribution.h ================================================ #ifndef CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ #define CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ #include "Eigen/Cholesky" #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { namespace kalman_filter { /* 高斯分布类 构造函数是N*1的均值矩阵和N*N的协方差矩阵 */ template class GaussianDistribution { public: GaussianDistribution(const Eigen::Matrix& mean, const Eigen::Matrix& covariance) : mean_(mean), covariance_(covariance) {} const Eigen::Matrix& GetMean() const { return mean_; } const Eigen::Matrix& GetCovariance() const { return covariance_; } private: Eigen::Matrix mean_; //N*1，均值 Eigen::Matrix covariance_; //N*N。协方差 }; /* 重载+加号操作符, 高斯+高斯=对应均值+均值,对应协方差+协方差 返回值：新高斯对象 高斯分布性质: https://zh.wikipedia.org/wiki/%E6%AD%A3%E6%80%81%E5%88%86%E5%B8%83 */ template GaussianDistribution operator+(const GaussianDistribution& lhs, const GaussianDistribution& rhs) { return GaussianDistribution(lhs.GetMean() + rhs.GetMean(), lhs.GetCovariance() + rhs.GetCovariance()); } /* 重载乘法运算符 1,矩阵N*M 2,高斯分布M*M 返回值:高斯分布：N*N */ template GaussianDistribution operator*(const Eigen::Matrix& lhs, const GaussianDistribution& rhs) { return GaussianDistribution( lhs * rhs.GetMean(), // N*M || M*1 -> N*1 lhs * rhs.GetCovariance() * lhs.transpose()); // N*M ||M*M || M*N -> N*N } } // namespace kalman_filter } // namespace cartographer #endif // CARTOGRAPHER_KALMAN_FILTER_GAUSSIAN_DISTRIBUTION_H_ ================================================ FILE: kalman_filter/gaussian_distribution_test.cc ================================================ #include "cartographer/kalman_filter/gaussian_distribution.h" #include "gtest/gtest.h" namespace cartographer { namespace kalman_filter { namespace { TEST(GaussianDistributionTest, testConstructor) { Eigen::Matrix2d covariance;//2*2 covariance << 1., 2., 3., 4.; GaussianDistribution distribution(Eigen::Vector2d(0., 1.), //均值是0,1 covariance); EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);//0列的的均值是0 EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9); EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);//0行1列的方差是2 } } // namespace } // namespace kalman_filter } // namespace cartographer ================================================ FILE: kalman_filter/pose_tracker.cc ================================================ #include "cartographer/kalman_filter/pose_tracker.h" #include #include #include #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" #include "cartographer/kalman_filter/gaussian_distribution.h" #include "cartographer/kalman_filter/unscented_kalman_filter.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace kalman_filter { namespace { /*定义函数AddDelta,当前状态State加上delta. */ PoseTracker::State AddDelta(const PoseTracker::State& state, const PoseTracker::State& delta) { PoseTracker::State new_state = state + delta; const Eigen::Quaterniond orientation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(state[PoseTracker::kMapOrientationX], state[PoseTracker::kMapOrientationY], state[PoseTracker::kMapOrientationZ])); const Eigen::Vector3d rotation_vector(delta[PoseTracker::kMapOrientationX], delta[PoseTracker::kMapOrientationY], delta[PoseTracker::kMapOrientationZ]); CHECK_LT(rotation_vector.norm(), M_PI / 2.) << "Sigma point is far from the mean, recovered delta may be incorrect."; const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion(rotation_vector); const Eigen::Vector3d new_orientation = transform::RotationQuaternionToAngleAxisVector(orientation * rotation); new_state[PoseTracker::kMapOrientationX] = new_orientation.x(); new_state[PoseTracker::kMapOrientationY] = new_orientation.y(); new_state[PoseTracker::kMapOrientationZ] = new_orientation.z(); return new_state; } /*定义ComputeDelta()函数,计算State的差值delta */ PoseTracker::State ComputeDelta(const PoseTracker::State& origin, const PoseTracker::State& target) { PoseTracker::State delta = target - origin; const Eigen::Quaterniond origin_orientation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(origin[PoseTracker::kMapOrientationX], origin[PoseTracker::kMapOrientationY], origin[PoseTracker::kMapOrientationZ])); const Eigen::Quaterniond target_orientation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(target[PoseTracker::kMapOrientationX], target[PoseTracker::kMapOrientationY], target[PoseTracker::kMapOrientationZ])); const Eigen::Vector3d rotation = transform::RotationQuaternionToAngleAxisVector( origin_orientation.inverse() * target_orientation); delta[PoseTracker::kMapOrientationX] = rotation.x(); delta[PoseTracker::kMapOrientationY] = rotation.y(); delta[PoseTracker::kMapOrientationZ] = rotation.z(); return delta; } // Build a model matrix for the given time delta.返回delta时间后的状态 PoseTracker::State ModelFunction(const PoseTracker::State& state, const double delta_t) { CHECK_GT(delta_t, 0.); PoseTracker::State new_state; new_state[PoseTracker::kMapPositionX] = //x=x+delta * v state[PoseTracker::kMapPositionX] + delta_t * state[PoseTracker::kMapVelocityX]; new_state[PoseTracker::kMapPositionY] = //y=y+delta* v state[PoseTracker::kMapPositionY] + delta_t * state[PoseTracker::kMapVelocityY]; new_state[PoseTracker::kMapPositionZ] = //z=z+ delta* v state[PoseTracker::kMapPositionZ] + delta_t * state[PoseTracker::kMapVelocityZ]; new_state[PoseTracker::kMapOrientationX] =//不变 state[PoseTracker::kMapOrientationX]; new_state[PoseTracker::kMapOrientationY] = state[PoseTracker::kMapOrientationY]; new_state[PoseTracker::kMapOrientationZ] = state[PoseTracker::kMapOrientationZ]; new_state[PoseTracker::kMapVelocityX] = state[PoseTracker::kMapVelocityX];//不变 new_state[PoseTracker::kMapVelocityY] = state[PoseTracker::kMapVelocityY]; new_state[PoseTracker::kMapVelocityZ] = state[PoseTracker::kMapVelocityZ]; return new_state; } } // namespace /* 定义乘法操作,C=A*B */ PoseAndCovariance operator*(const transform::Rigid3d& transform, const PoseAndCovariance& pose_and_covariance) { GaussianDistribution distribution( // 6行1列 Eigen::Matrix::Zero(), pose_and_covariance.covariance); Eigen::Matrix linear_transform; //6行6列 linear_transform << transform.rotation().matrix(), Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), transform.rotation().matrix(); return {transform * pose_and_covariance.pose, (linear_transform * distribution).GetCovariance()}; } /* trajectory_builder_3d.lua: pose_tracker = { orientation_model_variance = 5e-3, position_model_variance = 0.00654766, velocity_model_variance = 0.53926, -- This disables gravity alignment in local SLAM. imu_gravity_time_constant = 1e9, imu_gravity_variance = 0, num_odometry_states = 1, }, */ proto::PoseTrackerOptions CreatePoseTrackerOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::PoseTrackerOptions options; options.set_position_model_variance( parameter_dictionary->GetDouble("position_model_variance")); options.set_orientation_model_variance( parameter_dictionary->GetDouble("orientation_model_variance")); options.set_velocity_model_variance( parameter_dictionary->GetDouble("velocity_model_variance")); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_imu_gravity_variance( parameter_dictionary->GetDouble("imu_gravity_variance")); options.set_num_odometry_states( parameter_dictionary->GetNonNegativeInt("num_odometry_states")); CHECK_GT(options.num_odometry_states(), 0); return options; } PoseTracker::Distribution PoseTracker::KalmanFilterInit() { State initial_state = State::Zero(); // We are certain about the complete state at the beginning. We define the // initial pose to be at the origin and axis aligned. Additionally, we claim // that we are not moving. StateCovariance initial_covariance = 1e-9 * StateCovariance::Identity(); return Distribution(initial_state, initial_covariance); } PoseTracker::PoseTracker(const proto::PoseTrackerOptions& options, const common::Time time) : options_(options), time_(time), kalman_filter_(KalmanFilterInit(), AddDelta, ComputeDelta), imu_tracker_(options.imu_gravity_time_constant(), time), odometry_state_tracker_(options.num_odometry_states()) {} PoseTracker::~PoseTracker() {} PoseTracker::Distribution PoseTracker::GetBelief(const common::Time time) { Predict(time); return kalman_filter_.GetBelief(); } void PoseTracker::GetPoseEstimateMeanAndCovariance(const common::Time time, transform::Rigid3d* pose, PoseCovariance* covariance) { const Distribution belief = GetBelief(time); *pose = RigidFromState(belief.GetMean()); static_assert(kMapPositionX == 0, "Cannot extract PoseCovariance."); static_assert(kMapPositionY == 1, "Cannot extract PoseCovariance."); static_assert(kMapPositionZ == 2, "Cannot extract PoseCovariance."); static_assert(kMapOrientationX == 3, "Cannot extract PoseCovariance."); static_assert(kMapOrientationY == 4, "Cannot extract PoseCovariance."); static_assert(kMapOrientationZ == 5, "Cannot extract PoseCovariance."); *covariance = belief.GetCovariance().block<6, 6>(0, 0); covariance->block<2, 2>(3, 3) += options_.imu_gravity_variance() * Eigen::Matrix2d::Identity(); } const PoseTracker::Distribution PoseTracker::BuildModelNoise( const double delta_t) const { // Position is constant, but orientation changes. StateCovariance model_noise = StateCovariance::Zero(); model_noise.diagonal() << // Position in map. options_.position_model_variance() * delta_t, options_.position_model_variance() * delta_t, options_.position_model_variance() * delta_t, // Orientation in map. options_.orientation_model_variance() * delta_t, options_.orientation_model_variance() * delta_t, options_.orientation_model_variance() * delta_t, // Linear velocities in map. options_.velocity_model_variance() * delta_t, options_.velocity_model_variance() * delta_t, options_.velocity_model_variance() * delta_t; return Distribution(State::Zero(), model_noise); } void PoseTracker::Predict(const common::Time time) { imu_tracker_.Advance(time); CHECK_LE(time_, time); const double delta_t = common::ToSeconds(time - time_); if (delta_t == 0.) { return; } kalman_filter_.Predict( [this, delta_t](const State& state) -> State { return ModelFunction(state, delta_t); }, BuildModelNoise(delta_t)); time_ = time; } void PoseTracker::AddImuLinearAccelerationObservation( const common::Time time, const Eigen::Vector3d& imu_linear_acceleration) { imu_tracker_.Advance(time); imu_tracker_.AddImuLinearAccelerationObservation(imu_linear_acceleration); Predict(time); } void PoseTracker::AddImuAngularVelocityObservation( const common::Time time, const Eigen::Vector3d& imu_angular_velocity) { imu_tracker_.Advance(time); imu_tracker_.AddImuAngularVelocityObservation(imu_angular_velocity); Predict(time); } void PoseTracker::AddPoseObservation(const common::Time time, const transform::Rigid3d& pose, const PoseCovariance& covariance) { Predict(time); // Noise covariance is taken directly from the input values. const GaussianDistribution delta( Eigen::Matrix::Zero(), covariance); kalman_filter_.Observe<6>( [this, &pose](const State& state) -> Eigen::Matrix { const transform::Rigid3d state_pose = RigidFromState(state); const Eigen::Vector3d delta_orientation = transform::RotationQuaternionToAngleAxisVector( pose.rotation().inverse() * state_pose.rotation()); const Eigen::Vector3d delta_translation = state_pose.translation() - pose.translation(); Eigen::Matrix return_value; return_value << delta_translation, delta_orientation; return return_value; }, delta); } // Updates from the odometer are in the odometer's map-like frame, called the // 'odometry' frame. The odometer_pose converts data from the map frame // into the odometry frame. void PoseTracker::AddOdometerPoseObservation( const common::Time time, const transform::Rigid3d& odometer_pose, const PoseCovariance& covariance) { if (!odometry_state_tracker_.empty()) { const auto& previous_odometry_state = odometry_state_tracker_.newest(); const transform::Rigid3d delta = previous_odometry_state.odometer_pose.inverse() * odometer_pose; const transform::Rigid3d new_pose = previous_odometry_state.state_pose * delta; AddPoseObservation(time, new_pose, covariance); } const Distribution belief = GetBelief(time); odometry_state_tracker_.AddOdometryState( {time, odometer_pose, RigidFromState(belief.GetMean())}); } const mapping::OdometryStateTracker::OdometryStates& PoseTracker::odometry_states() const { return odometry_state_tracker_.odometry_states(); } transform::Rigid3d PoseTracker::RigidFromState( const PoseTracker::State& state) { return transform::Rigid3d( Eigen::Vector3d(state[PoseTracker::kMapPositionX], state[PoseTracker::kMapPositionY], state[PoseTracker::kMapPositionZ]), transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(state[PoseTracker::kMapOrientationX], state[PoseTracker::kMapOrientationY], state[PoseTracker::kMapOrientationZ])) * imu_tracker_.orientation()); } PoseCovariance BuildPoseCovariance(const double translational_variance, const double rotational_variance) { const Eigen::Matrix3d translational = Eigen::Matrix3d::Identity() * translational_variance; const Eigen::Matrix3d rotational = Eigen::Matrix3d::Identity() * rotational_variance; // clang-format off PoseCovariance covariance; covariance << translational, Eigen::Matrix3d::Zero(), Eigen::Matrix3d::Zero(), rotational; // clang-format on return covariance; } } // namespace kalman_filter } // namespace cartographer ================================================ FILE: kalman_filter/pose_tracker.h ================================================ #ifndef CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_ #define CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_ #include #include #include "Eigen/Cholesky" #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/kalman_filter/gaussian_distribution.h" #include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h" #include "cartographer/kalman_filter/unscented_kalman_filter.h" #include "cartographer/mapping/imu_tracker.h" #include "cartographer/mapping/odometry_state_tracker.h" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace kalman_filter { typedef Eigen::Matrix3d Pose2DCovariance; //3*3矩阵 typedef Eigen::Matrix PoseCovariance;// 6*6 矩阵 struct PoseAndCovariance { transform::Rigid3d pose; PoseCovariance covariance; //6*6 }; PoseAndCovariance operator*(const transform::Rigid3d& transform, const PoseAndCovariance& pose_and_covariance); PoseCovariance BuildPoseCovariance(double translational_variance, double rotational_variance); proto::PoseTrackerOptions CreatePoseTrackerOptions( common::LuaParameterDictionary* parameter_dictionary); /* 基于卡尔曼滤波的3维位置和方向估计 */ // A Kalman filter for a 3D state of position and orientation. // Includes functions to update from IMU and range data matches. class PoseTracker { public: enum { kMapPositionX = 0,//位置信息{X,Y,Z} kMapPositionY, kMapPositionZ, kMapOrientationX,//方向信息,3 kMapOrientationY, kMapOrientationZ, kMapVelocityX, //速度信息,6 kMapVelocityY, kMapVelocityZ, kDimension //9, We terminate loops with this. 只追踪9个维度 }; using KalmanFilter = UnscentedKalmanFilter;//9维的卡尔曼滤波 using State = KalmanFilter::StateType; //N*1矩阵 using StateCovariance = Eigen::Matrix;//9*9 using Distribution = GaussianDistribution; //参数类型的double,9*1的矩阵 // Create a new Kalman filter starting at the origin with a standard normal // distribution at 'time'. //在给定的time时刻初始化卡尔曼滤波参数 PoseTracker(const proto::PoseTrackerOptions& options, common::Time time); virtual ~PoseTracker(); // Sets 'pose' and 'covariance' to the mean and covariance of the belief at // 'time' which must be >= to the current time. Must not be nullptr. //通过指针获取pose的旋转参数和covariance方差 void GetPoseEstimateMeanAndCovariance(common::Time time, transform::Rigid3d* pose, PoseCovariance* covariance); // Updates from an IMU reading (in the IMU frame). //根据imu观测值更新 void AddImuLinearAccelerationObservation( common::Time time, const Eigen::Vector3d& imu_linear_acceleration); void AddImuAngularVelocityObservation( common::Time time, const Eigen::Vector3d& imu_angular_velocity); // Updates from a pose estimate in the map frame. //根据map-frame的位姿估计更新 void AddPoseObservation(common::Time time, const transform::Rigid3d& pose, const PoseCovariance& covariance); // Updates from a pose estimate in the odometer's map-like frame. //根据里程计的map-like frame位姿估计更新 void AddOdometerPoseObservation(common::Time time, const transform::Rigid3d& pose, const PoseCovariance& covariance); common::Time time() const { return time_; } //最新有效时间 // Returns the belief at the 'time' which must be >= to the current time. Distribution GetBelief(common::Time time); //未来某一时刻的状态估计值 const mapping::OdometryStateTracker::OdometryStates& odometry_states() const; //imu的重力方向 Eigen::Quaterniond gravity_orientation() const { return imu_tracker_.orientation(); } private: //返回初始状态的状态变量的高斯分布 // Returns the distribution required to initialize the KalmanFilter. static Distribution KalmanFilterInit(); //建立零均值噪声模型 // Build a model noise distribution (zero mean) for the given time delta. const Distribution BuildModelNoise(double delta_t) const; // Predict the state forward in time. This is a no-op if 'time' has not moved // forward. //根据当前状态预测time时刻的状态 void Predict(common::Time time); // Computes a pose combining the given 'state' with the 'imu_tracker_' // orientation. //结合 imu_tracker_和state,计算位姿pose的旋转变换。 transform::Rigid3d RigidFromState(const PoseTracker::State& state); const proto::PoseTrackerOptions options_; //用于位姿估计的传感器特性 common::Time time_; //测量时间 KalmanFilter kalman_filter_; //卡尔曼滤波 mapping::ImuTracker imu_tracker_; //imu跟踪 mapping::OdometryStateTracker odometry_state_tracker_;//里程计跟踪 }; } // namespace kalman_filter } // namespace cartographer #endif // CARTOGRAPHER_KALMAN_FILTER_POSE_TRACKER_H_ ================================================ FILE: kalman_filter/pose_tracker_test.cc ================================================ #include "cartographer/kalman_filter/pose_tracker.h" #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { namespace kalman_filter { namespace { constexpr double kOdometerVariance = 1e-12; using transform::IsNearly; using transform::Rigid3d; using ::testing::Not; class PoseTrackerTest : public ::testing::Test { protected: PoseTrackerTest() { auto parameter_dictionary = common::MakeDictionary(R"text( return { orientation_model_variance = 1e-8, position_model_variance = 1e-8, velocity_model_variance = 1e-8, imu_gravity_time_constant = 100., imu_gravity_variance = 1e-9, num_odometry_states = 1, } )text"); const proto::PoseTrackerOptions options = CreatePoseTrackerOptions(parameter_dictionary.get()); pose_tracker_ = common::make_unique(options, common::FromUniversal(1000)); //创建卡尔曼滤波跟踪器 } std::unique_ptr pose_tracker_; }; TEST_F(PoseTrackerTest, SaveAndRestore) { std::vector poses(3); std::vector covariances(3); //0号，获取t=1500处的预测值 pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(1500), &poses[0], &covariances[0]); //0号，添加t=2000的测量值 pose_tracker_->AddImuLinearAccelerationObservation( common::FromUniversal(2000), Eigen::Vector3d(1, 1, 9)); PoseTracker copy_of_pose_tracker = *pose_tracker_;//默认copy构造函数，1号 const Eigen::Vector3d observation(2, 0, 8); //0号，添加t=3000的测量值 pose_tracker_->AddImuLinearAccelerationObservation( common::FromUniversal(3000), observation); //0号，获取t=3500的预测值 pose_tracker_->GetPoseEstimateMeanAndCovariance(common::FromUniversal(3500), &poses[1], &covariances[1]); //1号，添加t=3000的测量值 copy_of_pose_tracker.AddImuLinearAccelerationObservation( common::FromUniversal(3000), observation); //1号，获取t=3500的测量值 copy_of_pose_tracker.GetPoseEstimateMeanAndCovariance( common::FromUniversal(3500), &poses[2], &covariances[2]); EXPECT_THAT(poses[0], Not(IsNearly(poses[1], 1e-6))); EXPECT_FALSE((covariances[0].array() == covariances[1].array()).all()); //相同的测量，导致相同的结果 EXPECT_THAT(poses[1], IsNearly(poses[2], 1e-6)); EXPECT_TRUE((covariances[1].array() == covariances[2].array()).all()); } TEST_F(PoseTrackerTest, AddImuLinearAccelerationObservation) { auto time = common::FromUniversal(1000); for (int i = 0; i < 300; ++i) { time += std::chrono::seconds(5); //循环300次，添加测量值 pose_tracker_->AddImuLinearAccelerationObservation( time, Eigen::Vector3d(0., 0., 10.)); } { Rigid3d pose; PoseCovariance covariance; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation()); const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity(); //expected.coeffs():0,0,0,1 //预测值和真实值相等 EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n" << actual.coeffs(); } for (int i = 0; i < 300; ++i) { time += std::chrono::seconds(5); pose_tracker_->AddImuLinearAccelerationObservation( time, Eigen::Vector3d(0., 10., 0.)); } time += std::chrono::milliseconds(5); Rigid3d pose; PoseCovariance covariance; //获取预测值 pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation()); const Eigen::Quaterniond expected = Eigen::Quaterniond( Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX())); //预测值与真实值比较 EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n" << actual.coeffs(); } TEST_F(PoseTrackerTest, AddImuAngularVelocityObservation) { auto time = common::FromUniversal(1000); for (int i = 0; i < 300; ++i) { time += std::chrono::milliseconds(5); pose_tracker_->AddImuAngularVelocityObservation(time, Eigen::Vector3d::Zero()); } { Rigid3d pose; PoseCovariance covariance; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation()); const Eigen::Quaterniond expected = Eigen::Quaterniond::Identity(); EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n" << actual.coeffs(); } const double target_radians = M_PI / 2.; const double num_observations = 300.; const double angular_velocity = target_radians / (num_observations * 5e-3); for (int i = 0; i < num_observations; ++i) { time += std::chrono::milliseconds(5); pose_tracker_->AddImuAngularVelocityObservation( time, Eigen::Vector3d(angular_velocity, 0., 0.)); } time += std::chrono::milliseconds(5); Rigid3d pose; PoseCovariance covariance; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose, &covariance); const Eigen::Quaterniond actual = Eigen::Quaterniond(pose.rotation()); const Eigen::Quaterniond expected = Eigen::Quaterniond( Eigen::AngleAxisd(M_PI / 2., Eigen::Vector3d::UnitX())); EXPECT_TRUE(actual.isApprox(expected, 1e-3)) << expected.coeffs() << " vs\n" << actual.coeffs(); } TEST_F(PoseTrackerTest, AddPoseObservation) { auto time = common::FromUniversal(1000); for (int i = 0; i < 300; ++i) { time += std::chrono::milliseconds(5); pose_tracker_->AddPoseObservation( time, Rigid3d::Identity(), Eigen::Matrix::Identity() * 1e-6); } { Rigid3d actual; PoseCovariance covariance; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance); EXPECT_THAT(actual, IsNearly(Rigid3d::Identity(), 1e-3)); } const Rigid3d expected = Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) * Rigid3d::Rotation(Eigen::AngleAxisd( M_PI / 2., Eigen::Vector3d(0., 0., 3.).normalized())); for (int i = 0; i < 300; ++i) { time += std::chrono::milliseconds(15); pose_tracker_->AddPoseObservation( time, expected, Eigen::Matrix::Identity() * 1e-9); } time += std::chrono::milliseconds(15); Rigid3d actual; PoseCovariance covariance; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &covariance); EXPECT_THAT(actual, IsNearly(expected, 1e-3)); } TEST_F(PoseTrackerTest, AddOdometerPoseObservation) { common::Time time = common::FromUniversal(0); std::vector odometer_track; odometer_track.push_back(Rigid3d::Identity()); odometer_track.push_back( Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()))); odometer_track.push_back( Rigid3d::Translation(Eigen::Vector3d(0.2, 0., 0.)) * Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()))); odometer_track.push_back( Rigid3d::Translation(Eigen::Vector3d(0.3, 0.1, 0.)) * Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ()))); odometer_track.push_back( Rigid3d::Translation(Eigen::Vector3d(0.2, 0.2, 0.1)) * Rigid3d::Rotation(Eigen::AngleAxisd(0.3, Eigen::Vector3d::UnitZ()))); odometer_track.push_back( Rigid3d::Translation(Eigen::Vector3d(0.1, 0.2, 0.2)) * Rigid3d::Rotation(Eigen::AngleAxisd(0.6, Eigen::Vector3d::UnitZ()))); odometer_track.push_back(Rigid3d::Translation(Eigen::Vector3d(0., 0.1, 0.2))); Rigid3d actual; PoseCovariance unused_covariance; for (const Rigid3d& pose : odometer_track) { time += std::chrono::seconds(1); pose_tracker_->AddOdometerPoseObservation( time, pose, kOdometerVariance * PoseCovariance::Identity()); pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &actual, &unused_covariance); EXPECT_THAT(actual, IsNearly(pose, 1e-2)); } // Sanity check that the test has signal: EXPECT_THAT(actual, Not(IsNearly(odometer_track[0], 1e-2))); } } // namespace } // namespace kalman_filter } // namespace cartographer ================================================ FILE: kalman_filter/proto/pose_tracker_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.kalman_filter.proto; message PoseTrackerOptions { // Model variances depend linearly on time. optional double position_model_variance = 1; //位置方差 optional double orientation_model_variance = 2; //方向方差 optional double velocity_model_variance = 3; //速度 // Time constant for the orientation moving average based on observed gravity // via linear acceleration. optional double imu_gravity_time_constant = 4; //时间 optional double imu_gravity_variance = 5; //重力加速度方差 g // Maximum number of previous odometry states to keep. optional int32 num_odometry_states = 6; } ================================================ FILE: kalman_filter/unscented_kalman_filter.h ================================================ #ifndef CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_ #define CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_ #include #include #include #include #include "Eigen/Cholesky" #include "Eigen/Core" #include "Eigen/Eigenvalues" #include "cartographer/kalman_filter/gaussian_distribution.h" #include "glog/logging.h" /* 无损卡尔曼滤波,去芳香卡尔曼滤波https://en.wikipedia.org/wiki/Kalman_filter */ namespace cartographer { namespace kalman_filter { //平方,常量表达式 template constexpr FloatType sqr(FloatType a) { return a * a; } // N*1 || 1*N -> 外积,N*N template Eigen::Matrix OuterProduct( const Eigen::Matrix& v) { return v * v.transpose(); } // Checks if 'A' is a symmetric matrix.检查A是否是对称矩阵,A减去A的转置~=0 template void CheckSymmetric(const Eigen::Matrix& A) { // This should be pretty much Eigen::Matrix<>::Zero() if the matrix is // symmetric. //The NaN values are used to identify undefined or non-representable values for floating-point elements, //such as the square root of negative numbers or the result of 0/0. const FloatType norm = (A - A.transpose()).norm(); CHECK(!std::isnan(norm) && std::abs(norm) < 1e-5) << "Symmetry check failed with norm: '" << norm << "' from matrix:\n" << A; } /* https://zh.wikipedia.org/wiki/%E6%AD%A3%E5%AE%9A%E7%9F%A9%E9%98%B5 几个术语: Av=λv. λ 为一标量，称为 v 对应的特征值。也称 v 为特征值 λ 对应的特征向量。 eigendecomposition,特征分解,谱分解,是将矩阵分解为由其特征值和特征向量表示的矩阵之积的方法。 需要注意只有对可对角化矩阵才可以施以特征分解。 eigenvalues 特征值, eigenvectors 特征向量 返回对称半正定矩阵的平方根B,M=B*B */ // Returns the matrix square root of a symmetric positive semidefinite matrix. template Eigen::Matrix MatrixSqrt( const Eigen::Matrix& A) { CheckSymmetric(A);//必须是对称矩阵 Eigen::SelfAdjointEigenSolver> adjoint_eigen_solver((A + A.transpose()) / 2.); //A==A的转置，构造特征分解对象。 const auto& eigenvalues = adjoint_eigen_solver.eigenvalues();//特征值 CHECK_GT(eigenvalues.minCoeff(), -1e-5) << "MatrixSqrt failed with negative eigenvalues: " << eigenvalues.transpose(); //Cholesky分解：把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。 return adjoint_eigen_solver.eigenvectors() * adjoint_eigen_solver.eigenvalues() .cwiseMax(Eigen::Matrix::Zero()) .cwiseSqrt() .asDiagonal() * adjoint_eigen_solver.eigenvectors().transpose(); } /* 无损卡尔曼滤波 对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。 卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。 这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测）， 也可以是对过去位置的估计（插值或平滑）。 卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值， 因此不需要记录观测或者估计的历史信息。 卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。 在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。 UnscentedKalmanFilter类是根据《Probabilistic Robotics》实现的卡尔曼滤波类， 并且扩展到了处理非线性噪声和传感器。 数据成员: 1),add_delta_ 2),compute_delta_ 成员函数： 1),Predict():预测 */ /* Implementation of a Kalman filter. We follow the nomenclature (过程)from Thrun, S. et al., Probabilistic Robotics, 2006. Extended to handle non-additive noise/sensors inspired by Kraft, E., A Quaternion-based Unscented Kalman Filter for Orientation Tracking. */ template class UnscentedKalmanFilter { public: using StateType = Eigen::Matrix; //状态矩阵N*1 using StateCovarianceType = Eigen::Matrix; //协方差矩阵N*N /* 构造函数, 参数1,N*1矩阵, 参数2,stl函数对象 add_delta(默认), 参数3,stl函数对象 compute_delta(默认), */ explicit UnscentedKalmanFilter( const GaussianDistribution& initial_belief, //参数1 std::function //参数2 add_delta = [](const StateType& state, const StateType& delta) { return state + delta; }, std::function //参数3 compute_delta = [](const StateType& origin, const StateType& target) { return target - origin; }) : belief_(initial_belief), add_delta_(add_delta), compute_delta_(compute_delta) {} /* 预测step。使用卡尔曼滤波对当前状态进行预测。 参数1是std函数对象g, 作用是状态转移方程 参数2是高斯分布的“ε”,控制和模型噪声的线性组合。 */ // Does the control/prediction step for the filter. The control must be // implicitly added by the function g which also does the state transition. // 'epsilon' is the additive combination of control and model noise. void Predict(std::function g, const GaussianDistribution& epsilon) { CheckSymmetric(epsilon.GetCovariance()); // Get the state mean and matrix root of its covariance. const StateType& mu = belief_.GetMean(); const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance());//N*N std::vector Y;//需要计算的状态矩阵，N*1矩阵。 Y.reserve(2 * N + 1);//公式：p65 Y.emplace_back(g(mu));//状态转移方程,公式p65:3.68,p70:3 /* 按照公式p65,3.66计算： 1),mu是u, 2),add_delta_()得到Xi, 3),g(Xi)得到Yi, 4),ComputeMean()得到 u', 5),更新Σ得到new_sigma 6),返回新的高斯分布。均值是new_mu,方差是new_sigma。二者再加上epsilon。 */ const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda); for (int i = 0; i < N; ++i) { // Order does not matter here as all have the same weights in the // summation later on anyways. Y.emplace_back(g(add_delta_(mu, kSqrtNPlusLambda * sqrt_sigma.col(i)))); Y.emplace_back(g(add_delta_(mu, -kSqrtNPlusLambda * sqrt_sigma.col(i)))); } const StateType new_mu = ComputeMean(Y); StateCovarianceType new_sigma = kCovWeight0 * OuterProduct(compute_delta_(new_mu, Y[0])); for (int i = 0; i < N; ++i) { new_sigma += kCovWeightI * OuterProduct( compute_delta_(new_mu, Y[2 * i + 1])); new_sigma += kCovWeightI * OuterProduct( compute_delta_(new_mu, Y[2 * i + 2])); } CheckSymmetric(new_sigma); belief_ = GaussianDistribution(new_mu, new_sigma) + epsilon;//更新状态 } /* 观察step. 参数1:stl函数对象h 参数2:高斯分布的测量噪声delta,均值为0 按照《Probabilistic Robotics》p70,的公式(7)-(14)计算。 template :k是欲观测的变量数。 1),看书 2),... 3),... 4),... 5),... 6),... */ // The observation step of the Kalman filter. 'h' transfers the state // into an observation that should be zero, i.e., the sensor readings should // be included in this function already. 'delta' is the measurement noise and // must have zero mean. template void Observe( std::function(const StateType&)> h, const GaussianDistribution& delta) { CheckSymmetric(delta.GetCovariance()); // We expect zero mean delta. CHECK_NEAR(delta.GetMean().norm(), 0., 1e-9); // Get the state mean and matrix root of its covariance. const StateType& mu = belief_.GetMean(); const StateCovarianceType sqrt_sigma = MatrixSqrt(belief_.GetCovariance()); // As in Kraft's paper, we compute W containing the zero-mean sigma points, // since this is all we need. std::vector W; W.reserve(2 * N + 1); W.emplace_back(StateType::Zero()); std::vector> Z; Z.reserve(2 * N + 1); Z.emplace_back(h(mu)); Eigen::Matrix z_hat = kMeanWeight0 * Z[0]; const FloatType kSqrtNPlusLambda = std::sqrt(N + kLambda); for (int i = 0; i < N; ++i) { // Order does not matter here as all have the same weights in the // summation later on anyways. W.emplace_back(kSqrtNPlusLambda * sqrt_sigma.col(i)); Z.emplace_back(h(add_delta_(mu, W.back()))); W.emplace_back(-kSqrtNPlusLambda * sqrt_sigma.col(i)); Z.emplace_back(h(add_delta_(mu, W.back()))); z_hat += kMeanWeightI * Z[2 * i + 1]; z_hat += kMeanWeightI * Z[2 * i + 2]; } Eigen::Matrix S = kCovWeight0 * OuterProduct(Z[0] - z_hat); for (int i = 0; i < N; ++i) { S += kCovWeightI * OuterProduct(Z[2 * i + 1] - z_hat); S += kCovWeightI * OuterProduct(Z[2 * i + 2] - z_hat); } CheckSymmetric(S); S += delta.GetCovariance(); Eigen::Matrix sigma_bar_xz = kCovWeight0 * W[0] * (Z[0] - z_hat).transpose(); for (int i = 0; i < N; ++i) { sigma_bar_xz += kCovWeightI * W[2 * i + 1] * (Z[2 * i + 1] - z_hat).transpose(); sigma_bar_xz += kCovWeightI * W[2 * i + 2] * (Z[2 * i + 2] - z_hat).transpose(); } const Eigen::Matrix kalman_gain = sigma_bar_xz * S.inverse(); const StateCovarianceType new_sigma = belief_.GetCovariance() - kalman_gain * S * kalman_gain.transpose(); CheckSymmetric(new_sigma); belief_ = GaussianDistribution( add_delta_(mu, kalman_gain * -z_hat), new_sigma); } const GaussianDistribution& GetBelief() const { return belief_; } private: //计算带权重的偏差 StateType ComputeWeightedError(const StateType& mean_estimate, const std::vector& states) { StateType weighted_error = kMeanWeight0 * compute_delta_(mean_estimate, states[0]); for (int i = 1; i != 2 * N + 1; ++i) { weighted_error += kMeanWeightI * compute_delta_(mean_estimate, states[i]); } return weighted_error; } // Algorithm for computing the mean of non-additive states taken from Kraft's // Section 3.4, adapted to our implementation. //计算均值 StateType ComputeMean(const std::vector& states) { CHECK_EQ(states.size(), 2 * N + 1); StateType current_estimate = states[0]; StateType weighted_error = ComputeWeightedError(current_estimate, states); int iterations = 0; while (weighted_error.norm() > 1e-9) { double step_size = 1.; while (true) { const StateType next_estimate = add_delta_(current_estimate, step_size * weighted_error); const StateType next_error = ComputeWeightedError(next_estimate, states); if (next_error.norm() < weighted_error.norm()) { current_estimate = next_estimate; weighted_error = next_error; break; } step_size *= 0.5; CHECK_GT(step_size, 1e-3) << "Step size too small, line search failed."; } ++iterations; CHECK_LT(iterations, 20) << "Too many iterations."; } return current_estimate; } // According to Wikipedia these are the normal values. Thrun does not // mention those. constexpr static FloatType kAlpha = 1e-3; constexpr static FloatType kKappa = 0.; constexpr static FloatType kBeta = 2.; constexpr static FloatType kLambda = sqr(kAlpha) * (N + kKappa) - N; constexpr static FloatType kMeanWeight0 = kLambda / (N + kLambda); constexpr static FloatType kCovWeight0 = kLambda / (N + kLambda) + (1. - sqr(kAlpha) + kBeta); constexpr static FloatType kMeanWeightI = 1. / (2. * (N + kLambda)); constexpr static FloatType kCovWeightI = kMeanWeightI; //1),N*1矩阵,对N个变量的估计 GaussianDistribution belief_; //2),add_delta_，加法操作 const std::function add_delta_; //3),compute_delta_，计算偏差操作 const std::function compute_delta_; }; //外部声明。 template constexpr FloatType UnscentedKalmanFilter::kAlpha; template constexpr FloatType UnscentedKalmanFilter::kKappa; template constexpr FloatType UnscentedKalmanFilter::kBeta; template constexpr FloatType UnscentedKalmanFilter::kLambda; template constexpr FloatType UnscentedKalmanFilter::kMeanWeight0; template constexpr FloatType UnscentedKalmanFilter::kCovWeight0; template constexpr FloatType UnscentedKalmanFilter::kMeanWeightI; template constexpr FloatType UnscentedKalmanFilter::kCovWeightI; } // namespace kalman_filter } // namespace cartographer #endif // CARTOGRAPHER_KALMAN_FILTER_UNSCENTED_KALMAN_FILTER_H_ ================================================ FILE: kalman_filter/unscented_kalman_filter_test.cc ================================================ #include "cartographer/kalman_filter/unscented_kalman_filter.h" #include "cartographer/kalman_filter/gaussian_distribution.h" #include "gtest/gtest.h" namespace cartographer { namespace kalman_filter { namespace { //返回1*1的矩阵[val] Eigen::Matrix Scalar(double value) { return value * Eigen::Matrix::Identity(); } //motion运动函数/状态转移函数,g，可以理解为自身的状态变化 // A simple model that copies the first to the second state variable. Eigen::Matrix g(const Eigen::Matrix& state) { //值拷贝,返回矩阵[state[0],state[0]],即只修改第二参数 Eigen::Matrix new_state; new_state << state[0], state[0]; return new_state; } //观察/测量矩阵,h: state[0]-5.,可以理解为传感器到landmark的测量距离。 // A simple observation of the first state variable. Eigen::Matrix h(const Eigen::Matrix& state) {// return Scalar(state[0]) - Scalar(5.); } TEST(KalmanFilterTest, testConstructor) { /* 构造一个卡尔曼滤波,参数是: 1.长度为2的高斯分布,均值是0,42,协方差是10*[1,0;0,1] 2.默认 3.默认 */ UnscentedKalmanFilter filter(GaussianDistribution( Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); EXPECT_NEAR(42., filter.GetBelief().GetMean()[1], 1e-9); //均值第1列是42,index 从0开始。 } TEST(KalmanFilterTest, testPredict) { //创建一个含2个变量的卡尔曼滤波。均值是42,0,协方差是10*[1,0;0,1] UnscentedKalmanFilter filter(GaussianDistribution( Eigen::Vector2d(42., 0.), 10. * Eigen::Matrix2d::Identity())); /* 执行预测step,在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。 测量噪声是0均值单位阵。 预测值应该与[42,42]相等：g没有修改第一参数，只修改第二参数使其等于第一参数。 */ filter.Predict( g, GaussianDistribution(Eigen::Vector2d(0., 0.), Eigen::Matrix2d::Identity() * 1e-9)); EXPECT_NEAR(filter.GetBelief().GetMean()[0], 42., 1e-2); EXPECT_NEAR(filter.GetBelief().GetMean()[1], 42., 1e-2); } TEST(KalmanFilterTest, testObserve) { //创建一个含2个变量的卡尔曼滤波。均值是0,42,协方差是10*[1,0;0,1] UnscentedKalmanFilter filter(GaussianDistribution( Eigen::Vector2d(0., 42.), 10. * Eigen::Matrix2d::Identity())); /* 预测+观察,重复500次,最后均值是[5,5]： 观测矩阵h表示landmark到state[0]的距离永远是5，故稳定后state[0],state[1]应都为5 卡尔曼滤波：总是先预测，再根据传感器进行数据融合(校正)。 */ for (int i = 0; i < 500; ++i) { //预测。motion函数是g，control噪声的均值和方差是0 filter.Predict( g, GaussianDistribution(Eigen::Vector2d(0., 0.), Eigen::Matrix2d::Identity() * 1e-9)); //测量/观察。传感器噪声的均值和方差是0 filter.Observe<1>( h, GaussianDistribution(Scalar(0.), Scalar(1e-2))); } EXPECT_NEAR(filter.GetBelief().GetMean()[0], 5, 1e-2); EXPECT_NEAR(filter.GetBelief().GetMean()[1], 5, 1e-2); } } // namespace } // namespace kalman_filter } // namespace cartographer /* 运行unscented_kalman_filter_le_test.cc: before:0 42 #滤波前 Predict:0 0 # 执行预测 Observe:4.995 4.995 # 执行校正 -- before:4.995 4.995 Predict4.995 4.995 Observe: 4.9975 4.9975 可见经过2次迭代就已经较为准确了。 */ ================================================ FILE: mapping/collated_trajectory_builder.cc ================================================ #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/common/time.h" #include "glog/logging.h" namespace cartographer { namespace mapping { namespace { constexpr double kSensorDataRatesLoggingPeriodSeconds = 15.; } // namespace CollatedTrajectoryBuilder::CollatedTrajectoryBuilder( sensor::Collator* const sensor_collator, const int trajectory_id, const std::unordered_set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder) : sensor_collator_(sensor_collator), trajectory_id_(trajectory_id), wrapped_trajectory_builder_(std::move(wrapped_trajectory_builder)), last_logging_time_(std::chrono::steady_clock::now()) { sensor_collator_->AddTrajectory( trajectory_id, expected_sensor_ids, [this](const string& sensor_id, std::unique_ptr data) { HandleCollatedSensorData(sensor_id, std::move(data)); }); } CollatedTrajectoryBuilder::~CollatedTrajectoryBuilder() {} const Submaps* CollatedTrajectoryBuilder::submaps() const { return wrapped_trajectory_builder_->submaps(); } const TrajectoryBuilder::PoseEstimate& CollatedTrajectoryBuilder::pose_estimate() const { return wrapped_trajectory_builder_->pose_estimate(); } void CollatedTrajectoryBuilder::AddSensorData( const string& sensor_id, std::unique_ptr data) { sensor_collator_->AddSensorData(trajectory_id_, sensor_id, std::move(data)); } void CollatedTrajectoryBuilder::HandleCollatedSensorData( const string& sensor_id, std::unique_ptr data) { auto it = rate_timers_.find(sensor_id); if (it == rate_timers_.end()) { it = rate_timers_ .emplace( std::piecewise_construct, std::forward_as_tuple(sensor_id), std::forward_as_tuple( common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds))) .first; } it->second.Pulse(data->time); if (std::chrono::steady_clock::now() - last_logging_time_ > common::FromSeconds(kSensorDataRatesLoggingPeriodSeconds)) { for (const auto& pair : rate_timers_) { LOG(INFO) << pair.first << " rate: " << pair.second.DebugString(); } last_logging_time_ = std::chrono::steady_clock::now(); } switch (data->type) { case sensor::Data::Type::kImu: wrapped_trajectory_builder_->AddImuData(data->time, data->imu.linear_acceleration, data->imu.angular_velocity); return; case sensor::Data::Type::kRangefinder: wrapped_trajectory_builder_->AddRangefinderData( data->time, data->rangefinder.origin, data->rangefinder.ranges); return; case sensor::Data::Type::kOdometer: wrapped_trajectory_builder_->AddOdometerData(data->time, data->odometer_pose); return; } LOG(FATAL); } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/collated_trajectory_builder.h ================================================ #ifndef CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ #include #include #include #include #include #include "cartographer/common/port.h" #include "cartographer/common/rate_timer.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" #include "cartographer/sensor/collator.h" #include "cartographer/sensor/data.h" namespace cartographer { namespace mapping { /* CollatedTrajectoryBuilder类继承自TrajectoryBuilder, 作用：使用Collator处理从传感器收集而来的数据. 并传递给GlobalTrajectoryBuilderInterface。 数据成员包括 sensor::Collator* const sensor_collator_; 传感器收集类实例 const int trajectory_id_; 轨迹id std::unique_ptr wrapped_trajectory_builder_;全局的建图接口 std::chrono::steady_clock::time_point last_logging_time_; 上一次传递数据时间 std::map> rate_timers_;频率 成员函数,重写了父类的接口 submaps()用于建立子图 pose_estimate() 位姿估计 AddSensorData() 添加传感器数据 HandleCollatedSensorData() 处理收集得来的传感器数据 Handles collating sensor data using a sensor::Collator, then passing it on toa mapping::GlobalTrajectoryBuilderInterface which is common for 2D and 3D. */ class CollatedTrajectoryBuilder : public TrajectoryBuilder { public: CollatedTrajectoryBuilder( sensor::Collator* sensor_collator, int trajectory_id, const std::unordered_set& expected_sensor_ids, std::unique_ptr wrapped_trajectory_builder); ~CollatedTrajectoryBuilder() override; CollatedTrajectoryBuilder(const CollatedTrajectoryBuilder&) = delete; CollatedTrajectoryBuilder& operator=(const CollatedTrajectoryBuilder&) = delete; const Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; void AddSensorData(const string& sensor_id, std::unique_ptr data) override; private: void HandleCollatedSensorData(const string& sensor_id, std::unique_ptr data); sensor::Collator* const sensor_collator_; const int trajectory_id_; std::unique_ptr wrapped_trajectory_builder_; // Time at which we last logged the rates of incoming sensor data. std::chrono::steady_clock::time_point last_logging_time_; std::map> rate_timers_; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_COLLATED_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping/detect_floors.cc ================================================ #include "cartographer/mapping/detect_floors.h" #include #include #include #include "Eigen/Core" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping { namespace { // A union-find structure for assigning levels to 'spans' in the trajectory // implemented as a disjoint-set forest: // https://en.wikipedia.org/wiki/Disjoint-set_data_structure#Disjoint-set_forests // TODO(hrapp): We use this elsewhere. Pull out into a common place. using Levels = std::map; constexpr double kMaxShortSpanLengthMeters = 25.; constexpr double kLevelHeightMeters = 2.5; constexpr double kMinLevelSeparationMeters = 1.0; // Indices into 'trajectory.node', so that 'start_index' <= i < 'end_index'. struct Span { int start_index; int end_index; std::vector z_values; bool operator<(const Span& other) const { return std::forward_as_tuple(start_index, end_index) < std::forward_as_tuple(other.start_index, other.end_index); } }; // Union-find implementation for classifying spans into levels. int LevelFind(const int i, const Levels& levels) { auto it = levels.find(i); CHECK(it != levels.end()); if (it->first == it->second) { return it->second; } return LevelFind(it->second, levels); } void LevelUnion(int i, int j, Levels* levels) { const int repr_i = LevelFind(i, *levels); const int repr_j = LevelFind(j, *levels); (*levels)[repr_i] = repr_j; } void InsertSorted(const double val, std::vector* vals) { vals->insert(std::upper_bound(vals->begin(), vals->end(), val), val); } double Median(const std::vector& sorted) { CHECK(!sorted.empty()); return sorted.at(sorted.size() / 2); } // Cut the trajectory at jumps in z. A new span is started when the current // node's z differes by more than kLevelHeightMeters from the median z values. std::vector SliceByAltitudeChange(const proto::Trajectory& trajectory) { CHECK_GT(trajectory.node_size(), 0); std::vector spans; spans.push_back(Span{0, 0, {trajectory.node(0).pose().translation().z()}}); for (int i = 1; i < trajectory.node_size(); ++i) { const auto& node = trajectory.node(i); const double z = node.pose().translation().z(); if (std::abs(Median(spans.back().z_values) - z) > kLevelHeightMeters) { spans.push_back(Span{i, i, {}}); } InsertSorted(z, &spans.back().z_values); spans.back().end_index = i + 1; } return spans; } // Returns the length of 'span' in meters. double SpanLength(const proto::Trajectory& trajectory, const Span& span) { double length = 0; for (int i = span.start_index + 1; i < span.end_index; ++i) { const auto a = transform::ToEigen(trajectory.node(i - 1).pose().translation()); const auto b = transform::ToEigen(trajectory.node(i).pose().translation()); length += (a - b).head<2>().norm(); } return length; } // True if 'span' is considered to be short, i.e. not interesting on its own, // but should be folded into the levels before and after entering it. bool IsShort(const proto::Trajectory& trajectory, const Span& span) { return SpanLength(trajectory, span) < kMaxShortSpanLengthMeters; } // Merges all 'spans' that have similar median z value into the same level. void GroupSegmentsByAltitude(const proto::Trajectory& trajectory, const std::vector& spans, Levels* levels) { for (size_t i = 0; i < spans.size(); ++i) { for (size_t j = i + 1; j < spans.size(); ++j) { if (std::abs(Median(spans[i].z_values) - Median(spans[j].z_values)) < kMinLevelSeparationMeters) { LevelUnion(i, j, levels); } } } } std::vector FindFloors(const proto::Trajectory& trajectory, const std::vector& spans, const Levels& levels) { std::map> level_spans; // Initialize the levels to start out with only long spans. for (size_t i = 0; i < spans.size(); ++i) { const Span& span = spans[i]; if (!IsShort(trajectory, span)) { level_spans[LevelFind(i, levels)].push_back(span); } } for (size_t i = 0; i < spans.size(); ++i) { const Span& span = spans[i]; if (!IsShort(trajectory, span)) { continue; } // If we have a long piece on this floor already, merge this short piece // into it. int level = LevelFind(i, levels); if (!level_spans[level].empty()) { level_spans[level].push_back(span); continue; } // Otherwise, add this short piece to the level before and after it. It is // likely some intermediate level on stairs. size_t index = i - 1; if (index < spans.size()) { level_spans[LevelFind(index, levels)].push_back(span); } index = i + 1; if (index < spans.size()) { level_spans[LevelFind(index, levels)].push_back(span); } } // Convert the level_spans structure to 'Floor'. std::vector floors; for (auto& level : level_spans) { if (level.second.empty()) { continue; } std::vector z_values; std::sort(level.second.begin(), level.second.end()); floors.emplace_back(); for (const auto& span : level.second) { if (!IsShort(trajectory, span)) { // To figure out the median height of this floor, we only care for the // long pieces that are guaranteed to be in the structure. This is a // heuristic to leave out intermediate (short) levels. z_values.insert(z_values.end(), span.z_values.begin(), span.z_values.end()); } floors.back().timespans.push_back(Timespan{ common::FromUniversal(trajectory.node(span.start_index).timestamp()), common::FromUniversal( trajectory.node(span.end_index - 1).timestamp())}); } std::sort(z_values.begin(), z_values.end()); floors.back().z = Median(z_values); } return floors; } } // namespace，只能本文件访问以上函数。 std::vector DetectFloors(const proto::Trajectory& trajectory) { const std::vector spans = SliceByAltitudeChange(trajectory); Levels levels; for (size_t i = 0; i < spans.size(); ++i) { levels[i] = i; } GroupSegmentsByAltitude(trajectory, spans, &levels); std::vector floors = FindFloors(trajectory, spans, levels); std::sort(floors.begin(), floors.end(), [](const Floor& a, const Floor& b) { return a.z < b.z; }); return floors; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/detect_floors.h ================================================ #ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ #define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/common/time.h" namespace cartographer { namespace mapping { /* span,范围 Timespan表征时间范围。 */ struct Timespan { common::Time start; common::Time end; }; /* 一个楼层对应多个扫描timespan：有可能重复的扫描多次 但只有一个高度z。 */ struct Floor { // The spans of time we spent on this floor. Since we might have //walked up and down many times in this place, there can be many spans // of time we spent on a particular floor. std::vector timespans; // The median z-value of this floor. double z; //z轴的中值 }; /* heuristic:启发式, 使用启发式搜索寻找building的不同楼层的z值。 对楼层的要求：同一floor同一z值，只要有“楼梯”出现，即为“产生”一层 */ // Uses a heuristic which looks at z-values of the poses to detect individual // floors of a building. This requires that floors are *mostly* on the same // z-height and that level changes happen *relatively* abrubtly, e.g. by taking // the stairs. std::vector DetectFloors(const proto::Trajectory& trajectory); } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ ================================================ FILE: mapping/global_trajectory_builder_interface.h ================================================ #ifndef CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ #include #include #include #include "cartographer/common/time.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { /* 虚基类,提供一个接口用于2d和3d的slam,不可拷贝/赋值 */ // This interface is used for both 2D and 3D SLAM. Implementations wire up a // global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching // to detect loop closure, and a sparse pose graph optimization to compute // optimized pose estimates. class GlobalTrajectoryBuilderInterface { public: using PoseEstimate = TrajectoryBuilder::PoseEstimate;//位姿估计 GlobalTrajectoryBuilderInterface() {} virtual ~GlobalTrajectoryBuilderInterface() {} GlobalTrajectoryBuilderInterface(const GlobalTrajectoryBuilderInterface&) = delete; GlobalTrajectoryBuilderInterface& operator=( const GlobalTrajectoryBuilderInterface&) = delete; virtual const Submaps* submaps() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; virtual void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) = 0; virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; virtual void AddOdometerData(common::Time time, const transform::Rigid3d& pose) = 0; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_GLOBAL_TRAJECTORY_BUILDER_INTERFACE_H_ ================================================ FILE: mapping/id.h ================================================ #ifndef CARTOGRAPHER_MAPPING_ID_H_ #define CARTOGRAPHER_MAPPING_ID_H_ #include #include #include #include namespace cartographer { namespace mapping { /* 每一个轨迹trajectory上有多个节点node。 如何唯一的表达这些节点node？节点标号:轨迹id+{0,1,2,...} */ // Uniquely identifies a trajectory node using a combination of a unique // trajectory ID and a zero-based index of the node inside that trajectory. struct NodeId { int trajectory_id; int node_index; //比较<符号，成员函数 bool operator<(const NodeId& other) const { return std::forward_as_tuple(trajectory_id, node_index) < std::forward_as_tuple(other.trajectory_id, other.node_index); } }; //<<运算符，友元函数 inline std::ostream& operator<<(std::ostream& os, const NodeId& v) { return os << "(" << v.trajectory_id << ", " << v.node_index << ")"; } /* 一般来说，重建全局地图global map时，是由多个submap组成。 如何给这些submap标号? 轨迹id+ {0,1,2,3...} */ // Uniquely identifies a submap using a combination of a unique trajectory ID // and a zero-based index of the submap inside that trajectory. struct SubmapId { int trajectory_id; int submap_index; bool operator<(const SubmapId& other) const { return std::forward_as_tuple(trajectory_id, submap_index) < std::forward_as_tuple(other.trajectory_id, other.submap_index); } }; //友元函数 inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) { return os << "(" << v.trajectory_id << ", " << v.submap_index << ")"; } /* Nested,嵌套. NestedVectorsById类只有1个数据成员,vector> data_; 作用：嵌套存储多条轨迹线上的数据 对外提供3个操作 1,Append()根据轨迹id和data添加数据 2,at()查询某轨迹对应的vector的某值 3,num_indices(),某轨迹对应的数据的大小 */ template class NestedVectorsById { public: /* data_是vector,size()是轨迹的数量。data_存储的是轨迹id data_[i]也是vector.存储的是轨迹上的data 向data_[i]添加数据。 */ // Appends data to a trajectory, creating trajectories as needed. IdType Append(int trajectory_id, const ValueType& value) { data_.resize(std::max(data_.size(), trajectory_id + 1)); //构造一个IdType对象用于返回，该id是轨迹id+{n} const IdType id{trajectory_id, static_cast(data_[trajectory_id].size())}; data_[trajectory_id].push_back(value); return id; } //在某一轨迹id上对应的data const ValueType& at(const IdType& id) const { //vector的成员函数:at(size_type n);即v[x][y] return data_.at(id.trajectory_id).at(GetIndex(id)); } ValueType& at(const IdType& id) { return data_.at(id.trajectory_id).at(GetIndex(id)); } //轨迹数量data_的size() int num_trajectories() const { return static_cast(data_.size()); } //某个轨迹对应的的大小,data_[i].size() int num_indices(int trajectory_id) const { return static_cast(data_.at(trajectory_id).size()); } // TODO(whess): Remove once no longer needed. const std::vector> data() const { return data_; } private: //返回id对应的index，即{0,1,2,...} static int GetIndex(const NodeId& id) { return id.node_index; } static int GetIndex(const SubmapId& id) { return id.submap_index; } std::vector> data_; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_ID_H_ ================================================ FILE: mapping/imu_tracker.cc ================================================ #include "cartographer/mapping/imu_tracker.h" #include #include #include "cartographer/common/math.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping { /* imu_gravity_time_constant = 10., num_odometry_states = 1000, 构造函数初始化 */ ImuTracker::ImuTracker(const double imu_gravity_time_constant, const common::Time time) : imu_gravity_time_constant_(imu_gravity_time_constant), time_(time), last_linear_acceleration_time_(common::Time::min()),// 1796年 orientation_(Eigen::Quaterniond::Identity()), gravity_vector_(Eigen::Vector3d::UnitZ()), //0,0,1，只有z轴有重力加速度。 imu_angular_velocity_(Eigen::Vector3d::Zero()) {} /* 从time_增加到time。 dt=t-t_; ds=v*dt */ void ImuTracker::Advance(const common::Time time) { CHECK_LE(time_, time); const double delta_t = common::ToSeconds(time - time_); //方向角 const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(imu_angular_velocity_ * delta_t)); //ds=v*dt orientation_ = (orientation_ * rotation).normalized();//更新角度 gravity_vector_ = rotation.inverse() * gravity_vector_;//根据方向角，更新加速度 time_ = time; } /* 更新imu测量得到的加速度。 参数：vector3d，测量值 1),dt=t_-t; 2),alpha=1-e^(-dt/g); 3),gravity_vector_=(1-alpha)*gv_+alpha*imu_line; 4),更新orientation_ exponential,指数. 为何是指数变化： */ void ImuTracker::AddImuLinearAccelerationObservation( const Eigen::Vector3d& imu_linear_acceleration) { // Update the 'gravity_vector_' with an exponential moving average using the // 'imu_gravity_time_constant'. const double delta_t = last_linear_acceleration_time_ > common::Time::min() ? common::ToSeconds(time_ - last_linear_acceleration_time_) : std::numeric_limits::infinity(); last_linear_acceleration_time_ = time_; const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_); gravity_vector_ = (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration; // Change the 'orientation_' so that it agrees with the current // 'gravity_vector_'. const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors( gravity_vector_, orientation_.inverse() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized();//更新方向角 CHECK_GT((orientation_ * gravity_vector_).z(), 0.); CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99); } /* 更新imu测量得到的角速度 */ void ImuTracker::AddImuAngularVelocityObservation( const Eigen::Vector3d& imu_angular_velocity) { imu_angular_velocity_ = imu_angular_velocity; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/imu_tracker.h ================================================ #ifndef CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ #define CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ #include "Eigen/Geometry" #include "cartographer/common/time.h" namespace cartographer { namespace mapping { /* ImuTracker 作用：使用来自imu的角速度+加速度用于跟踪pose的orientation 构造函数指定重力加速度g和最新测量时间。 数据成员包含 1,加速度g 2,测量时间 3,方向,四元数 4,三个加速度3d 5,imu角速度 */ // Keeps track of the orientation using angular velocities and linear // accelerations from an IMU. Because averaged linear acceleration (assuming // slow movement) is a direct measurement of gravity, // roll/pitch does not drift,though yaw does. //x,y轴不会drift漂移。z轴可能会产生漂移。(有累计误差) class ImuTracker { public: ImuTracker(double imu_gravity_time_constant, common::Time time); // Advances to the given 'time' and updates the orientation to reflect this. // 系统时间增加t，更新方向角 void Advance(common::Time time); // Updates from an IMU reading (in the IMU frame). void AddImuLinearAccelerationObservation( //加速度 const Eigen::Vector3d& imu_linear_acceleration); void AddImuAngularVelocityObservation( //角速度 const Eigen::Vector3d& imu_angular_velocity); // Query the current orientation estimate.返回目前估计pose的方向角。 Eigen::Quaterniond orientation() const { return orientation_; } private: const double imu_gravity_time_constant_; // g，10.0 common::Time time_; //最新一次测量时间 common::Time last_linear_acceleration_time_;//加速度测量时间 Eigen::Quaterniond orientation_; //pose的方向角 Eigen::Vector3d gravity_vector_; //加速度测量的方向 Eigen::Vector3d imu_angular_velocity_; //角速度 }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_IMU_TRACKER_H_ /* 预备知识： 惯性测量单元（英文：Inertial measurement unit，简称IMU）是测量物体三轴姿态角(或角速率)以及加速度的装置。 陀螺仪及加速度计是IMU的主要元件，其精度直接影响到惯性系统的精度。在实际工作中，由于不可避免的各种干扰因素，而导致陀螺仪及加速度计产生误差，从初始对准开始，其导航误差就随时间而增长，尤其是位置误差，这是惯导系统的主要缺点。所以需要利用外部信息进行辅助，实现组合导航，使其有效地减小误差随时间积累的问题。为了提高可靠性，还可以为每个轴配备更多的传感器。一般而言IMU要安装在被测物体的重心上。 一般情况，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在导航中有着很重要的应用价值。 IMU大多用在需要进行运动控制的设备，如汽车和机器人上。也被用在需要用姿态进行精密位移推算的场合，如潜艇、飞机、导弹和航天器的惯性导航设备等。 利用三轴地磁解耦和三轴加速度计，受外力加速度影响很大，在运动/振动等环境中，输出方向角误差较大,此外地磁传感器有缺点，它的绝对参照物是地磁场的磁力线,地磁的特点是使用范围大，但强度较低，约零点几高斯，非常容易受到其它磁体的干扰， 如果融合了Z轴陀螺仪的瞬时角度，就可以使系统数据更加稳定。加速度测量的是重力方向，在无外力加速度的情况下，能准确输出ROLL/PITCH两轴姿态角度 并且此角度不会有累积误差，在更长的时间尺度内都是准确的。但是加速度传感器测角度的缺点是加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变，而惯性力与重力本质是一样的,所以加速度计就不会区分重力加速度与外力加速度，当系统在三维空间做变速运动时，它的输出就不正确了。 陀螺仪输出角速度是瞬时量，角速度在姿态平衡上不能直接使用， 需要角速度与时间积分计算角度，得到的角度变化量与初始角度相加，就得到目标角度，其中积分时间Dt越小输出的角度越精确。但陀螺仪的原理决定了它的测量基准是自身，并没有系统外的绝对参照物，加上Dt是不可能无限小的，所以积分的累积误差会随着时间的流逝迅速增加，最终导致输出角度与实际不符，所以陀螺仪只能工作在相对较短的时间尺度内[1] 。 所以在没有其它参照物的基础上，要得到较为真实的姿态角，就要利用加权算法扬长避短，结合两者的优点，摈弃其各自缺点,设计算法在短时间尺度内增加陀螺仪的权值，在更长时间尺度内增加加速度权值，这样系统输出角度就接近真实值了。 roll/pitch does not drift,though yaw does： 如果机器人移动缓慢，那么加速度测量影响因素直接来源于G/g： f=ma，a=f/m，m=G（重力）/g 。（9.8N/kg) */ ================================================ FILE: mapping/map_builder.cc ================================================ #include "cartographer/mapping/map_builder.h" #include #include #include #include #include #include "cartographer/common/make_unique.h" #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/mapping_2d/global_trajectory_builder.h" #include "cartographer/mapping_3d/global_trajectory_builder.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping { //backpack_3d.lua/revo_lds.lua/taurob_tracker.lua /* include "map_builder.lua" include "trajectory_builder.lua" options = { map_builder = MAP_BUILDER, trajectory_builder = TRAJECTORY_BUILDER, map_frame = "map", tracking_frame = "base_link", published_frame = "base_link", odom_frame = "odom", provide_odom_frame = true, use_odometry = false, use_laser_scan = false, use_multi_echo_laser_scan = false, num_point_clouds = 2, lookup_transform_timeout_sec = 0.2, submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, } TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160 MAP_BUILDER.use_trajectory_builder_3d = true MAP_BUILDER.num_background_threads = 7 MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2 MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 320 MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03 MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10 -- Reuse the coarser 3D voxel filter to speed up the computation of loop closure -- constraints. MAP_BUILDER.sparse_pose_graph.constraint_builder.adaptive_voxel_filter = TRAJECTORY_BUILDER_3D.high_resolution_adaptive_voxel_filter MAP_BUILDER.sparse_pose_graph.constraint_builder.min_score = 0.62 return options */ proto::MapBuilderOptions CreateMapBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::MapBuilderOptions options; options.set_use_trajectory_builder_2d( parameter_dictionary->GetBool("use_trajectory_builder_2d"));//false options.set_use_trajectory_builder_3d( parameter_dictionary->GetBool("use_trajectory_builder_3d"));//true options.set_num_background_threads( parameter_dictionary->GetNonNegativeInt("num_background_threads"));//7 //sparse_pose_graph.lua *options.mutable_sparse_pose_graph_options() = CreateSparsePoseGraphOptions( parameter_dictionary->GetDictionary("sparse_pose_graph").get()); CHECK_NE(options.use_trajectory_builder_2d(), options.use_trajectory_builder_3d()); return options; } MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) : options_(options), thread_pool_(options.num_background_threads()) { if (options.use_trajectory_builder_2d()) { sparse_pose_graph_2d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); sparse_pose_graph_ = sparse_pose_graph_2d_.get(); } if (options.use_trajectory_builder_3d()) { sparse_pose_graph_3d_ = common::make_unique( options_.sparse_pose_graph_options(), &thread_pool_); sparse_pose_graph_ = sparse_pose_graph_3d_.get(); } } MapBuilder::~MapBuilder() {} int MapBuilder::AddTrajectoryBuilder( const std::unordered_set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options) { const int trajectory_id = trajectory_builders_.size(); if (options_.use_trajectory_builder_3d()) { CHECK(trajectory_options.has_trajectory_builder_3d_options()); trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( trajectory_options.trajectory_builder_3d_options(), trajectory_id, sparse_pose_graph_3d_.get()))); } else { CHECK(trajectory_options.has_trajectory_builder_2d_options()); trajectory_builders_.push_back( common::make_unique( &sensor_collator_, trajectory_id, expected_sensor_ids, common::make_unique( trajectory_options.trajectory_builder_2d_options(), trajectory_id, sparse_pose_graph_2d_.get()))); } return trajectory_id; } TrajectoryBuilder* MapBuilder::GetTrajectoryBuilder( const int trajectory_id) const { return trajectory_builders_.at(trajectory_id).get(); } void MapBuilder::FinishTrajectory(const int trajectory_id) { sensor_collator_.FinishTrajectory(trajectory_id); } int MapBuilder::GetBlockingTrajectoryId() const { return sensor_collator_.GetBlockingTrajectoryId(); } proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() { return ToProto(sparse_pose_graph_->GetConnectedTrajectories()); } string MapBuilder::SubmapToProto(const int trajectory_id, const int submap_index, proto::SubmapQuery::Response* const response) { if (trajectory_id < 0 || trajectory_id >= num_trajectory_builders()) { return "Requested submap from trajectory " + std::to_string(trajectory_id) + " but there are only " + std::to_string(num_trajectory_builders()) + " trajectories."; } const std::vector submap_transforms = sparse_pose_graph_->GetSubmapTransforms(trajectory_id); if (submap_index < 0 || static_cast(submap_index) >= submap_transforms.size()) { return "Requested submap " + std::to_string(submap_index) + " from trajectory " + std::to_string(trajectory_id) + " but there are only " + std::to_string(submap_transforms.size()) + " submaps in this trajectory."; } const Submaps* const submaps = trajectory_builders_.at(trajectory_id)->submaps(); response->set_submap_version(submaps->Get(submap_index)->num_range_data); submaps->SubmapToProto(submap_index, submap_transforms[submap_index], response); return ""; } int MapBuilder::num_trajectory_builders() const { return trajectory_builders_.size(); } SparsePoseGraph* MapBuilder::sparse_pose_graph() { return sparse_pose_graph_; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/map_builder.h ================================================ #ifndef CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #define CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ #include #include #include #include #include #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/sensor/collator.h" namespace cartographer { namespace mapping { proto::MapBuilderOptions CreateMapBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary); /* MapBuilder类,建图,不可拷贝/赋值 MapBuilder类和TrajectoryBuilder类即真正的开始重建局部子图submaps, 并且采集稀疏位姿图用于闭环检测。 1)构造函数根据options初始化数据成员，包括线程数量和sparse_pose_graph_options。 Wires up（为…） 接通电源; Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps) and a SparsePoseGraph for loop closure. */ class MapBuilder { public: MapBuilder(const proto::MapBuilderOptions& options); ~MapBuilder(); MapBuilder(const MapBuilder&) = delete; MapBuilder& operator=(const MapBuilder&) = delete; // Create a new trajectory and return its index. //根据传感器id和options新建一个轨迹线，返回轨迹线的索引 int AddTrajectoryBuilder( const std::unordered_set& expected_sensor_ids, const proto::TrajectoryBuilderOptions& trajectory_options); // Returns the TrajectoryBuilder corresponding to the specified // 'trajectory_id'. //根据轨迹id返回指向该轨迹的TrajectoryBuilder对象指针。 mapping::TrajectoryBuilder* GetTrajectoryBuilder(int trajectory_id) const; // Marks the TrajectoryBuilder corresponding to 'trajectory_id' as finished, // i.e. no further sensor data is expected. //标记该轨迹已完成data采集，后续不再接收data void FinishTrajectory(int trajectory_id); // Must only be called if at least one unfinished trajectory exists. Returns // the ID of the trajectory that needs more data before the MapBuilder is // unblocked. //阻塞的轨迹，常见于该条轨迹上的传感器迟迟不提交data。 int GetBlockingTrajectoryId() const; // Returns the trajectory connectivity.获得一系列轨迹的连通域 proto::TrajectoryConnectivity GetTrajectoryConnectivity(); // Fills the SubmapQuery::Response corresponding to 'submap_index' from // 'trajectory_id'. Returns an error string on failure, or an empty string on // success. //把轨迹id和子图索引对应的submap，序列化到文件 string SubmapToProto(int trajectory_id, int submap_index, proto::SubmapQuery::Response* response); //在建图的轨迹数量 int num_trajectory_builders() const; mapping::SparsePoseGraph* sparse_pose_graph(); private: const proto::MapBuilderOptions options_; // 建图选项 common::ThreadPool thread_pool_; //线程数量，不可变。 std::unique_ptr sparse_pose_graph_2d_; //稀疏2D图 std::unique_ptr sparse_pose_graph_3d_; //稀疏3D图 mapping::SparsePoseGraph* sparse_pose_graph_; //稀疏位姿 sensor::Collator sensor_collator_; //收集传感器采集的数据 std::vector> trajectory_builders_;//轨迹线集合 }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_MAP_BUILDER_H_ ================================================ FILE: mapping/odometry_state_tracker.cc ================================================ #include "cartographer/mapping/odometry_state_tracker.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { OdometryState::OdometryState(const common::Time time, const transform::Rigid3d& odometer_pose, const transform::Rigid3d& state_pose) : time(time), odometer_pose(odometer_pose), state_pose(state_pose) {} OdometryStateTracker::OdometryStateTracker(const int window_size) : window_size_(window_size) { CHECK_GT(window_size, 0); } const OdometryStateTracker::OdometryStates& OdometryStateTracker::odometry_states() const { return odometry_states_; } //添新删旧 void OdometryStateTracker::AddOdometryState( const OdometryState& odometry_state) { odometry_states_.push_back(odometry_state); while (odometry_states_.size() > window_size_) { odometry_states_.pop_front(); } } bool OdometryStateTracker::empty() const { return odometry_states_.empty(); } //返回最后一个 const OdometryState& OdometryStateTracker::newest() const { return odometry_states_.back(); } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/odometry_state_tracker.h ================================================ #ifndef CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_ #define CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_ #include #include "cartographer/common/time.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { /* .lua设置： imu_gravity_time_constant = 10., num_odometry_states = 1000, OdometryState类含3个数据成员 1,time，时间 2,Rigid3d，里程计的位置 3,Rigid3d，状态位置 */ struct OdometryState { OdometryState(common::Time time, const transform::Rigid3d& odometer_pose, const transform::Rigid3d& state_pose); OdometryState() {} common::Time time = common::Time::min(); transform::Rigid3d odometer_pose = transform::Rigid3d::Identity(); transform::Rigid3d state_pose = transform::Rigid3d::Identity(); }; /* OdometryStateTracker:记录跟踪里程计的多个状态类,maxsize :window_size 含义2个数据成员 1,OdometryStates，记录多个里程计状态，是一个双端队列deque 2,window_size_,滑动窗大小,即队列大小 成员函数： 1，构造函数初始化滑动窗大小. 2，AddOdometryState()添加一个新的里程计状态，若deque已满则删除旧有的States */ // Keeps track of the odometry states by keeping sliding window over some // number of them. class OdometryStateTracker { public: using OdometryStates = std::deque; explicit OdometryStateTracker(int window_size); const OdometryStates& odometry_states() const; /*添加新的里程计状态,超出滑动窗大小时,旧的删除*/ // Adds a new 'odometry_state' and makes sure the maximum number of previous // odometry states is not exceeded. void AddOdometryState(const OdometryState& odometry_state); // Returns true if no elements are present in the odometry queue. bool empty() const; // Retrieves the most recent OdometryState. Must not be called when empty. const OdometryState& newest() const; private: OdometryStates odometry_states_; size_t window_size_; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_ODOMETRY_STATE_TRACKER_H_ ================================================ FILE: mapping/probability_values.cc ================================================ #include "cartographer/mapping/probability_values.h" namespace cartographer { namespace mapping { namespace { //直接使用math计算法,相比于查表法，较慢。所以先用math计算好， //然后储存到table中，以后直接查找value对应的p // 0 is unknown, [1, 32767] maps to [kMinProbability, kMaxProbability]. float SlowValueToProbability(const uint16 value) { CHECK_GE(value, 0); CHECK_LE(value, 32767); if (value == kUnknownProbabilityValue) { //没有概率就返回最低概率 // Unknown cells have kMinProbability. return kMinProbability; } const float kScale = (kMaxProbability - kMinProbability) / 32766.f; return value * kScale + (kMinProbability - kScale);//返回[0.1,0.9] } /* 查表法,快速.类似于9*9乘法表的作用,vector存储的是提前计算好的value到p的映射。 提前计算好[1,32767]到[0.1,0.9]之间的映射关系。 返回值：vector: 长度size()=32767*2. 即[0,1,2,...,32767, 0,1,2,...,32767] */ const std::vector* PrecomputeValueToProbability() { std::vector* result = new std::vector; // Repeat two times, so that both values with and without the update marker // can be converted to a probability. for (int repeat = 0; repeat != 2; ++repeat) { for (int value = 0; value != 32768; ++value) { result->push_back(SlowValueToProbability(value)); } } return result; } } // namespace //定义,计算value到p的映射：[0,32767]->[0.1,0.9] const std::vector* const kValueToProbability = PrecomputeValueToProbability(); /* 参数：odds， 返回值：vector,size()=1+32767. 之前没有hit过，则没有update，按论文公式(2)计算： 求p, 求[1,32767], 求[1,32767]+32768 push_back() 之前有hit过，则有update，按论文公式(3)计算： 求(*kValueToProbability)[cell]->[0.1,0.9]即原始p 求p'=odds*Odds(p) 求p'映射到[1,32767] push_back():[1,32767]+32768. */ std::vector ComputeLookupTableToApplyOdds(const float odds) { std::vector result; result.push_back(ProbabilityToValue(ProbabilityFromOdds(odds)) + kUpdateMarker);//标记没有update。 //标记有update。 for (int cell = 1; cell != 32768; ++cell) { result.push_back(ProbabilityToValue(ProbabilityFromOdds( odds * Odds((*kValueToProbability)[cell]))) + kUpdateMarker); } return result; } } // namespace mapping } // namespace cartographer /* kUpdateMarker:32768 */ ================================================ FILE: mapping/probability_values.h ================================================ #ifndef CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ #define CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ #include #include #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "glog/logging.h" namespace cartographer { namespace mapping { /* 多个用于计算概率的mapping命名空间下的全局函数 */ inline float Odds(float probability) { //论文公式(2),求胜负比。y=x/(1-x) return probability / (1.f - probability); } inline float ProbabilityFromOdds(const float odds) { //求概率，即x=y/(1+y) return odds / (odds + 1.f); } constexpr float kMinProbability = 0.1f;//p最小是0.1 constexpr float kMaxProbability = 1.f - kMinProbability;//最大是0.9 /* 限制概率p在[0.1,0.9]之间 */ // Clamps probability to be in the range [kMinProbability, kMaxProbability]. inline float ClampProbability(const float probability) { return common::Clamp(probability, kMinProbability, kMaxProbability); } constexpr uint16 kUnknownProbabilityValue = 0;//标记未初始化的概率 constexpr uint16 kUpdateMarker = 1u << 15;// 32768 /* 将概率p映射为整数Value， [0.0,1.0]: ->[0.1,0.9] ->[1,32767] */ // Converts a probability to a uint16 in the [1, 32767] range. inline uint16 ProbabilityToValue(const float probability) { const int value = common::RoundToInt((ClampProbability(probability) - kMinProbability) * (32766.f / (kMaxProbability - kMinProbability))) + 1; // DCHECK for performance. DCHECK_GE(value, 1); DCHECK_LE(value, 32767); return value; } //声明，定义在.cc文件。vector是value到p的映射 extern const std::vector* const kValueToProbability; /* 反映射 [1,32767]->[0.1,0.9] 利用查表法，快速获得值，而不是用math函数求值。 */ // Converts a uint16 (which may or may not have the update marker set) to a // probability in the range [kMinProbability, kMaxProbability]. inline float ValueToProbability(const uint16 value) { return (*kValueToProbability)[value]; } //2份value:前一半对应没有hit，后一半对应hit。 std::vector ComputeLookupTableToApplyOdds(float odds); } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_PROBABILITY_VALUES_H_ /* Odds()含义: Odds are ratios of a player’s chances of losing to his or her chances of winning, or the average frequency of a loss to the average frequency of a win. http://www.problemgambling.ca/en/resourcesforprofessionals/pages/probabilityoddsandrandomchance.aspx */ ================================================ FILE: mapping/probability_values_test.cc ================================================ #include "cartographer/mapping/probability_values.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping { namespace { TEST(ProbabilityValuesTest, OddsConversions) { EXPECT_NEAR(ProbabilityFromOdds(Odds(kMinProbability)), kMinProbability, 1e-6); EXPECT_NEAR(ProbabilityFromOdds(Odds(kMaxProbability)), kMaxProbability, 1e-6); EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6); } } // namespace } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/proto/map_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping/proto/sparse_pose_graph_options.proto"; package cartographer.mapping.proto; message MapBuilderOptions { optional bool use_trajectory_builder_2d = 1; optional bool use_trajectory_builder_3d = 2; // Number of threads to use for background computations. optional int32 num_background_threads = 3; optional SparsePoseGraphOptions sparse_pose_graph_options = 4; } ================================================ FILE: mapping/proto/sparse_pose_graph.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.proto; import "cartographer/mapping/proto/trajectory.proto"; import "cartographer/transform/proto/transform.proto"; message SparsePoseGraph { message Constraint { message SubmapId { optional int32 trajectory_id = 1; optional int32 submap_index = 2; // Submap index in the given trajectory. } message ScanId { optional int32 trajectory_id = 1; optional int32 scan_index = 2; // Scan index in the given trajectory. } // Differentiates between intra-submap (where the scan was inserted into the // submap) and inter-submap constraints (where the scan was not inserted // into the submap). enum Tag { INTRA_SUBMAP = 0; INTER_SUBMAP = 1; } optional SubmapId submap_id = 1; // Submap ID. optional ScanId scan_id = 2; // Scan ID. // Pose of the scan relative to submap, i.e. taking data from the scan frame // into the submap frame. optional transform.proto.Rigid3d relative_pose = 3; // Weight of the translational part of the constraint. optional double translation_weight = 6; // Weight of the rotational part of the constraint. optional double rotation_weight = 7; optional Tag tag = 5; } repeated Constraint constraint = 2; repeated Trajectory trajectory = 4; } ================================================ FILE: mapping/proto/sparse_pose_graph_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.proto; import "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto"; import "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.proto"; message SparsePoseGraphOptions { // Online loop closure: If positive, will run the loop closure while the map // is built. optional int32 optimize_every_n_scans = 1; // Options for the constraint builder. optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options = 3; // Weight used in the optimization problem for the translational component of // non-loop-closure scan matcher constraints. optional double matcher_translation_weight = 7; // Weight used in the optimization problem for the rotational component of // non-loop-closure scan matcher constraints. optional double matcher_rotation_weight = 8; // Options for the optimization problem. optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options = 4; // Number of iterations to use in 'optimization_problem_options' for the final // optimization. optional int32 max_num_final_iterations = 6; // Rate at which we sample a single trajectory's scans for global // localization. optional double global_sampling_ratio = 5; } ================================================ FILE: mapping/proto/submap_visualization.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/transform/proto/transform.proto"; package cartographer.mapping.proto; message SubmapList { message SubmapEntry { optional int32 submap_version = 1; optional transform.proto.Rigid3d pose = 3; } message TrajectorySubmapList { repeated SubmapEntry submap = 1; } repeated TrajectorySubmapList trajectory = 2; } message SubmapQuery { message Request { // Index into 'SubmapList.trajectory(trajectory_id).submap'. optional int32 submap_index = 1; // Index into 'TrajectoryList.trajectory'. optional int32 trajectory_id = 2; } message Response { // Version of the given submap, higher means newer. optional int32 submap_version = 2; // GZipped map data, in row-major order, starting with (0,0). Each cell // consists of two bytes: value (premultiplied by alpha) and alpha. optional bytes cells = 3; // Dimensions of the grid in cells. optional int32 width = 4; optional int32 height = 5; // Size of one cell in meters. optional double resolution = 6; // Pose of the resolution*width x resolution*height rectangle in the submap // frame. optional transform.proto.Rigid3d slice_pose = 9; // Error message in response to malformed requests. optional string error_message = 8; } optional Request request = 1; optional Response response = 2; } ================================================ FILE: mapping/proto/trajectory.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.proto; import "cartographer/transform/proto/transform.proto"; option java_outer_classname = "TrajectoryOuterClass"; message Trajectory { // NEXT_ID: 7 message Node { optional int64 timestamp = 1; // Transform from tracking to map frame. optional transform.proto.Rigid3d pose = 5; } message Submap { optional transform.proto.Rigid3d pose = 1; } // Time-ordered sequence of Nodes. repeated Node node = 1; // Submaps associated with the trajectory. repeated Submap submap = 2; } ================================================ FILE: mapping/proto/trajectory_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto"; import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; package cartographer.mapping.proto; message TrajectoryBuilderOptions { optional mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options = 1; optional mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options = 2; } ================================================ FILE: mapping/proto/trajectory_connectivity.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.proto; // This is how proto2 calls the outer class since there is already a message // with the same name in the file. option java_outer_classname = "TrajectoryConnectivityOuterClass"; // Connectivity structure between trajectories. message TrajectoryConnectivity { message ConnectedComponent { repeated int32 trajectory_id = 1; } repeated ConnectedComponent connected_component = 1; } ================================================ FILE: mapping/sparse_pose_graph/constraint_builder.cc ================================================ #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping { namespace sparse_pose_graph { //参数值可以在sparse_pose_graph.lua查看。 proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::ConstraintBuilderOptions options; options.set_sampling_ratio(parameter_dictionary->GetDouble("sampling_ratio")); options.set_max_constraint_distance( parameter_dictionary->GetDouble("max_constraint_distance")); *options.mutable_adaptive_voxel_filter_options() = sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); options.set_min_score(parameter_dictionary->GetDouble("min_score")); options.set_global_localization_min_score( parameter_dictionary->GetDouble("global_localization_min_score")); options.set_loop_closure_translation_weight( parameter_dictionary->GetDouble("loop_closure_translation_weight")); options.set_loop_closure_rotation_weight( parameter_dictionary->GetDouble("loop_closure_rotation_weight")); options.set_log_matches(parameter_dictionary->GetBool("log_matches")); *options.mutable_fast_correlative_scan_matcher_options() = mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions( parameter_dictionary->GetDictionary("fast_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = mapping_2d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_fast_correlative_scan_matcher_options_3d() = mapping_3d::scan_matching::CreateFastCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("fast_correlative_scan_matcher_3d") .get()); *options.mutable_ceres_scan_matcher_options_3d() = mapping_3d::scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher_3d").get()); return options; } } // namespace sparse_pose_graph } // namespace mapping } // namespace cartographer /* sparse_pose_graph.lua参数： constraint_builder = { sampling_ratio = 0.3, max_constraint_distance = 15., adaptive_voxel_filter = { max_length = 0.9, min_num_points = 100, max_range = 50., }, min_score = 0.55, global_localization_min_score = 0.6, loop_closure_translation_weight = 1.1e4, loop_closure_rotation_weight = 1e5, log_matches = true, fast_correlative_scan_matcher = { linear_search_window = 7., angular_search_window = math.rad(30.), branch_and_bound_depth = 7, }, ceres_scan_matcher = { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, num_threads = 1, }, }, fast_correlative_scan_matcher_3d = { branch_and_bound_depth = 8, full_resolution_depth = 3, rotational_histogram_size = 120, min_rotational_score = 0.77, linear_xy_search_window = 4., linear_z_search_window = 1., angular_search_window = math.rad(15.), }, ceres_scan_matcher_3d = { occupied_space_weight_0 = 20., translation_weight = 10., rotation_weight = 1., only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 10, num_threads = 1, }, }, }, */ ================================================ FILE: mapping/sparse_pose_graph/constraint_builder.h ================================================ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" namespace cartographer { namespace mapping { namespace sparse_pose_graph { //获取sparse_pose_graph.lua配置参数 proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace sparse_pose_graph } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ ================================================ FILE: mapping/sparse_pose_graph/optimization_problem_options.cc ================================================ #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/common/ceres_solver_options.h" namespace cartographer { namespace mapping { namespace sparse_pose_graph { /* sparse_pose_graph.lua: optimization_problem = { huber_scale = 1e1, acceleration_weight = 1e3, rotation_weight = 3e5, consecutive_scan_translation_penalty_factor = 1e5, consecutive_scan_rotation_penalty_factor = 1e5, log_solver_summary = false, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 50, num_threads = 7, }, }, 参数配置如上。 */ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::OptimizationProblemOptions options; options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); options.set_acceleration_weight( parameter_dictionary->GetDouble("acceleration_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); options.set_consecutive_scan_translation_penalty_factor( parameter_dictionary->GetDouble( "consecutive_scan_translation_penalty_factor")); options.set_consecutive_scan_rotation_penalty_factor( parameter_dictionary->GetDouble( "consecutive_scan_rotation_penalty_factor")); options.set_log_solver_summary( parameter_dictionary->GetBool("log_solver_summary")); *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); return options; } } // namespace sparse_pose_graph } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/sparse_pose_graph/optimization_problem_options.h ================================================ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" namespace cartographer { namespace mapping { namespace sparse_pose_graph { /* 从sparse_pose_graph.lua文件配置 获取参数 message OptimizationProblemOptions { optional double huber_scale = 1; optional double acceleration_weight = 8; optional double rotation_weight = 9; optional double consecutive_scan_translation_penalty_factor = 2; optional double consecutive_scan_rotation_penalty_factor = 3; optional bool log_solver_summary = 5; optional common.proto.CeresSolverOptions ceres_solver_options = 7; } */ proto::OptimizationProblemOptions CreateOptimizationProblemOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace sparse_pose_graph } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ ================================================ FILE: mapping/sparse_pose_graph/proto/constraint_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.sparse_pose_graph.proto; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto"; message ConstraintBuilderOptions { // A constraint will be added if the proportion of added constraints to // potential constraints drops below this number. optional double sampling_ratio = 1; // Threshold for poses to be considered near a submap. optional double max_constraint_distance = 2; // Voxel filter used to compute a sparser point cloud for matching. optional sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 3; // Threshold for the scan match score below which a match is not considered. // Low scores indicate that the scan and map do not look similar. optional double min_score = 4; // Threshold below which global localizations are not trusted. optional double global_localization_min_score = 5; // Weight used in the optimization problem for the translational component of // loop closure constraints. optional double loop_closure_translation_weight = 13; // Weight used in the optimization problem for the rotational component of // loop closure constraints. optional double loop_closure_rotation_weight = 14; // If enabled, logs information of loop-closing constraints for debugging. optional bool log_matches = 8; // Options for the internally used scan matchers. optional mapping_2d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options = 9; optional mapping_2d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 11; optional mapping_3d.scan_matching.proto.FastCorrelativeScanMatcherOptions fast_correlative_scan_matcher_options_3d = 10; optional mapping_3d.scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options_3d = 12; } ================================================ FILE: mapping/sparse_pose_graph/proto/optimization_problem_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping.sparse_pose_graph.proto; import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 11 message OptimizationProblemOptions { // Scaling parameter for Huber loss function. optional double huber_scale = 1; // Scaling parameter for the IMU acceleration term. optional double acceleration_weight = 8; // Scaling parameter for the IMU rotation term. optional double rotation_weight = 9; // Penalty factors for changes to the relative pose between consecutive scans. optional double consecutive_scan_translation_penalty_factor = 2; optional double consecutive_scan_rotation_penalty_factor = 3; // If true, the Ceres solver summary will be logged for every optimization. optional bool log_solver_summary = 5; optional common.proto.CeresSolverOptions ceres_solver_options = 7; } ================================================ FILE: mapping/sparse_pose_graph.cc ================================================ #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping { proto::SparsePoseGraph::Constraint::Tag ToProto( const SparsePoseGraph::Constraint::Tag& tag) { switch (tag) { case SparsePoseGraph::Constraint::Tag::INTRA_SUBMAP: return proto::SparsePoseGraph::Constraint::INTRA_SUBMAP; case SparsePoseGraph::Constraint::Tag::INTER_SUBMAP: return proto::SparsePoseGraph::Constraint::INTER_SUBMAP; } LOG(FATAL) << "Unsupported tag."; } //sparse_pose_graph.lua proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::SparsePoseGraphOptions options; options.set_optimize_every_n_scans( parameter_dictionary->GetInt("optimize_every_n_scans")); *options.mutable_constraint_builder_options() = sparse_pose_graph::CreateConstraintBuilderOptions( parameter_dictionary->GetDictionary("constraint_builder").get()); options.set_matcher_translation_weight( parameter_dictionary->GetDouble("matcher_translation_weight"));//5e2 options.set_matcher_rotation_weight( parameter_dictionary->GetDouble("matcher_rotation_weight"));// 1.6e3 *options.mutable_optimization_problem_options() = sparse_pose_graph::CreateOptimizationProblemOptions( parameter_dictionary->GetDictionary("optimization_problem").get()); options.set_max_num_final_iterations( parameter_dictionary->GetNonNegativeInt("max_num_final_iterations")); //200 CHECK_GT(options.max_num_final_iterations(), 0); options.set_global_sampling_ratio( parameter_dictionary->GetDouble("global_sampling_ratio"));//0.003 return options; } proto::SparsePoseGraph SparsePoseGraph::ToProto() { proto::SparsePoseGraph proto; for (const auto& constraint : constraints()) { auto* const constraint_proto = proto.add_constraint(); *constraint_proto->mutable_relative_pose() = transform::ToProto(constraint.pose.zbar_ij); constraint_proto->set_translation_weight( constraint.pose.translation_weight); constraint_proto->set_rotation_weight(constraint.pose.rotation_weight); constraint_proto->mutable_submap_id()->set_trajectory_id( constraint.submap_id.trajectory_id); constraint_proto->mutable_submap_id()->set_submap_index( constraint.submap_id.submap_index); constraint_proto->mutable_scan_id()->set_trajectory_id( constraint.node_id.trajectory_id); constraint_proto->mutable_scan_id()->set_scan_index( constraint.node_id.node_index); constraint_proto->set_tag(mapping::ToProto(constraint.tag)); } for (const auto& trajectory_nodes : GetTrajectoryNodes()) { auto* trajectory_proto = proto.add_trajectory(); for (const auto& node : trajectory_nodes) { auto* node_proto = trajectory_proto->add_node(); node_proto->set_timestamp(common::ToUniversal(node.constant_data->time)); *node_proto->mutable_pose() = transform::ToProto(node.pose * node.constant_data->tracking_to_pose); } if (!trajectory_nodes.empty()) { for (const auto& transform : GetSubmapTransforms( trajectory_nodes[0].constant_data->trajectory_id)) { *trajectory_proto->add_submap()->mutable_pose() = transform::ToProto(transform); } } } return proto; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/sparse_pose_graph.h ================================================ #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ #include #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/proto/sparse_pose_graph.pb.h" #include "cartographer/mapping/proto/sparse_pose_graph_options.pb.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { /* 根据sparse_pose_graph.lua文件设置option */ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( common::LuaParameterDictionary* const parameter_dictionary); /* SparsePoseGraph:稀疏位姿图模型,虚基类,提供多个抽象接口,不可拷贝/赋值 有一个Constraint的内部类, */ class SparsePoseGraph { public: // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse // pose adjustment for 2d mapping." Intelligent Robots and Systems (IROS), // 2010 IEEE/RSJ International Conference on (pp. 22--29). IEEE, 2010. struct Constraint { //约束 struct Pose { transform::Rigid3d zbar_ij; //paper-公式(2) double translation_weight; double rotation_weight; }; mapping::SubmapId submap_id; // 'i' in the paper. mapping::NodeId node_id; // 'j' in the paper. // Pose of the scan 'j' relative to submap 'i'. Pose pose; // Differentiates between intra-submap (where scan 'j' was inserted into // submap 'i') and inter-submap constraints (where scan 'j' was not inserted // into submap 'i'). enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag; }; SparsePoseGraph() {} virtual ~SparsePoseGraph() {} SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; // Computes optimized poses. //计算优化后的位姿估计 virtual void RunFinalOptimization() = 0; // Get the current trajectory clusters. //获取已连接的轨迹集合 virtual std::vector> GetConnectedTrajectories() = 0; // Returns the current optimized transforms for the given 'trajectory_id'. //获取优化后的位姿估计的3D变换 virtual std::vector GetSubmapTransforms( int trajectory_id) = 0; // Returns the transform converting data in the local map frame (i.e. the // continuous, non-loop-closed frame) into the global map frame (i.e. the // discontinuous, loop-closed frame). virtual transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) = 0; // Returns the current optimized trajectories. //优化后的轨迹线 virtual std::vector> GetTrajectoryNodes() = 0; // Serializes the constraints and trajectories. proto::SparsePoseGraph ToProto(); // Returns the collection of constraints. //获取约束集 virtual std::vector constraints() = 0; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_H_ ================================================ FILE: mapping/submaps.cc ================================================ #include "cartographer/mapping/submaps.h" #include #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping { constexpr uint8 Submaps::kUnknownLogOdds; Submaps::Submaps() {} Submaps::~Submaps() {} int Submaps::matching_index() const { if (size() > 1) { return size() - 2; } return size() - 1; } std::vector Submaps::insertion_indices() const { if (size() > 1) { return {size() - 2, size() - 1}; } return {size() - 1}; } void Submaps::AddProbabilityGridToResponse( const transform::Rigid3d& local_submap_pose, const mapping_2d::ProbabilityGrid& probability_grid, proto::SubmapQuery::Response* response) { Eigen::Array2i offset; mapping_2d::CellLimits limits; probability_grid.ComputeCroppedLimits(&offset, &limits); string cells; for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator(limits)) { if (probability_grid.IsKnown(xy_index + offset)) { // We would like to add 'delta' but this is not possible using a value and // alpha. We use premultiplied alpha, so when 'delta' is positive we can // add it by setting 'alpha' to zero. If it is negative, we set 'value' to // zero, and use 'alpha' to subtract. This is only correct when the pixel // is currently white, so walls will look too gray. This should be hard to // detect visually for the user, though. const int delta = 128 - ProbabilityToLogOddsInteger( probability_grid.GetProbability(xy_index + offset)); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; cells.push_back(value); cells.push_back((value || alpha) ? alpha : 1); } else { cells.push_back(static_cast(kUnknownLogOdds)); // value cells.push_back(0); // alpha } } common::FastGzipString(cells, response->mutable_cells()); response->set_width(limits.num_x_cells); response->set_height(limits.num_y_cells); const double resolution = probability_grid.limits().resolution(); response->set_resolution(resolution); const double max_x = probability_grid.limits().max().x() - resolution * offset.y(); const double max_y = probability_grid.limits().max().y() - resolution * offset.x(); *response->mutable_slice_pose() = transform::ToProto( local_submap_pose.inverse() * transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.))); } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/submaps.h ================================================ #ifndef CARTOGRAPHER_MAPPING_SUBMAPS_H_ #define CARTOGRAPHER_MAPPING_SUBMAPS_H_ #include #include #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/probability_grid.h" #include "glog/logging.h" namespace cartographer { namespace mapping { //求odds(p)的log对数 // Converts the given probability to log odds. inline float Logit(float probability) { return std::log(probability / (1.f - probability)); } const float kMaxLogOdds = Logit(kMaxProbability); const float kMinLogOdds = Logit(kMinProbability); /* 将[0.1,0.9]映射为0-255之间的数 */ // Converts a probability to a log odds integer. 0 means unknown, [kMinLogOdds, // kMaxLogOdds] is mapped to [1, 255]. inline uint8 ProbabilityToLogOddsInteger(const float probability) { const int value = common::RoundToInt((Logit(probability) - kMinLogOdds) * 254.f / (kMaxLogOdds - kMinLogOdds)) + 1; CHECK_LE(1, value); CHECK_GE(255, value); return value; } /* 一个独立的子图,在局部slam frame中有一个local_pose, 追踪有多少range data 添加到其中.并设置finished_probability_grid用于闭环检查 数据成员有 1,local_pose 位姿 2,num_range_data 已经插入的测量数据数量 3,finished_probability_grid 完成建图的概率网格 */ // An individual submap, which has a 'local_pose' in the local SLAM frame, keeps // track of how many range data were inserted into it, and sets the // 'finished_probability_grid' to be used for loop closing once the map no // longer changes. struct Submap { Submap(const transform::Rigid3d& local_pose) : local_pose(local_pose) {} // Local SLAM pose of this submap. const transform::Rigid3d local_pose;//子图的位姿 // Number of RangeData inserted. int num_range_data = 0; // The 'finished_probability_grid' when this submap is finished and will not // change anymore. Otherwise, this is nullptr and the next call to // InsertRangeData() will change the submap. //当子图不再变化时，指向该子图的概率分布网格。 const mapping_2d::ProbabilityGrid* finished_probability_grid = nullptr; }; /* Submaps is a sequence of maps to which scans are matched and into which scans are inserted. Except during initialization when only a single submap exists, there are always two submaps into which scans are inserted: an old submap that is used for matching, and a new one, which will be used for matching next, that is being initialized. Once a certain number of scans have been inserted, the new submap is considered initialized: the old submap is no longer changed, the "new" submap is now the "old" submap and is used for scan-to-map matching. Moreover, a "new" submap gets inserted. Submaps是一连串的子图,初始化以后任何阶段均有2个子图被当前scan point影响： old submap 用于now match,new submap用于next match,一直交替下去。 一旦new submap有足够多的scan point，那么old submap不再更新。 此时new submap变为old，用于 scan-to-map匹配。 Submaps不可拷贝/赋值,是虚基类 虚基类,没有数据成员,只提供成员函数用于实现接口。 成员函数 1,matching_index()。返回最后一个submap的索引,用于配准:len-2 2,insertion_indices()。返回最后2个submap的索引,用于点云插入;{len-2,len-1} 3,Get()返回给定索引的子图 4,AddProbabilityGridToResponse() */ class Submaps { public: static constexpr uint8 kUnknownLogOdds = 0; Submaps(); virtual ~Submaps(); Submaps(const Submaps&) = delete; Submaps& operator=(const Submaps&) = delete; // Returns the index of the newest initialized Submap which can be // used for scan-to-map matching. // size() - 2; int matching_index() const;//最新的初始化的子图索引，用于scan-to-map-match // Returns the indices of the Submap into which point clouds will // be inserted. // {size() - 2, size() - 1}; std::vector insertion_indices() const;//待插入的子图的索引 // Returns the Submap with the given 'index'. The same 'index' will always // return the same pointer, so that Submaps can be identified by it. virtual const Submap* Get(int index) const = 0;//纯虚函数，按索引返回子图指针。 // Returns the number of Submaps. virtual int size() const = 0; //将对应的子图序列化到proto文件中 // Fills data about the Submap with 'index' into the 'response'. virtual void SubmapToProto(int index, const transform::Rigid3d& global_submap_pose, proto::SubmapQuery::Response* response) const = 0; protected: //将子图对应的概率网格序列化到proto文件中 static void AddProbabilityGridToResponse( const transform::Rigid3d& local_submap_pose, const mapping_2d::ProbabilityGrid& probability_grid, proto::SubmapQuery::Response* response); }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_SUBMAPS_H_ ================================================ FILE: mapping/submaps_test.cc ================================================ #include "cartographer/mapping/submaps.h" #include #include "gtest/gtest.h" namespace cartographer { namespace mapping { namespace { // Converts the given log odds to a probability. This function is known to be // very slow, because expf is incredibly slow. inline float Expit(float log_odds) { //求指数倍 const float exp_log_odds = std::exp(log_odds); return exp_log_odds / (1.f + exp_log_odds); } TEST(SubmapsTest, LogOddsConversions) { EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6); EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6); EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6); //变换再反变换- >不变 } } // namespace } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/trajectory_builder.cc ================================================ #include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping_2d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" namespace cartographer { namespace mapping { //获取trajectory_builder.lua的内容 proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::TrajectoryBuilderOptions options; *options.mutable_trajectory_builder_2d_options() = mapping_2d::CreateLocalTrajectoryBuilderOptions( parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); *options.mutable_trajectory_builder_3d_options() = mapping_3d::CreateLocalTrajectoryBuilderOptions( parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); return options; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/trajectory_builder.h ================================================ #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_ #include #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/sensor/data.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping { //trajectory_builder.lua proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary); /* TrajectoryBuilder:虚基类,提供多个抽象接口 作用：根据轨迹Builder收集data。 成员函数有： 虚函数： 1),submaps() 2),AddSensorData() 3),pose_estimate() 非虚函数： 1),AddRangefinderData() 2),AddImuData() 3),AddOdometerData() 有一个内部类PoseEstimate,标识当前关键帧的Pose，包括估计位姿的时间,位置,点云 */ // This interface is used for both 2D and 3D SLAM. class TrajectoryBuilder { public: /* PoseEstimate代表一个已经估计好的位姿, pose表示从start点看去的视觉定位, point_cloud表示局部帧的点云 */ // Represents a newly computed pose. 'pose' is the end-user visualization of // orientation and 'point_cloud' is the point cloud, in the local map frame. struct PoseEstimate { PoseEstimate() = default; PoseEstimate(common::Time time, const transform::Rigid3d& pose, const sensor::PointCloud& point_cloud) : time(time), pose(pose), point_cloud(point_cloud) {} common::Time time = common::Time::min(); //测量时间 transform::Rigid3d pose = transform::Rigid3d::Identity();//世界坐标转换 sensor::PointCloud point_cloud;//子图local map frame的点云 }; TrajectoryBuilder() {} virtual ~TrajectoryBuilder() {} TrajectoryBuilder(const TrajectoryBuilder&) = delete; TrajectoryBuilder& operator=(const TrajectoryBuilder&) = delete; virtual const Submaps* submaps() const = 0;//一系列子图 virtual const PoseEstimate& pose_estimate() const = 0;//子图位姿及其采集的点云 /* 根据sensor_id添加data，虚函数。 */ virtual void AddSensorData(const string& sensor_id, std::unique_ptr data) = 0; /* 下面3个函数都是非虚函数。 分别是添加雷达/imu/里程计的data。 参数： 1),sensor_id,标识传感器。 2),time 测量时间 3),PointCloud/Vector3d /Rigid/Rigid3d 测量得到的数据 */ void AddRangefinderData(const string& sensor_id, common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { AddSensorData(sensor_id, common::make_unique( time, sensor::Data::Rangefinder{origin, ranges})); } void AddImuData(const string& sensor_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { AddSensorData(sensor_id, common::make_unique( time, sensor::Data::Imu{linear_acceleration, angular_velocity})); } void AddOdometerData(const string& sensor_id, common::Time time, const transform::Rigid3d& odometer_pose) { AddSensorData(sensor_id, common::make_unique(time, odometer_pose)); } }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping/trajectory_connectivity.cc ================================================ #include "cartographer/mapping/trajectory_connectivity.h" #include #include #include "cartographer/mapping/proto/trajectory_connectivity.pb.h" #include "glog/logging.h" namespace cartographer { namespace mapping { TrajectoryConnectivity::TrajectoryConnectivity() : lock_(), forest_(), connection_map_() {} void TrajectoryConnectivity::Add(const int trajectory_id) { common::MutexLocker locker(&lock_); forest_.emplace(trajectory_id, trajectory_id); } void TrajectoryConnectivity::Connect(const int trajectory_id_a, const int trajectory_id_b) { common::MutexLocker locker(&lock_); Union(trajectory_id_a, trajectory_id_b); auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); ++connection_map_[sorted_pair]; } void TrajectoryConnectivity::Union(const int trajectory_id_a, const int trajectory_id_b) { forest_.emplace(trajectory_id_a, trajectory_id_a); forest_.emplace(trajectory_id_b, trajectory_id_b); const int representative_a = FindSet(trajectory_id_a); const int representative_b = FindSet(trajectory_id_b); forest_[representative_a] = representative_b; } int TrajectoryConnectivity::FindSet(const int trajectory_id) { auto it = forest_.find(trajectory_id); CHECK(it != forest_.end()); if (it->first != it->second) { it->second = FindSet(it->second); } return it->second; } bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a, const int trajectory_id_b) { common::MutexLocker locker(&lock_); if (forest_.count(trajectory_id_a) == 0 || forest_.count(trajectory_id_b) == 0) { return false; } return FindSet(trajectory_id_a) == FindSet(trajectory_id_b); } std::vector> TrajectoryConnectivity::ConnectedComponents() { // Map from cluster exemplar -> growing cluster. std::unordered_map> map; common::MutexLocker locker(&lock_); for (const auto& trajectory_id_entry : forest_) { map[FindSet(trajectory_id_entry.first)].push_back( trajectory_id_entry.first); } std::vector> result; result.reserve(map.size()); for (auto& pair : map) { result.emplace_back(std::move(pair.second)); } return result; } int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a, const int trajectory_id_b) { common::MutexLocker locker(&lock_); const auto it = connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); return it != connection_map_.end() ? it->second : 0; } proto::TrajectoryConnectivity ToProto( std::vector> connected_components) { proto::TrajectoryConnectivity proto; for (auto& connected_component : connected_components) { std::sort(connected_component.begin(), connected_component.end()); } std::sort(connected_components.begin(), connected_components.end()); for (const auto& connected_component : connected_components) { auto* proto_connected_component = proto.add_connected_component(); for (const int trajectory_id : connected_component) { proto_connected_component->add_trajectory_id(trajectory_id); } } return proto; } proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent( const proto::TrajectoryConnectivity& trajectory_connectivity, const int trajectory_id) { for (const auto& connected_component : trajectory_connectivity.connected_component()) { if (std::find(connected_component.trajectory_id().begin(), connected_component.trajectory_id().end(), trajectory_id) != connected_component.trajectory_id().end()) { return connected_component; } } proto::TrajectoryConnectivity::ConnectedComponent connected_component; connected_component.add_trajectory_id(trajectory_id); return connected_component; } } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/trajectory_connectivity.h ================================================ #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_ #define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_ #include #include #include "cartographer/common/mutex.h" #include "cartographer/mapping/proto/trajectory_connectivity.pb.h" #include "cartographer/mapping/submaps.h" namespace cartographer { namespace mapping { /* TrajectoryConnectivity用于解决不同轨迹线的连通性问题. 多条轨迹构成一颗森林，而相互联通的轨迹应该合并。 不可拷贝/赋值 包含3个数据成员 1,互斥锁lock_ 2,forest_ 不同轨迹线组成的森林 3,connection_map_ 连通图 成员函数 Add():添加一条轨迹线 Connect():将2条轨迹线联通 TransitivelyConnected():判断是否处于同一个连通域 ConnectionCount():返回直接联通的数量 the transitive connectivity:传递连通性 ConnectedComponents():由联通分量id组成的已联通分类组 */ // A class that tracks the connectivity structure between trajectories. // // Connectivity includes both the count ("How many times have I _directly_ // connected trajectories i and j?") and the transitive connectivity. // // This class is thread-safe. class TrajectoryConnectivity { public: TrajectoryConnectivity(); TrajectoryConnectivity(const TrajectoryConnectivity&) = delete; TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete; // Add a trajectory which is initially connected to nothing. //添加一条轨迹线，默认不连接到任何轨迹线 void Add(int trajectory_id) EXCLUDES(lock_); /* Connect two trajectories. If either trajectory is untracked, it will be tracked. This function is invariant to the order of its arguments. Repeated calls to Connect increment the connectivity count. 将轨迹a和轨迹b联通 */ void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); // Determines if two trajectories have been (transitively) connected. If // either trajectory is not being tracked, returns false. This function is // invariant to the order of its arguments. //:判断是否处于同一个连通域 bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); // Return the number of _direct_ connections between 'trajectory_id_a' and // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0. // This function is invariant to the order of its arguments. //返回直接联通的数量 int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); // The trajectory IDs, grouped by connectivity. std::vector> ConnectedComponents() EXCLUDES(lock_); private: // Find the representative and compresses the path to it. int FindSet(int trajectory_id) REQUIRES(lock_); void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_); common::Mutex lock_; //互斥锁 // Tracks transitive connectivity using a disjoint set forest, i.e. each // entry points towards the representative for the given trajectory. // 不同轨迹线组成的森林，即连通域问题。 std::map forest_ GUARDED_BY(lock_); // Tracks the number of direct connections between a pair of trajectories. //直接链接的轨迹线 std::map, int> connection_map_ GUARDED_BY(lock_); }; // Returns a proto encoding connected components.编码已联通成分到proto文件 proto::TrajectoryConnectivity ToProto( std::vector> connected_components); // Returns the connected component containing 'trajectory_index'. //返回连接到联通id的所有联通分量。 proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent( const cartographer::mapping::proto::TrajectoryConnectivity& trajectory_connectivity, int trajectory_id); } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_H_ ================================================ FILE: mapping/trajectory_connectivity_test.cc ================================================ #include "cartographer/mapping/trajectory_connectivity.h" #include #include #include #include "gtest/gtest.h" namespace cartographer { namespace mapping { namespace { constexpr int kNumTrajectories = 10;//轨迹数 TEST(TrajectoryConnectivityTest, TransitivelyConnected) { TrajectoryConnectivity trajectory_connectivity; // Make sure nothing's connected until we connect some things. for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) { for (int trajectory_b = 0; trajectory_b < kNumTrajectories; ++trajectory_b) { //默认不连通 EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a, trajectory_b)); } } // Connect some stuff up. trajectory_connectivity.Connect(0, 1); //联通0-1 EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1)); //true trajectory_connectivity.Connect(8, 9); //联通8-9 EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9)); //true EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9));//联通0-9 trajectory_connectivity.Connect(1, 8); //联通1-8 for (int i : {0, 1}) { for (int j : {8, 9}) { EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j));//均联通 } } } TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) { TrajectoryConnectivity trajectory_connectivity; auto connections = trajectory_connectivity.ConnectedComponents(); EXPECT_EQ(0, connections.size()); //0分量 } TEST(TrajectoryConnectivityTest, ConnectedComponents) { TrajectoryConnectivity trajectory_connectivity; for (int i = 0; i <= 4; ++i) { trajectory_connectivity.Connect(0, i); //联通0-1,0-2,0-3,0-4 } for (int i = 5; i <= 9; ++i) { trajectory_connectivity.Connect(5, i);//联通5-6,5-7,5-8,5-9 } auto connections = trajectory_connectivity.ConnectedComponents(); ASSERT_EQ(2, connections.size()); //2个分量/分组 //connections的顺序未定 ，所以所以需要使用find函数。 // The clustering is arbitrary; we need to figure out which one is which. const std::vector* zero_cluster = nullptr; const std::vector* five_cluster = nullptr; if (std::find(connections[0].begin(), connections[0].end(), 0) != connections[0].end()) { zero_cluster = &connections[0]; five_cluster = &connections[1]; } else { zero_cluster = &connections[1]; five_cluster = &connections[0]; } for (int i = 0; i <= 9; ++i) { //查找联通分量 EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(), i) != zero_cluster->end()); EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) != five_cluster->end()); } } TEST(TrajectoryConnectivityTest, ConnectionCount) { TrajectoryConnectivity trajectory_connectivity; //对同一个连线调用多次"Connect()"函数 for (int i = 0; i < kNumTrajectories; ++i) { trajectory_connectivity.Connect(0, 1); // Permute the arguments to check invariance. EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0)); } for (int i = 1; i < 9; ++i) { EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1)); } } } // namespace } // namespace mapping } // namespace cartographer ================================================ FILE: mapping/trajectory_node.h ================================================ #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ #define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ #include #include #include "Eigen/Core" #include "cartographer/common/time.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping { class Submaps; /* 轨迹节点TrajectoryNode类，含有一个内部类ConstantData。 TrajectoryNode作用：在连续的轨迹上采样一些离散的点用于key frame，标识pose frame。 数据成员： 1), ConstantData* 2) 位姿，pose ConstantData类： 数据成员： 1，time ：时间 2，range_data_2d和range_data_3d：测量数据 3,trajectory_id:本节点所属的轨迹 4，Rigid3d ：tracking frame 到 pose frame的矩阵变换。 */ struct TrajectoryNode { struct ConstantData { common::Time time; // Range data in 'pose' frame. Only used in the 2D case. sensor::RangeData range_data_2d;//测量得到的2D range数据 // Range data in 'pose' frame. Only used in the 3D case. sensor::CompressedRangeData range_data_3d;//测量得到的3D range数据 // Trajectory this node belongs to. int trajectory_id; //节点所属id // Transform from the 3D 'tracking' frame to the 'pose' frame of the range // data, which contains roll, pitch and height for 2D. In 3D this is always // identity. transform::Rigid3d tracking_to_pose; //涉及的3D变换 }; common::Time time() const { return constant_data->time; } const ConstantData* constant_data; //常指针.指向某块内存,该内存块的数据不变，指针本身可以变。 transform::Rigid3d pose; }; } // namespace mapping } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ ================================================ FILE: mapping_2d/global_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/global_trajectory_builder.h" namespace cartographer { namespace mapping_2d { GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options, const int trajectory_id, SparsePoseGraph* sparse_pose_graph) : trajectory_id_(trajectory_id), sparse_pose_graph_(sparse_pose_graph), local_trajectory_builder_(options) {} GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} const Submaps* GlobalTrajectoryBuilder::submaps() const { return local_trajectory_builder_.submaps(); } void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { std::unique_ptr insertion_result = local_trajectory_builder_.AddHorizontalRangeData( time, sensor::RangeData{origin, ranges, {}}); if (insertion_result != nullptr) { sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->tracking_to_tracking_2d, insertion_result->range_data_in_tracking_2d, insertion_result->pose_estimate_2d, trajectory_id_, insertion_result->matching_submap, insertion_result->insertion_submaps); } } void GlobalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_.AddImuData(time, linear_acceleration, angular_velocity); sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration, angular_velocity); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, const transform::Rigid3d& pose) { local_trajectory_builder_.AddOdometerData(time, pose); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_.pose_estimate(); } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/global_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_2d/local_trajectory_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph.h" namespace cartographer { namespace mapping_2d { class GlobalTrajectoryBuilder : public mapping::GlobalTrajectoryBuilderInterface { public: GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, int trajectory_id, SparsePoseGraph* sparse_pose_graph); ~GlobalTrajectoryBuilder() override; GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; const Submaps* submaps() const override; const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const override; // Projects 'ranges' into 2D. Therefore, 'ranges' should be approximately // parallel to the ground plane. void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; private: const int trajectory_id_; SparsePoseGraph* const sparse_pose_graph_; LocalTrajectoryBuilder local_trajectory_builder_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_GLOBAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_2d/local_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/local_trajectory_builder.h" #include #include "cartographer/common/make_unique.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_2d { LocalTrajectoryBuilder::LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), submaps_(options.submaps_options()), motion_filter_(options_.motion_filter_options()), real_time_correlative_scan_matcher_( options_.real_time_correlative_scan_matcher_options()), ceres_scan_matcher_(options_.ceres_scan_matcher_options()), odometry_state_tracker_(options_.num_odometry_states()) {} LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {} const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; } sensor::RangeData LocalTrajectoryBuilder::TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const { // Drop any returns below the minimum range and convert returns beyond the // maximum range into misses. sensor::RangeData returns_and_misses{range_data.origin, {}, {}}; for (const Eigen::Vector3f& hit : range_data.returns) { const float range = (hit - range_data.origin).norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { returns_and_misses.returns.push_back(hit); } else { returns_and_misses.misses.push_back( range_data.origin + options_.missing_data_ray_length() * (hit - range_data.origin).normalized()); } } } const sensor::RangeData cropped = sensor::CropRangeData( sensor::TransformRangeData(returns_and_misses, tracking_to_tracking_2d), options_.min_z(), options_.max_z()); return sensor::RangeData{ cropped.origin, sensor::VoxelFiltered(cropped.returns, options_.voxel_filter_size()), sensor::VoxelFiltered(cropped.misses, options_.voxel_filter_size())}; } void LocalTrajectoryBuilder::ScanMatch( common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, transform::Rigid3d* pose_observation) { const ProbabilityGrid& probability_grid = submaps_.Get(submaps_.matching_index())->probability_grid; transform::Rigid2d pose_prediction_2d = transform::Project2D(pose_prediction * tracking_to_tracking_2d.inverse()); // The online correlative scan matcher will refine the initial estimate for // the Ceres scan matcher. transform::Rigid2d initial_ceres_pose = pose_prediction_2d; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.adaptive_voxel_filter_options()); const sensor::PointCloud filtered_point_cloud_in_tracking_2d = adaptive_voxel_filter.Filter(range_data_in_tracking_2d.returns); if (options_.use_online_correlative_scan_matching()) { real_time_correlative_scan_matcher_.Match( pose_prediction_2d, filtered_point_cloud_in_tracking_2d, probability_grid, &initial_ceres_pose); } transform::Rigid2d tracking_2d_to_map; ceres::Solver::Summary summary; ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose, filtered_point_cloud_in_tracking_2d, probability_grid, &tracking_2d_to_map, &summary); *pose_observation = transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d; } std::unique_ptr LocalTrajectoryBuilder::AddHorizontalRangeData( const common::Time time, const sensor::RangeData& range_data) { // Initialize IMU tracker now if we do not ever use an IMU. if (!options_.use_imu_data()) { InitializeImuTracker(time); } if (imu_tracker_ == nullptr) { // Until we've initialized the IMU tracker with our first IMU message, we // cannot compute the orientation of the rangefinder. LOG(INFO) << "ImuTracker not yet initialized."; return nullptr; } Predict(time); const transform::Rigid3d odometry_prediction = pose_estimate_ * odometry_correction_; const transform::Rigid3d model_prediction = pose_estimate_; // TODO(whess): Prefer IMU over odom orientation if configured? const transform::Rigid3d& pose_prediction = odometry_prediction; // Computes the rotation without yaw, as defined by GetYaw(). const transform::Rigid3d tracking_to_tracking_2d = transform::Rigid3d::Rotation( Eigen::Quaterniond(Eigen::AngleAxisd( -transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) * pose_prediction.rotation()); const sensor::RangeData range_data_in_tracking_2d = TransformAndFilterRangeData(tracking_to_tracking_2d.cast(), range_data); if (range_data_in_tracking_2d.returns.empty()) { LOG(WARNING) << "Dropped empty horizontal range data."; return nullptr; } ScanMatch(time, pose_prediction, tracking_to_tracking_2d, range_data_in_tracking_2d, &pose_estimate_); odometry_correction_ = transform::Rigid3d::Identity(); if (!odometry_state_tracker_.empty()) { // We add an odometry state, so that the correction from the scan matching // is not removed by the next odometry data we get. odometry_state_tracker_.AddOdometryState( {time, odometry_state_tracker_.newest().odometer_pose, odometry_state_tracker_.newest().state_pose * odometry_prediction.inverse() * pose_estimate_}); } // Improve the velocity estimate. if (last_scan_match_time_ > common::Time::min() && time > last_scan_match_time_) { const double delta_t = common::ToSeconds(time - last_scan_match_time_); velocity_estimate_ += (pose_estimate_.translation().head<2>() - model_prediction.translation().head<2>()) / delta_t; } last_scan_match_time_ = time_; // Remove the untracked z-component which floats around 0 in the UKF. const auto translation = pose_estimate_.translation(); pose_estimate_ = transform::Rigid3d( transform::Rigid3d::Vector(translation.x(), translation.y(), 0.), pose_estimate_.rotation()); const transform::Rigid3d tracking_2d_to_map = pose_estimate_ * tracking_to_tracking_2d.inverse(); last_pose_estimate_ = { time, pose_estimate_, sensor::TransformPointCloud(range_data_in_tracking_2d.returns, tracking_2d_to_map.cast())}; const transform::Rigid2d pose_estimate_2d = transform::Project2D(tracking_2d_to_map); if (motion_filter_.IsSimilar(time, transform::Embed3D(pose_estimate_2d))) { return nullptr; } const mapping::Submap* const matching_submap = submaps_.Get(submaps_.matching_index()); std::vector insertion_submaps; for (int insertion_index : submaps_.insertion_indices()) { insertion_submaps.push_back(submaps_.Get(insertion_index)); } submaps_.InsertRangeData( TransformRangeData(range_data_in_tracking_2d, transform::Embed3D(pose_estimate_2d.cast()))); return common::make_unique(InsertionResult{ time, matching_submap, insertion_submaps, tracking_to_tracking_2d, range_data_in_tracking_2d, pose_estimate_2d}); } const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& LocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } void LocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added."; InitializeImuTracker(time); Predict(time); imu_tracker_->AddImuLinearAccelerationObservation(linear_acceleration); imu_tracker_->AddImuAngularVelocityObservation(angular_velocity); } void LocalTrajectoryBuilder::AddOdometerData( const common::Time time, const transform::Rigid3d& odometer_pose) { if (imu_tracker_ == nullptr) { // Until we've initialized the IMU tracker we do not want to call Predict(). LOG(INFO) << "ImuTracker not yet initialized."; return; } Predict(time); if (!odometry_state_tracker_.empty()) { const auto& previous_odometry_state = odometry_state_tracker_.newest(); const transform::Rigid3d delta = previous_odometry_state.odometer_pose.inverse() * odometer_pose; const transform::Rigid3d new_pose = previous_odometry_state.state_pose * delta; odometry_correction_ = pose_estimate_.inverse() * new_pose; } odometry_state_tracker_.AddOdometryState( {time, odometer_pose, pose_estimate_ * odometry_correction_}); } void LocalTrajectoryBuilder::InitializeImuTracker(const common::Time time) { if (imu_tracker_ == nullptr) { imu_tracker_ = common::make_unique( options_.imu_gravity_time_constant(), time); } } void LocalTrajectoryBuilder::Predict(const common::Time time) { CHECK(imu_tracker_ != nullptr); CHECK_LE(time_, time); const double last_yaw = transform::GetYaw(imu_tracker_->orientation()); imu_tracker_->Advance(time); if (time_ > common::Time::min()) { const double delta_t = common::ToSeconds(time - time_); // Constant velocity model. const Eigen::Vector3d translation = pose_estimate_.translation() + delta_t * Eigen::Vector3d(velocity_estimate_.x(), velocity_estimate_.y(), 0.); // Use the current IMU tracker roll and pitch for gravity alignment, and // apply its change in yaw. const Eigen::Quaterniond rotation = Eigen::AngleAxisd( transform::GetYaw(pose_estimate_.rotation()) - last_yaw, Eigen::Vector3d::UnitZ()) * imu_tracker_->orientation(); pose_estimate_ = transform::Rigid3d(translation, rotation); } time_ = time; } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/local_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_ #include #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/imu_tracker.h" #include "cartographer/mapping/odometry_state_tracker.h" #include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/sensor/configuration.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_2d { // Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop // closure. class LocalTrajectoryBuilder { public: struct InsertionResult { common::Time time; const mapping::Submap* matching_submap; std::vector insertion_submaps; transform::Rigid3d tracking_to_tracking_2d; sensor::RangeData range_data_in_tracking_2d; transform::Rigid2d pose_estimate_2d; }; explicit LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); ~LocalTrajectoryBuilder(); LocalTrajectoryBuilder(const LocalTrajectoryBuilder&) = delete; LocalTrajectoryBuilder& operator=(const LocalTrajectoryBuilder&) = delete; const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate& pose_estimate() const; std::unique_ptr AddHorizontalRangeData( common::Time, const sensor::RangeData& range_data); void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void AddOdometerData(common::Time time, const transform::Rigid3d& pose); const Submaps* submaps() const; private: sensor::RangeData TransformAndFilterRangeData( const transform::Rigid3f& tracking_to_tracking_2d, const sensor::RangeData& range_data) const; // Scan matches 'range_data_in_tracking_2d' and fill in the 'pose_observation' // with the result. void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction, const transform::Rigid3d& tracking_to_tracking_2d, const sensor::RangeData& range_data_in_tracking_2d, transform::Rigid3d* pose_observation); // Lazily constructs an ImuTracker. void InitializeImuTracker(common::Time time); // Updates the current estimate to reflect the given 'time'. void Predict(common::Time time); const proto::LocalTrajectoryBuilderOptions options_; Submaps submaps_; mapping::GlobalTrajectoryBuilderInterface::PoseEstimate last_pose_estimate_; // Current 'pose_estimate_' and 'velocity_estimate_' at 'time_'. common::Time time_ = common::Time::min(); transform::Rigid3d pose_estimate_ = transform::Rigid3d::Identity(); Eigen::Vector2d velocity_estimate_ = Eigen::Vector2d::Zero(); common::Time last_scan_match_time_ = common::Time::min(); // This is the difference between the model (constant velocity, IMU) // prediction 'pose_estimate_' and the odometry prediction. To get the // odometry prediction, right-multiply this to 'pose_estimate_'. transform::Rigid3d odometry_correction_ = transform::Rigid3d::Identity(); mapping_3d::MotionFilter motion_filter_; scan_matching::RealTimeCorrelativeScanMatcher real_time_correlative_scan_matcher_; scan_matching::CeresScanMatcher ceres_scan_matcher_; std::unique_ptr imu_tracker_; mapping::OdometryStateTracker odometry_state_tracker_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_2d/local_trajectory_builder_options.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/local_trajectory_builder_options.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping_2d { proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::LocalTrajectoryBuilderOptions options; options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_min_z(parameter_dictionary->GetDouble("min_z")); options.set_max_z(parameter_dictionary->GetDouble("max_z")); options.set_missing_data_ray_length( parameter_dictionary->GetDouble("missing_data_ray_length")); options.set_voxel_filter_size( parameter_dictionary->GetDouble("voxel_filter_size")); options.set_use_online_correlative_scan_matching( parameter_dictionary->GetBool("use_online_correlative_scan_matching")); *options.mutable_adaptive_voxel_filter_options() = sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); *options.mutable_real_time_correlative_scan_matcher_options() = scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_ceres_scan_matcher_options() = scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = mapping_3d::CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); options.set_imu_gravity_time_constant( parameter_dictionary->GetDouble("imu_gravity_time_constant")); options.set_num_odometry_states( parameter_dictionary->GetNonNegativeInt("num_odometry_states")); CHECK_GT(options.num_odometry_states(), 0); *options.mutable_submaps_options() = CreateSubmapsOptions( parameter_dictionary->GetDictionary("submaps").get()); options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); return options; } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/local_trajectory_builder_options.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_2d { proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ ================================================ FILE: mapping_2d/map_limits.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ #define CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/proto/map_limits.pb.h" #include "cartographer/mapping_2d/xy_index.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { // Defines the limits of a grid map. This class must remain inlined for // performance reasons. class MapLimits { public: MapLimits(const double resolution, const Eigen::Vector2d& max, const CellLimits& cell_limits) : resolution_(resolution), max_(max), cell_limits_(cell_limits) { CHECK_GT(resolution_, 0.); CHECK_GT(cell_limits.num_x_cells, 0.); CHECK_GT(cell_limits.num_y_cells, 0.); } explicit MapLimits(const proto::MapLimits& map_limits) : resolution_(map_limits.resolution()), max_(transform::ToEigen(map_limits.max())), cell_limits_(map_limits.cell_limits()) {} // Returns the cell size in meters. All cells are square and the resolution is // the length of one side. double resolution() const { return resolution_; } // Returns the corner of the limits, i.e., all pixels have positions with // smaller coordinates. const Eigen::Vector2d& max() const { return max_; } // Returns the limits of the grid in number of cells. const CellLimits& cell_limits() const { return cell_limits_; } // Returns the index of the cell containing the point ('x', 'y') which may be // outside the map, i.e., negative or too large indices that will return // false for Contains(). Eigen::Array2i GetXYIndexOfCellContainingPoint(const double x, const double y) const { // Index values are row major and the top left has Eigen::Array2i::Zero() // and contains (centered_max_x, centered_max_y). We need to flip and // rotate. return Eigen::Array2i( common::RoundToInt((max_.y() - y) / resolution_ - 0.5), common::RoundToInt((max_.x() - x) / resolution_ - 0.5)); } // Returns true of the ProbabilityGrid contains 'xy_index'. bool Contains(const Eigen::Array2i& xy_index) const { return (Eigen::Array2i(0, 0) <= xy_index).all() && (xy_index < Eigen::Array2i(cell_limits_.num_x_cells, cell_limits_.num_y_cells)) .all(); } private: double resolution_; Eigen::Vector2d max_; CellLimits cell_limits_; }; inline proto::MapLimits ToProto(const MapLimits& map_limits) { proto::MapLimits result; result.set_resolution(map_limits.resolution()); *result.mutable_max() = transform::ToProto(map_limits.max()); *result.mutable_cell_limits() = ToProto(map_limits.cell_limits()); return result; } } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_MAP_LIMITS_H_ ================================================ FILE: mapping_2d/map_limits_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/map_limits.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace { TEST(MapLimitsTest, ToProto) { const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); const auto proto = ToProto(limits); EXPECT_EQ(limits.resolution(), proto.resolution()); EXPECT_EQ(limits.max().x(), proto.max().x()); EXPECT_EQ(limits.max().y(), proto.max().y()); EXPECT_EQ(ToProto(limits.cell_limits()).DebugString(), proto.cell_limits().DebugString()); } TEST(MapLimitsTest, ProtoConstructor) { proto::MapLimits limits; limits.set_resolution(1.); limits.mutable_max()->set_x(2.); limits.mutable_max()->set_y(3.); limits.mutable_cell_limits()->set_num_x_cells(4); limits.mutable_cell_limits()->set_num_y_cells(5); const MapLimits native(limits); EXPECT_EQ(limits.resolution(), native.resolution()); EXPECT_EQ(limits.max().x(), native.max().x()); EXPECT_EQ(limits.max().y(), native.max().y()); EXPECT_EQ(limits.cell_limits().DebugString(), ToProto(native.cell_limits()).DebugString()); } TEST(MapLimitsTest, ConstructAndGet) { const MapLimits limits(42., Eigen::Vector2d(3., 0.), CellLimits(2, 3)); const CellLimits& cell_limits = limits.cell_limits(); EXPECT_EQ(2, cell_limits.num_x_cells); EXPECT_EQ(3, cell_limits.num_y_cells); const Eigen::Vector2d& max = limits.max(); EXPECT_EQ(3., max.x()); EXPECT_EQ(0., max.y()); EXPECT_EQ(42., limits.resolution()); } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/probability_grid.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ #define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ #include #include #include #include #include #include #include #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/proto/probability_grid.pb.h" #include "cartographer/mapping_2d/xy_index.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { // Represents a 2D grid of probabilities. class ProbabilityGrid { public: explicit ProbabilityGrid(const MapLimits& limits) : limits_(limits), cells_(limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells, mapping::kUnknownProbabilityValue), max_x_(0), max_y_(0), min_x_(limits_.cell_limits().num_x_cells - 1), min_y_(limits_.cell_limits().num_y_cells - 1) {} explicit ProbabilityGrid(const proto::ProbabilityGrid& proto) : limits_(proto.limits()), cells_(), update_indices_(proto.update_indices().begin(), proto.update_indices().end()), max_x_(proto.max_x()), max_y_(proto.max_y()), min_x_(proto.min_x()), min_y_(proto.min_y()) { cells_.reserve(proto.cells_size()); for (const auto cell : proto.cells()) { CHECK_LE(cell, std::numeric_limits::max()); cells_.push_back(cell); } } // Returns the limits of this ProbabilityGrid. const MapLimits& limits() const { return limits_; } // Starts the next update sequence. void StartUpdate() { while (!update_indices_.empty()) { DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker); cells_[update_indices_.back()] -= mapping::kUpdateMarker; update_indices_.pop_back(); } } // Sets the probability of the cell at 'xy_index' to the given 'probability'. // Only allowed if the cell was unknown before. void SetProbability(const Eigen::Array2i& xy_index, const float probability) { uint16& cell = cells_[GetIndexOfCell(xy_index)]; CHECK_EQ(cell, mapping::kUnknownProbabilityValue); cell = mapping::ProbabilityToValue(probability); UpdateBounds(xy_index); } // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'xy_index' if the cell has not already // been updated. Multiple updates of the same cell will be ignored until // StartUpdate() is called. Returns true if the cell was updated. // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. bool ApplyLookupTable(const Eigen::Array2i& xy_index, const std::vector& table) { DCHECK_EQ(table.size(), mapping::kUpdateMarker); const int cell_index = GetIndexOfCell(xy_index); uint16& cell = cells_[cell_index]; if (cell >= mapping::kUpdateMarker) { return false; } update_indices_.push_back(cell_index); cell = table[cell]; DCHECK_GE(cell, mapping::kUpdateMarker); UpdateBounds(xy_index); return true; } // Returns the probability of the cell with 'xy_index'. float GetProbability(const Eigen::Array2i& xy_index) const { if (limits_.Contains(xy_index)) { return mapping::ValueToProbability(cells_[GetIndexOfCell(xy_index)]); } return mapping::kMinProbability; } // Returns the probability of the cell containing the point ('x', 'y'). float GetProbability(const double x, const double y) const { return GetProbability(limits_.GetXYIndexOfCellContainingPoint(x, y)); } // Returns true if the probability at the specified index is known. bool IsKnown(const Eigen::Array2i& xy_index) const { return limits_.Contains(xy_index) && cells_[GetIndexOfCell(xy_index)] != mapping::kUnknownProbabilityValue; } // Fills in 'offset' and 'limits' to define a subregion of that contains all // known cells. void ComputeCroppedLimits(Eigen::Array2i* const offset, CellLimits* const limits) const { *offset = Eigen::Array2i(min_x_, min_y_); *limits = CellLimits(std::max(max_x_, min_x_) - min_x_ + 1, std::max(max_y_, min_y_) - min_y_ + 1); } // Grows the map as necessary to include 'x' and 'y'. This changes the meaning // of these coordinates going forward. This method must be called immediately // after 'StartUpdate', before any calls to 'ApplyLookupTable'. void GrowLimits(const double x, const double y) { CHECK(update_indices_.empty()); while (!limits_.Contains(limits_.GetXYIndexOfCellContainingPoint(x, y))) { const int x_offset = limits_.cell_limits().num_x_cells / 2; const int y_offset = limits_.cell_limits().num_y_cells / 2; const MapLimits new_limits( limits_.resolution(), limits_.max() + limits_.resolution() * Eigen::Vector2d(y_offset, x_offset), CellLimits(2 * limits_.cell_limits().num_x_cells, 2 * limits_.cell_limits().num_y_cells)); const int stride = new_limits.cell_limits().num_x_cells; const int offset = x_offset + stride * y_offset; const int new_size = new_limits.cell_limits().num_x_cells * new_limits.cell_limits().num_y_cells; std::vector new_cells(new_size, mapping::kUnknownProbabilityValue); for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) { for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) { new_cells[offset + j + i * stride] = cells_[j + i * limits_.cell_limits().num_x_cells]; } } cells_ = new_cells; limits_ = new_limits; min_x_ += x_offset; min_y_ += y_offset; max_x_ += x_offset; max_y_ += y_offset; } } proto::ProbabilityGrid ToProto() const { proto::ProbabilityGrid result; *result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_); result.mutable_cells()->Reserve(cells_.size()); for (const auto cell : cells_) { result.mutable_cells()->Add(cell); } result.mutable_update_indices()->Reserve(update_indices_.size()); for (const auto update : update_indices_) { result.mutable_update_indices()->Add(update); } result.set_max_x(max_x_); result.set_max_y(max_y_); result.set_min_x(min_x_); result.set_min_y(min_y_); return result; } private: // Returns the index of the cell at 'xy_index'. int GetIndexOfCell(const Eigen::Array2i& xy_index) const { CHECK(limits_.Contains(xy_index)) << xy_index; return limits_.cell_limits().num_x_cells * xy_index.y() + xy_index.x(); } void UpdateBounds(const Eigen::Array2i& xy_index) { min_x_ = std::min(min_x_, xy_index.x()); min_y_ = std::min(min_y_, xy_index.y()); max_x_ = std::max(max_x_, xy_index.x()); max_y_ = std::max(max_y_, xy_index.y()); } MapLimits limits_; std::vector cells_; // Highest bit is update marker. std::vector update_indices_; // Minimum and maximum cell coordinates of known cells to efficiently compute // cropping limits. int max_x_; int max_y_; int min_x_; int min_y_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_ ================================================ FILE: mapping_2d/probability_grid_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/probability_grid.h" #include #include #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace { TEST(ProbabilityGridTest, ProtoConstructor) { proto::ProbabilityGrid proto; const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.)); *proto.mutable_limits() = ToProto(limits); for (int i = 6; i < 12; ++i) { proto.mutable_cells()->Add(static_cast(i)); } for (int i = 13; i < 18; ++i) { proto.mutable_update_indices()->Add(i); } proto.set_max_x(19); proto.set_max_y(20); proto.set_min_x(21); proto.set_min_y(22); ProbabilityGrid grid(proto); EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString()); // TODO(macmason): Figure out how to test the contents of cells_, // update_indices_, and {min, max}_{x, y}_ gracefully. } TEST(ProbabilityGridTest, ToProto) { ProbabilityGrid probability_grid( MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); const auto proto = probability_grid.ToProto(); EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(), proto.limits().DebugString()); // TODO(macmason): Figure out how to test the contents of cells_, // update_indices_, and {min, max}_{x, y}_ gracefully. } TEST(ProbabilityGridTest, ApplyOdds) { ProbabilityGrid probability_grid( MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2))); const MapLimits& limits = probability_grid.limits(); EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0))); EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1))); EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0))); EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1))); EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0))); EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1))); EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0))); EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1))); probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5); probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 0), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5); probability_grid.StartUpdate(); probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5); probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(0, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1))); EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5); // Tests adding odds to an unknown cell. probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42))); EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, 1e-4); // Tests that further updates are ignored if StartUpdate() isn't called. probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42, 1e-4); probability_grid.StartUpdate(); probability_grid.ApplyLookupTable( Eigen::Array2i(1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9))); EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42); } TEST(ProbabilityGridTest, GetProbability) { ProbabilityGrid probability_grid( MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2))); const MapLimits& limits = probability_grid.limits(); EXPECT_EQ(1., limits.max().x()); EXPECT_EQ(2., limits.max().y()); const CellLimits& cell_limits = limits.cell_limits(); ASSERT_EQ(2, cell_limits.num_x_cells); ASSERT_EQ(2, cell_limits.num_y_cells); probability_grid.StartUpdate(); probability_grid.SetProbability( limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5), mapping::kMaxProbability); EXPECT_NEAR(probability_grid.GetProbability(-0.5, 0.5), mapping::kMaxProbability, 1e-6); for (const Eigen::Array2i& xy_index : {limits.GetXYIndexOfCellContainingPoint(-0.5, 1.5), limits.GetXYIndexOfCellContainingPoint(0.5, 0.5), limits.GetXYIndexOfCellContainingPoint(0.5, 1.5)}) { EXPECT_TRUE(limits.Contains(xy_index)); EXPECT_FALSE(probability_grid.IsKnown(xy_index)); } } TEST(ProbabilityGridTest, GetXYIndexOfCellContainingPoint) { ProbabilityGrid probability_grid( MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8))); const MapLimits& limits = probability_grid.limits(); const CellLimits& cell_limits = limits.cell_limits(); ASSERT_EQ(14, cell_limits.num_x_cells); ASSERT_EQ(8, cell_limits.num_y_cells); EXPECT_TRUE( (Eigen::Array2i(0, 0) == limits.GetXYIndexOfCellContainingPoint(7, 13)) .all()); EXPECT_TRUE( (Eigen::Array2i(13, 0) == limits.GetXYIndexOfCellContainingPoint(7, -13)) .all()); EXPECT_TRUE( (Eigen::Array2i(0, 7) == limits.GetXYIndexOfCellContainingPoint(-7, 13)) .all()); EXPECT_TRUE( (Eigen::Array2i(13, 7) == limits.GetXYIndexOfCellContainingPoint(-7, -13)) .all()); // Check around the origin. EXPECT_TRUE( (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, 0.5)) .all()); EXPECT_TRUE( (Eigen::Array2i(6, 3) == limits.GetXYIndexOfCellContainingPoint(1.5, 1.5)) .all()); EXPECT_TRUE((Eigen::Array2i(7, 3) == limits.GetXYIndexOfCellContainingPoint(0.5, -0.5)) .all()); EXPECT_TRUE((Eigen::Array2i(6, 4) == limits.GetXYIndexOfCellContainingPoint(-0.5, 0.5)) .all()); EXPECT_TRUE((Eigen::Array2i(7, 4) == limits.GetXYIndexOfCellContainingPoint(-0.5, -0.5)) .all()); } TEST(ProbabilityGridTest, CorrectCropping) { // Create a probability grid with random values. std::mt19937 rng(42); std::uniform_real_distribution value_distribution( mapping::kMinProbability, mapping::kMaxProbability); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator( Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) { probability_grid.SetProbability(xy_index, value_distribution(rng)); } Eigen::Array2i offset; CellLimits limits; probability_grid.ComputeCroppedLimits(&offset, &limits); EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all()); EXPECT_EQ(limits.num_x_cells, 200); EXPECT_EQ(limits.num_y_cells, 200); } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/proto/cell_limits.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.proto; message CellLimits { optional int32 num_x_cells = 1; optional int32 num_y_cells = 2; } ================================================ FILE: mapping_2d/proto/local_trajectory_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.proto; import "cartographer/mapping_3d/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_2d/proto/submaps_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; message LocalTrajectoryBuilderOptions { // Rangefinder points outside these ranges will be dropped. optional float min_range = 14; optional float max_range = 15; optional float min_z = 1; optional float max_z = 2; // Points beyond 'max_range' will be inserted with this length as empty space. optional float missing_data_ray_length = 16; // Voxel filter that gets applied to the range data immediately after // cropping. optional float voxel_filter_size = 3; // Whether to solve the online scan matching first using the correlative scan // matcher to generate a good starting point for Ceres. optional bool use_online_correlative_scan_matching = 5; // Voxel filter used to compute a sparser point cloud for matching. optional sensor.proto.AdaptiveVoxelFilterOptions adaptive_voxel_filter_options = 6; optional scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 7; optional scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 8; optional mapping_3d.proto.MotionFilterOptions motion_filter_options = 13; // Time constant in seconds for the orientation moving average based on // observed gravity via the IMU. It should be chosen so that the error // 1. from acceleration measurements not due to gravity (which gets worse when // the constant is reduced) and // 2. from integration of angular velocities (which gets worse when the // constant is increased) is balanced. optional double imu_gravity_time_constant = 17; // Maximum number of previous odometry states to keep. optional int32 num_odometry_states = 18; optional mapping_2d.proto.SubmapsOptions submaps_options = 11; // True if IMU data should be expected and used. optional bool use_imu_data = 12; } ================================================ FILE: mapping_2d/proto/map_limits.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_2d/proto/cell_limits.proto"; import "cartographer/transform/proto/transform.proto"; package cartographer.mapping_2d.proto; message MapLimits { optional double resolution = 1; optional cartographer.transform.proto.Vector2d max = 2; optional CellLimits cell_limits = 3; } ================================================ FILE: mapping_2d/proto/probability_grid.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_2d/proto/map_limits.proto"; package cartographer.mapping_2d.proto; message ProbabilityGrid { optional MapLimits limits = 1; // These values are actually int16s, but protos don't have a native int16 // type. repeated int32 cells = 2; repeated int32 update_indices = 3; optional int32 max_x = 4; optional int32 max_y = 5; optional int32 min_x = 6; optional int32 min_y = 7; } ================================================ FILE: mapping_2d/proto/range_data_inserter_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.proto; message RangeDataInserterOptions { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). optional double hit_probability = 1; // Probability change for a miss (this will be converted to odds and therefore // must be less than 0.5). optional double miss_probability = 2; // If 'false', free space will not change the probabilities in the occupancy // grid. optional bool insert_free_space = 3; } ================================================ FILE: mapping_2d/proto/submaps_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_2d/proto/range_data_inserter_options.proto"; package cartographer.mapping_2d.proto; message SubmapsOptions { // Resolution of the map in meters. optional double resolution = 1; // Half the width/height of each submap, its "radius". optional double half_length = 2; // Number of scans before adding a new submap. Each submap will get twice the // number of scans inserted: First for initialization without being matched // against, then while being matched. optional int32 num_range_data = 3; // If enabled, submap%d.png images are written for debugging. optional bool output_debug_images = 4; optional RangeDataInserterOptions range_data_inserter_options = 5; } ================================================ FILE: mapping_2d/range_data_inserter.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/range_data_inserter.h" #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/mapping_2d/ray_casting.h" #include "cartographer/mapping_2d/xy_index.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::RangeDataInserterOptions options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( parameter_dictionary->GetDouble("miss_probability")); options.set_insert_free_space( parameter_dictionary->HasKey("insert_free_space") ? parameter_dictionary->GetBool("insert_free_space") : true); CHECK_GT(options.hit_probability(), 0.5); CHECK_LT(options.miss_probability(), 0.5); return options; } RangeDataInserter::RangeDataInserter( const proto::RangeDataInserterOptions& options) : options_(options), hit_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options.hit_probability()))), miss_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options.miss_probability()))) {} void RangeDataInserter::Insert(const sensor::RangeData& range_data, ProbabilityGrid* const probability_grid) const { CHECK_NOTNULL(probability_grid)->StartUpdate(); // By not starting a new update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). CastRays(range_data, probability_grid->limits(), [this, &probability_grid](const Eigen::Array2i& hit) { probability_grid->ApplyLookupTable(hit, hit_table_); }, [this, &probability_grid](const Eigen::Array2i& miss) { if (options_.insert_free_space()) { probability_grid->ApplyLookupTable(miss, miss_table_); } }); } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/range_data_inserter.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ #define CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/proto/range_data_inserter_options.pb.h" #include "cartographer/mapping_2d/xy_index.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_2d { proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary); class RangeDataInserter { public: explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); RangeDataInserter(const RangeDataInserter&) = delete; RangeDataInserter& operator=(const RangeDataInserter&) = delete; // Inserts 'range_data' into 'probability_grid'. void Insert(const sensor::RangeData& range_data, ProbabilityGrid* probability_grid) const; const std::vector& hit_table() const { return hit_table_; } const std::vector& miss_table() const { return miss_table_; } private: const proto::RangeDataInserterOptions options_; const std::vector hit_table_; const std::vector miss_table_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_RANGE_DATA_INSERTER_H_ ================================================ FILE: mapping_2d/range_data_inserter_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/range_data_inserter.h" #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping_2d/probability_grid.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_2d { namespace { class RangeDataInserterTest : public ::testing::Test { protected: RangeDataInserterTest() : probability_grid_( MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) { auto parameter_dictionary = common::MakeDictionary( "return { " "insert_free_space = true, " "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); range_data_inserter_ = common::make_unique(options_); } void InsertPointCloud() { sensor::RangeData range_data; range_data.returns.emplace_back(-3.5, 0.5, 0.f); range_data.returns.emplace_back(-2.5, 1.5, 0.f); range_data.returns.emplace_back(-1.5, 2.5, 0.f); range_data.returns.emplace_back(-0.5, 3.5, 0.f); range_data.origin.x() = -0.5; range_data.origin.y() = 0.5; probability_grid_.StartUpdate(); range_data_inserter_->Insert(range_data, &probability_grid_); } ProbabilityGrid probability_grid_; std::unique_ptr range_data_inserter_; proto::RangeDataInserterOptions options_; }; TEST_F(RangeDataInserterTest, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9); EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9); const CellLimits& cell_limits = probability_grid_.limits().cell_limits(); EXPECT_EQ(5, cell_limits.num_x_cells); EXPECT_EQ(5, cell_limits.num_y_cells); enum class State { UNKNOWN, MISS, HIT }; State expected_states[5][5] = { {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN}, {State::UNKNOWN, State::HIT, State::MISS, State::MISS, State::MISS}, {State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS, State::MISS}, {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS}, {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::HIT}}; for (int row = 0; row != 5; ++row) { for (int column = 0; column != 5; ++column) { Eigen::Array2i xy_index(row, column); EXPECT_TRUE(probability_grid_.limits().Contains(xy_index)); switch (expected_states[column][row]) { case State::UNKNOWN: EXPECT_FALSE(probability_grid_.IsKnown(xy_index)); break; case State::MISS: EXPECT_NEAR(options_.miss_probability(), probability_grid_.GetProbability(xy_index), 1e-4); break; case State::HIT: EXPECT_NEAR(options_.hit_probability(), probability_grid_.GetProbability(xy_index), 1e-4); break; } } } } TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options_.hit_probability(), probability_grid_.GetProbability(-3.5, 0.5), 1e-4); EXPECT_NEAR(options_.miss_probability(), probability_grid_.GetProbability(-2.5, 0.5), 1e-4); for (int i = 0; i < 1000; ++i) { InsertPointCloud(); } EXPECT_NEAR(mapping::kMaxProbability, probability_grid_.GetProbability(-3.5, 0.5), 1e-3); EXPECT_NEAR(mapping::kMinProbability, probability_grid_.GetProbability(-2.5, 0.5), 1e-3); } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/ray_casting.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/ray_casting.h" namespace cartographer { namespace mapping_2d { namespace { // Factor for subpixel accuracy of start and end point. constexpr int kSubpixelScale = 1000; // We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin' // and 'end' are coordinates at subpixel precision. We compute all pixels in // which some part of the line segment connecting 'begin' and 'end' lies. void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end, const std::function& visitor) { // For simplicity, we order 'begin' and 'end' by their x coordinate. if (begin.x() > end.x()) { CastRay(end, begin, visitor); return; } CHECK_GE(begin.x(), 0); CHECK_GE(begin.y(), 0); CHECK_GE(end.y(), 0); // Special case: We have to draw a vertical line in full pixels, as 'begin' // and 'end' have the same full pixel x coordinate. if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) { Eigen::Array2i current(begin.x() / kSubpixelScale, std::min(begin.y(), end.y()) / kSubpixelScale); const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale; for (; current.y() <= end_y; ++current.y()) { visitor(current); } return; } const int64 dx = end.x() - begin.x(); const int64 dy = end.y() - begin.y(); const int64 denominator = 2 * kSubpixelScale * dx; // The current full pixel coordinates. We begin at 'begin'. Eigen::Array2i current = begin / kSubpixelScale; // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in // the denominator. // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale) // | | | | // +-+-+-+ // | | | | // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale) // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale) // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale) // The center of the subpixel part of 'begin.y()' assuming the // 'denominator', i.e., sub_y / denominator is in (0, 1). int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx; // The distance from the from 'begin' to the right pixel border, to be divided // by 2 * 'kSubpixelScale'. const int first_pixel = 2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1; // The same from the left pixel border to 'end'. const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1; // The full pixel x coordinate of 'end'. const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale; // Move from 'begin' to the next pixel border to the right. sub_y += dy * first_pixel; if (dy > 0) { while (true) { visitor(current); while (sub_y > denominator) { sub_y -= denominator; ++current.y(); visitor(current); } ++current.x(); if (sub_y == denominator) { sub_y -= denominator; ++current.y(); } if (current.x() == end_x) { break; } // Move from one pixel border to the next. sub_y += dy * 2 * kSubpixelScale; } // Move from the pixel border on the right to 'end'. sub_y += dy * last_pixel; visitor(current); while (sub_y > denominator) { sub_y -= denominator; ++current.y(); visitor(current); } CHECK_NE(sub_y, denominator); CHECK_EQ(current.y(), end.y() / kSubpixelScale); return; } // Same for lines non-ascending in y coordinates. while (true) { visitor(current); while (sub_y < 0) { sub_y += denominator; --current.y(); visitor(current); } ++current.x(); if (sub_y == 0) { sub_y += denominator; --current.y(); } if (current.x() == end_x) { break; } sub_y += dy * 2 * kSubpixelScale; } sub_y += dy * last_pixel; visitor(current); while (sub_y < 0) { sub_y += denominator; --current.y(); visitor(current); } CHECK_NE(sub_y, 0); CHECK_EQ(current.y(), end.y() / kSubpixelScale); } } // namespace void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor) { const double superscaled_resolution = limits.resolution() / kSubpixelScale; const MapLimits superscaled_limits( superscaled_resolution, limits.max(), CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale, limits.cell_limits().num_y_cells * kSubpixelScale)); const Eigen::Array2i begin = superscaled_limits.GetXYIndexOfCellContainingPoint(range_data.origin.x(), range_data.origin.y()); // Compute and add the end points. std::vector ends; ends.reserve(range_data.returns.size()); for (const Eigen::Vector3f& hit : range_data.returns) { ends.push_back( superscaled_limits.GetXYIndexOfCellContainingPoint(hit.x(), hit.y())); hit_visitor(ends.back() / kSubpixelScale); } // Now add the misses. for (const Eigen::Array2i& end : ends) { CastRay(begin, end, miss_visitor); } // Finally, compute and add empty rays based on misses in the scan. for (const Eigen::Vector3f& missing_echo : range_data.misses) { CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint( missing_echo.x(), missing_echo.y()), miss_visitor); } } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/ray_casting.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ #define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ #include #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/xy_index.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_2d { // For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the // appropriate cells. Hits are handled before misses. void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, const std::function& hit_visitor, const std::function& miss_visitor); } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ ================================================ FILE: mapping_2d/scan_matching/ceres_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include #include #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h" #include "cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::CeresScanMatcherOptions options; options.set_occupied_space_weight( parameter_dictionary->GetDouble("occupied_space_weight")); options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); return options; } CeresScanMatcher::CeresScanMatcher( const proto::CeresScanMatcherOptions& options) : options_(options), ceres_solver_options_( common::CreateCeresSolverOptions(options.ceres_solver_options())) { ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; } CeresScanMatcher::~CeresScanMatcher() {} void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* const pose_estimate, ceres::Solver::Summary* const summary) const { double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y(), initial_pose_estimate.rotation().angle()}; ceres::Problem problem; CHECK_GT(options_.occupied_space_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new OccupiedSpaceCostFunctor( options_.occupied_space_weight() / std::sqrt(static_cast(point_cloud.size())), point_cloud, probability_grid), point_cloud.size()), nullptr, ceres_pose_estimate); CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationDeltaCostFunctor(options_.translation_weight(), previous_pose)), nullptr, ceres_pose_estimate); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RotationDeltaCostFunctor(options_.rotation_weight(), ceres_pose_estimate[2])), nullptr, ceres_pose_estimate); ceres::Solve(ceres_solver_options_, &problem, summary); *pose_estimate = transform::Rigid2d( {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]); } } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/ceres_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ #include #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); // Align scans with an existing map using Ceres. class CeresScanMatcher { public: explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); virtual ~CeresScanMatcher(); CeresScanMatcher(const CeresScanMatcher&) = delete; CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver // 'summary'. void Match(const transform::Rigid2d& previous_pose, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate, ceres::Solver::Summary* summary) const; private: const proto::CeresScanMatcherOptions options_; ceres::Solver::Options ceres_solver_options_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ ================================================ FILE: mapping_2d/scan_matching/ceres_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { namespace { class CeresScanMatcherTest : public ::testing::Test { protected: CeresScanMatcherTest() : probability_grid_( MapLimits(1., Eigen::Vector2d(10., 10.), CellLimits(20, 20))) { probability_grid_.SetProbability( probability_grid_.limits().GetXYIndexOfCellContainingPoint(-3.5, 2.5), mapping::kMaxProbability); point_cloud_.emplace_back(-3.f, 2.f, 0.f); auto parameter_dictionary = common::MakeDictionary(R"text( return { occupied_space_weight = 1., translation_weight = 0.1, rotation_weight = 1.5, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, num_threads = 1, }, })text"); const proto::CeresScanMatcherOptions options = CreateCeresScanMatcherOptions(parameter_dictionary.get()); ceres_scan_matcher_ = common::make_unique(options); } void TestFromInitialPose(const transform::Rigid2d& initial_pose) { transform::Rigid2d pose; const transform::Rigid2d expected_pose = transform::Rigid2d::Translation({-0.5, 0.5}); ceres::Solver::Summary summary; ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_, probability_grid_, &pose, &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2)) << "Actual: " << transform::ToProto(pose).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } ProbabilityGrid probability_grid_; sensor::PointCloud point_cloud_; std::unique_ptr ceres_scan_matcher_; }; TEST_F(CeresScanMatcherTest, testPerfectEstimate) { TestFromInitialPose(transform::Rigid2d::Translation({-0.5, 0.5})); } TEST_F(CeresScanMatcherTest, testOptimizeAlongX) { TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.5})); } TEST_F(CeresScanMatcherTest, testOptimizeAlongY) { TestFromInitialPose(transform::Rigid2d::Translation({-0.45, 0.3})); } TEST_F(CeresScanMatcherTest, testOptimizeAlongXY) { TestFromInitialPose(transform::Rigid2d::Translation({-0.3, 0.3})); } } // namespace } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/correlative_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include #include "cartographer/common/math.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { SearchParameters::SearchParameters(const double linear_search_window, const double angular_search_window, const sensor::PointCloud& point_cloud, const double resolution) : resolution(resolution) { // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution; for (const Eigen::Vector3f& point : point_cloud) { const float range = point.head<2>().norm(); max_scan_range = std::max(range, max_scan_range); } const double kSafetyMargin = 1. - 1e-3; angular_perturbation_step_size = kSafetyMargin * std::acos(1. - common::Pow2(resolution) / (2. * common::Pow2(max_scan_range))); num_angular_perturbations = std::ceil(angular_search_window / angular_perturbation_step_size); num_scans = 2 * num_angular_perturbations + 1; const int num_linear_perturbations = std::ceil(linear_search_window / resolution); linear_bounds.reserve(num_scans); for (int i = 0; i != num_scans; ++i) { linear_bounds.push_back( LinearBounds{-num_linear_perturbations, num_linear_perturbations, -num_linear_perturbations, num_linear_perturbations}); } } SearchParameters::SearchParameters(const int num_linear_perturbations, const int num_angular_perturbations, const double angular_perturbation_step_size, const double resolution) : num_angular_perturbations(num_angular_perturbations), angular_perturbation_step_size(angular_perturbation_step_size), resolution(resolution), num_scans(2 * num_angular_perturbations + 1) { linear_bounds.reserve(num_scans); for (int i = 0; i != num_scans; ++i) { linear_bounds.push_back( LinearBounds{-num_linear_perturbations, num_linear_perturbations, -num_linear_perturbations, num_linear_perturbations}); } } void SearchParameters::ShrinkToFit(const std::vector& scans, const CellLimits& cell_limits) { CHECK_EQ(scans.size(), num_scans); CHECK_EQ(linear_bounds.size(), num_scans); for (int i = 0; i != num_scans; ++i) { Eigen::Array2i min_bound = Eigen::Array2i::Zero(); Eigen::Array2i max_bound = Eigen::Array2i::Zero(); for (const Eigen::Array2i& xy_index : scans[i]) { min_bound = min_bound.min(-xy_index); max_bound = max_bound.max(Eigen::Array2i(cell_limits.num_x_cells - 1, cell_limits.num_y_cells - 1) - xy_index); } linear_bounds[i].min_x = std::max(linear_bounds[i].min_x, min_bound.x()); linear_bounds[i].max_x = std::min(linear_bounds[i].max_x, max_bound.x()); linear_bounds[i].min_y = std::max(linear_bounds[i].min_y, min_bound.y()); linear_bounds[i].max_y = std::min(linear_bounds[i].max_y, max_bound.y()); } } std::vector GenerateRotatedScans( const sensor::PointCloud& point_cloud, const SearchParameters& search_parameters) { std::vector rotated_scans; rotated_scans.reserve(search_parameters.num_scans); double delta_theta = -search_parameters.num_angular_perturbations * search_parameters.angular_perturbation_step_size; for (int scan_index = 0; scan_index < search_parameters.num_scans; ++scan_index, delta_theta += search_parameters.angular_perturbation_step_size) { rotated_scans.push_back(sensor::TransformPointCloud( point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf( delta_theta, Eigen::Vector3f::UnitZ())))); } return rotated_scans; } std::vector DiscretizeScans( const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation) { std::vector discrete_scans; discrete_scans.reserve(scans.size()); for (const sensor::PointCloud& scan : scans) { discrete_scans.emplace_back(); discrete_scans.back().reserve(scan.size()); for (const Eigen::Vector3f& point : scan) { const Eigen::Vector2f translated_point = Eigen::Affine2f(initial_translation) * point.head<2>(); discrete_scans.back().push_back( map_limits.GetXYIndexOfCellContainingPoint(translated_point.x(), translated_point.y())); } } return discrete_scans; } } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/correlative_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/xy_index.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { typedef std::vector DiscreteScan; // Describes the search space. struct SearchParameters { // Linear search window in pixel offsets; bounds are inclusive. struct LinearBounds { int min_x; int max_x; int min_y; int max_y; }; SearchParameters(double linear_search_window, double angular_search_window, const sensor::PointCloud& point_cloud, double resolution); // For testing. SearchParameters(int num_linear_perturbations, int num_angular_perturbations, double angular_perturbation_step_size, double resolution); // Tightens the search window as much as possible. void ShrinkToFit(const std::vector& scans, const CellLimits& cell_limits); int num_angular_perturbations; double angular_perturbation_step_size; double resolution; int num_scans; std::vector linear_bounds; // Per rotated scans. }; // Generates a collection of rotated scans. std::vector GenerateRotatedScans( const sensor::PointCloud& point_cloud, const SearchParameters& search_parameters); // Translates and discretizes the rotated scans into a vector of integer // indices. std::vector DiscretizeScans( const MapLimits& map_limits, const std::vector& scans, const Eigen::Translation2f& initial_translation); // A possible solution. struct Candidate { Candidate(const int init_scan_index, const int init_x_index_offset, const int init_y_index_offset, const SearchParameters& search_parameters) : scan_index(init_scan_index), x_index_offset(init_x_index_offset), y_index_offset(init_y_index_offset), x(-y_index_offset * search_parameters.resolution), y(-x_index_offset * search_parameters.resolution), orientation((scan_index - search_parameters.num_angular_perturbations) * search_parameters.angular_perturbation_step_size) {} // Index into the rotated scans vector. int scan_index = 0; // Linear offset from the initial pose. int x_index_offset = 0; int y_index_offset = 0; // Pose of this Candidate relative to the initial pose. double x = 0.; double y = 0.; double orientation = 0.; // Score, higher is better. float score = 0.f; bool operator<(const Candidate& other) const { return score < other.score; } bool operator>(const Candidate& other) const { return score > other.score; } }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_CORRELATIVE_SCAN_MATCHER_H_ ================================================ FILE: mapping_2d/scan_matching/correlative_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { namespace { TEST(SearchParameters, Construction) { const SearchParameters search_parameters(4, 5, 0.03, 0.05); EXPECT_EQ(5, search_parameters.num_angular_perturbations); EXPECT_NEAR(0.03, search_parameters.angular_perturbation_step_size, 1e-9); EXPECT_NEAR(0.05, search_parameters.resolution, 1e-9); EXPECT_EQ(11, search_parameters.num_scans); EXPECT_EQ(11, search_parameters.linear_bounds.size()); for (const SearchParameters::LinearBounds linear_bounds : search_parameters.linear_bounds) { EXPECT_EQ(-4, linear_bounds.min_x); EXPECT_EQ(4, linear_bounds.max_x); EXPECT_EQ(-4, linear_bounds.min_y); EXPECT_EQ(4, linear_bounds.max_y); } } TEST(Candidate, Construction) { const SearchParameters search_parameters(4, 5, 0.03, 0.05); const Candidate candidate(3, 4, -5, search_parameters); EXPECT_EQ(3, candidate.scan_index); EXPECT_EQ(4, candidate.x_index_offset); EXPECT_EQ(-5, candidate.y_index_offset); EXPECT_NEAR(0.25, candidate.x, 1e-9); EXPECT_NEAR(-0.2, candidate.y, 1e-9); EXPECT_NEAR(-0.06, candidate.orientation, 1e-9); EXPECT_NEAR(0., candidate.score, 1e-9); Candidate bigger_candidate(3, 4, 5, search_parameters); bigger_candidate.score = 1.; EXPECT_LT(candidate, bigger_candidate); } TEST(GenerateRotatedScans, GenerateRotatedScans) { sensor::PointCloud point_cloud; point_cloud.emplace_back(-1.f, 1.f, 0.f); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.)); EXPECT_EQ(3, scans.size()); EXPECT_NEAR(1., scans[0][0].x(), 1e-6); EXPECT_NEAR(1., scans[0][0].y(), 1e-6); EXPECT_NEAR(-1., scans[1][0].x(), 1e-6); EXPECT_NEAR(1., scans[1][0].y(), 1e-6); EXPECT_NEAR(-1., scans[2][0].x(), 1e-6); EXPECT_NEAR(-1., scans[2][0].y(), 1e-6); } TEST(DiscretizeScans, DiscretizeScans) { sensor::PointCloud point_cloud; point_cloud.emplace_back(0.025f, 0.175f, 0.f); point_cloud.emplace_back(-0.025f, 0.175f, 0.f); point_cloud.emplace_back(-0.075f, 0.175f, 0.f); point_cloud.emplace_back(-0.125f, 0.175f, 0.f); point_cloud.emplace_back(-0.125f, 0.125f, 0.f); point_cloud.emplace_back(-0.125f, 0.075f, 0.f); point_cloud.emplace_back(-0.125f, 0.025f, 0.f); const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6)); const std::vector scans = GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity()); EXPECT_EQ(1, discrete_scans.size()); EXPECT_EQ(7, discrete_scans[0].size()); EXPECT_TRUE((Eigen::Array2i(1, 0) == discrete_scans[0][0]).all()); EXPECT_TRUE((Eigen::Array2i(1, 1) == discrete_scans[0][1]).all()); EXPECT_TRUE((Eigen::Array2i(1, 2) == discrete_scans[0][2]).all()); EXPECT_TRUE((Eigen::Array2i(1, 3) == discrete_scans[0][3]).all()); EXPECT_TRUE((Eigen::Array2i(2, 3) == discrete_scans[0][4]).all()); EXPECT_TRUE((Eigen::Array2i(3, 3) == discrete_scans[0][5]).all()); EXPECT_TRUE((Eigen::Array2i(4, 3) == discrete_scans[0][6]).all()); } } // namespace } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include #include #include #include #include #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { namespace { // A collection of values which can be added and later removed, and the maximum // of the current values in the collection can be retrieved. // All of it in (amortized) O(1). class SlidingWindowMaximum { public: void AddValue(const float value) { while (!non_ascending_maxima_.empty() && value > non_ascending_maxima_.back()) { non_ascending_maxima_.pop_back(); } non_ascending_maxima_.push_back(value); } void RemoveValue(const float value) { // DCHECK for performance, since this is done for every value in the // precomputation grid. DCHECK(!non_ascending_maxima_.empty()); DCHECK_LE(value, non_ascending_maxima_.front()); if (value == non_ascending_maxima_.front()) { non_ascending_maxima_.pop_front(); } } float GetMaximum() const { // DCHECK for performance, since this is done for every value in the // precomputation grid. DCHECK_GT(non_ascending_maxima_.size(), 0); return non_ascending_maxima_.front(); } void CheckIsEmpty() const { CHECK_EQ(non_ascending_maxima_.size(), 0); } private: // Maximum of the current sliding window at the front. Then the maximum of the // remaining window that came after this values first occurence, and so on. std::deque non_ascending_maxima_; }; } // namespace proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::FastCorrelativeScanMatcherOptions options; options.set_linear_search_window( parameter_dictionary->GetDouble("linear_search_window")); options.set_angular_search_window( parameter_dictionary->GetDouble("angular_search_window")); options.set_branch_and_bound_depth( parameter_dictionary->GetInt("branch_and_bound_depth")); return options; } PrecomputationGrid::PrecomputationGrid( const ProbabilityGrid& probability_grid, const CellLimits& limits, const int width, std::vector* reusable_intermediate_grid) : offset_(-width + 1, -width + 1), wide_limits_(limits.num_x_cells + width - 1, limits.num_y_cells + width - 1), cells_(wide_limits_.num_x_cells * wide_limits_.num_y_cells) { CHECK_GE(width, 1); CHECK_GE(limits.num_x_cells, 1); CHECK_GE(limits.num_y_cells, 1); const int stride = wide_limits_.num_x_cells; // First we compute the maximum probability for each (x0, y) achieved in the // span defined by x0 <= x < x0 + width. std::vector& intermediate = *reusable_intermediate_grid; intermediate.resize(wide_limits_.num_x_cells * limits.num_y_cells); for (int y = 0; y != limits.num_y_cells; ++y) { SlidingWindowMaximum current_values; current_values.AddValue( probability_grid.GetProbability(Eigen::Array2i(0, y))); for (int x = -width + 1; x != 0; ++x) { intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); if (x + width < limits.num_x_cells) { current_values.AddValue( probability_grid.GetProbability(Eigen::Array2i(x + width, y))); } } for (int x = 0; x < limits.num_x_cells - width; ++x) { intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); current_values.RemoveValue( probability_grid.GetProbability(Eigen::Array2i(x, y))); current_values.AddValue( probability_grid.GetProbability(Eigen::Array2i(x + width, y))); } for (int x = std::max(limits.num_x_cells - width, 0); x != limits.num_x_cells; ++x) { intermediate[x + width - 1 + y * stride] = current_values.GetMaximum(); current_values.RemoveValue( probability_grid.GetProbability(Eigen::Array2i(x, y))); } current_values.CheckIsEmpty(); } // For each (x, y), we compute the maximum probability in the width x width // region starting at each (x, y) and precompute the resulting bound on the // score. for (int x = 0; x != wide_limits_.num_x_cells; ++x) { SlidingWindowMaximum current_values; current_values.AddValue(intermediate[x]); for (int y = -width + 1; y != 0; ++y) { cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum()); if (y + width < limits.num_y_cells) { current_values.AddValue(intermediate[x + (y + width) * stride]); } } for (int y = 0; y < limits.num_y_cells - width; ++y) { cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum()); current_values.RemoveValue(intermediate[x + y * stride]); current_values.AddValue(intermediate[x + (y + width) * stride]); } for (int y = std::max(limits.num_y_cells - width, 0); y != limits.num_y_cells; ++y) { cells_[x + (y + width - 1) * stride] = ComputeCellValue(current_values.GetMaximum()); current_values.RemoveValue(intermediate[x + y * stride]); } current_values.CheckIsEmpty(); } } uint8 PrecomputationGrid::ComputeCellValue(const float probability) const { const int cell_value = common::RoundToInt( (probability - mapping::kMinProbability) * (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); CHECK_GE(cell_value, 0); CHECK_LE(cell_value, 255); return cell_value; } class PrecomputationGridStack { public: PrecomputationGridStack( const ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options) { CHECK_GE(options.branch_and_bound_depth(), 1); const int max_width = 1 << (options.branch_and_bound_depth() - 1); precomputation_grids_.reserve(options.branch_and_bound_depth()); std::vector reusable_intermediate_grid; const CellLimits limits = probability_grid.limits().cell_limits(); reusable_intermediate_grid.reserve((limits.num_x_cells + max_width - 1) * limits.num_y_cells); for (int i = 0; i != options.branch_and_bound_depth(); ++i) { const int width = 1 << i; precomputation_grids_.emplace_back(probability_grid, limits, width, &reusable_intermediate_grid); } } const PrecomputationGrid& Get(int index) { return precomputation_grids_[index]; } int max_depth() const { return precomputation_grids_.size() - 1; } private: std::vector precomputation_grids_; }; FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( const ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options) : options_(options), limits_(probability_grid.limits()), precomputation_grid_stack_( new PrecomputationGridStack(probability_grid, options)) {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const float min_score, float* score, transform::Rigid2d* pose_estimate) const { const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(), point_cloud, limits_.resolution()); return MatchWithSearchParameters(search_parameters, initial_pose_estimate, point_cloud, min_score, score, pose_estimate); } bool FastCorrelativeScanMatcher::MatchFullSubmap( const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const { // Compute a search window around the center of the submap that includes it // fully. const SearchParameters search_parameters( 1e6 * limits_.resolution(), // Linear search window, 1e6 cells/direction. M_PI, // Angular search window, 180 degrees in both directions. point_cloud, limits_.resolution()); const transform::Rigid2d center = transform::Rigid2d::Translation( limits_.max() - 0.5 * limits_.resolution() * Eigen::Vector2d(limits_.cell_limits().num_y_cells, limits_.cell_limits().num_x_cells)); return MatchWithSearchParameters(search_parameters, center, point_cloud, min_score, score, pose_estimate); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf( initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); const std::vector discrete_scans = DiscretizeScans( limits_, rotated_scans, Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y())); search_parameters.ShrinkToFit(discrete_scans, limits_.cell_limits()); const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(discrete_scans, search_parameters); const Candidate best_candidate = BranchAndBound( discrete_scans, search_parameters, lowest_resolution_candidates, precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { *score = best_candidate.score; *pose_estimate = transform::Rigid2d( {initial_pose_estimate.translation().x() + best_candidate.x, initial_pose_estimate.translation().y() + best_candidate.y}, initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); return true; } return false; } std::vector FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( const std::vector& discrete_scans, const SearchParameters& search_parameters) const { std::vector lowest_resolution_candidates = GenerateLowestResolutionCandidates(search_parameters); ScoreCandidates( precomputation_grid_stack_->Get(precomputation_grid_stack_->max_depth()), discrete_scans, search_parameters, &lowest_resolution_candidates); return lowest_resolution_candidates; } std::vector FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( const SearchParameters& search_parameters) const { const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); int num_candidates = 0; for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { const int num_lowest_resolution_linear_x_candidates = (search_parameters.linear_bounds[scan_index].max_x - search_parameters.linear_bounds[scan_index].min_x + linear_step_size) / linear_step_size; const int num_lowest_resolution_linear_y_candidates = (search_parameters.linear_bounds[scan_index].max_y - search_parameters.linear_bounds[scan_index].min_y + linear_step_size) / linear_step_size; num_candidates += num_lowest_resolution_linear_x_candidates * num_lowest_resolution_linear_y_candidates; } std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; x_index_offset += linear_step_size) { for (int y_index_offset = search_parameters.linear_bounds[scan_index].min_y; y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; y_index_offset += linear_step_size) { candidates.emplace_back(scan_index, x_index_offset, y_index_offset, search_parameters); } } } CHECK_EQ(candidates.size(), num_candidates); return candidates; } void FastCorrelativeScanMatcher::ScoreCandidates( const PrecomputationGrid& precomputation_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const { for (Candidate& candidate : *candidates) { int sum = 0; for (const Eigen::Array2i& xy_index : discrete_scans[candidate.scan_index]) { const Eigen::Array2i proposed_xy_index( xy_index.x() + candidate.x_index_offset, xy_index.y() + candidate.y_index_offset); sum += precomputation_grid.GetValue(proposed_xy_index); } candidate.score = PrecomputationGrid::ToProbability( sum / static_cast(discrete_scans[candidate.scan_index].size())); } std::sort(candidates->begin(), candidates->end(), std::greater()); } Candidate FastCorrelativeScanMatcher::BranchAndBound( const std::vector& discrete_scans, const SearchParameters& search_parameters, const std::vector& candidates, const int candidate_depth, float min_score) const { if (candidate_depth == 0) { // Return the best candidate. return *candidates.begin(); } Candidate best_high_resolution_candidate(0, 0, 0, search_parameters); best_high_resolution_candidate.score = min_score; for (const Candidate& candidate : candidates) { if (candidate.score <= min_score) { break; } std::vector higher_resolution_candidates; const int half_width = 1 << (candidate_depth - 1); for (int x_offset : {0, half_width}) { if (candidate.x_index_offset + x_offset > search_parameters.linear_bounds[candidate.scan_index].max_x) { break; } for (int y_offset : {0, half_width}) { if (candidate.y_index_offset + y_offset > search_parameters.linear_bounds[candidate.scan_index].max_y) { break; } higher_resolution_candidates.emplace_back( candidate.scan_index, candidate.x_index_offset + x_offset, candidate.y_index_offset + y_offset, search_parameters); } } ScoreCandidates(precomputation_grid_stack_->Get(candidate_depth - 1), discrete_scans, search_parameters, &higher_resolution_candidates); best_high_resolution_candidate = std::max( best_high_resolution_candidate, BranchAndBound(discrete_scans, search_parameters, higher_resolution_candidates, candidate_depth - 1, best_high_resolution_candidate.score)); } return best_high_resolution_candidate; } } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ // This is an implementation of the algorithm described in "Real-Time // Correlative Scan Matching" by Olson. // // It is similar to the RealTimeCorrelativeScanMatcher but has a different // trade-off: Scan matching is faster because more effort is put into the // precomputation done for a given map. However, this map is immutable after // construction. #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ #include #include #include "Eigen/Core" #include "cartographer/common/port.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); // A precomputed grid that contains in each cell (x0, y0) the maximum // probability in the width x width area defined by x0 <= x < x0 + width and // y0 <= y < y0. class PrecomputationGrid { public: PrecomputationGrid(const ProbabilityGrid& probability_grid, const CellLimits& limits, int width, std::vector* reusable_intermediate_grid); // Returns a value between 0 and 255 to represent probabilities between // kMinProbability and kMaxProbability. int GetValue(const Eigen::Array2i& xy_index) const { const Eigen::Array2i local_xy_index = xy_index - offset_; // The static_cast is for performance to check with 2 comparisons // xy_index.x() < offset_.x() || xy_index.y() < offset_.y() || // local_xy_index.x() >= wide_limits_.num_x_cells || // local_xy_index.y() >= wide_limits_.num_y_cells // instead of using 4 comparisons. if (static_cast(local_xy_index.x()) >= static_cast(wide_limits_.num_x_cells) || static_cast(local_xy_index.y()) >= static_cast(wide_limits_.num_y_cells)) { return 0; } const int stride = wide_limits_.num_x_cells; return cells_[local_xy_index.x() + local_xy_index.y() * stride]; } // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. static float ToProbability(float value) { return mapping::kMinProbability + value * ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); } private: uint8 ComputeCellValue(float probability) const; // Offset of the precomputation grid in relation to the 'probability_grid' // including the additional 'width' - 1 cells. const Eigen::Array2i offset_; // Size of the precomputation grid. const CellLimits wide_limits_; // Probabilites mapped to 0 to 255. std::vector cells_; }; class PrecomputationGridStack; // An implementation of "Real-Time Correlative Scan Matching" by Olson. class FastCorrelativeScanMatcher { public: FastCorrelativeScanMatcher( const ProbabilityGrid& probability_grid, const proto::FastCorrelativeScanMatcherOptions& options); ~FastCorrelativeScanMatcher(); FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) // is possible, true is returned, and 'score' and 'pose_estimate' are updated // with the result. bool Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; // Aligns 'point_cloud' within the full 'probability_grid', i.e., not // restricted to the configured search window. If a score above 'min_score' // (excluding equality) is possible, true is returned, and 'score' and // 'pose_estimate' are updated with the result. bool MatchFullSubmap(const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; private: // The actual implementation of the scan matcher, called by Match() and // MatchFullSubmap() with appropriate 'initial_pose_estimate' and // 'search_parameters'. bool MatchWithSearchParameters( SearchParameters search_parameters, const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, float min_score, float* score, transform::Rigid2d* pose_estimate) const; std::vector ComputeLowestResolutionCandidates( const std::vector& discrete_scans, const SearchParameters& search_parameters) const; std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters) const; void ScoreCandidates(const PrecomputationGrid& precomputation_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const; Candidate BranchAndBound(const std::vector& discrete_scans, const SearchParameters& search_parameters, const std::vector& candidates, int candidate_depth, float min_score) const; const proto::FastCorrelativeScanMatcherOptions options_; MapLimits limits_; std::unique_ptr precomputation_grid_stack_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ ================================================ FILE: mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include #include #include #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { namespace { TEST(PrecomputationGridTest, CorrectValues) { // Create a probability grid with random values that can be exactly // represented by uint8 values. std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) { probability_grid.SetProbability( xy_index, PrecomputationGrid::ToProbability(distribution(prng))); } std::vector reusable_intermediate_grid; for (const int width : {1, 2, 3, 8}) { PrecomputationGrid precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { float max_score = -std::numeric_limits::infinity(); for (int y = 0; y != width; ++y) { for (int x = 0; x != width; ++x) { max_score = std::max( max_score, probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); } } EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( precomputation_grid.GetValue(xy_index)), 1e-4); } } } TEST(PrecomputationGridTest, TinyProbabilityGrid) { std::mt19937 prng(42); std::uniform_int_distribution distribution(0, 255); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4))); probability_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { probability_grid.SetProbability( xy_index, PrecomputationGrid::ToProbability(distribution(prng))); } std::vector reusable_intermediate_grid; for (const int width : {1, 2, 3, 8, 200}) { PrecomputationGrid precomputation_grid( probability_grid, probability_grid.limits().cell_limits(), width, &reusable_intermediate_grid); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(probability_grid.limits().cell_limits())) { float max_score = -std::numeric_limits::infinity(); for (int y = 0; y != width; ++y) { for (int x = 0; x != width; ++x) { max_score = std::max( max_score, probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y))); } } EXPECT_NEAR(max_score, PrecomputationGrid::ToProbability( precomputation_grid.GetValue(xy_index)), 1e-4); } } } proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { auto parameter_dictionary = common::MakeDictionary(R"text( return { linear_search_window = 3., angular_search_window = 1., branch_and_bound_depth = )text" + std::to_string(branch_and_bound_depth) + "}"); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } mapping_2d::proto::RangeDataInserterOptions CreateRangeDataInserterTestOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { insert_free_space = true, hit_probability = 0.7, miss_probability = 0.4, })text"); return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(3); sensor::PointCloud point_cloud; point_cloud.emplace_back(-2.5f, 0.5f, 0.f); point_cloud.emplace_back(-2.f, 0.5f, 0.f); point_cloud.emplace_back(0.f, -0.5f, 0.f); point_cloud.emplace_back(0.5f, -1.6f, 0.f); point_cloud.emplace_back(2.5f, 0.5f, 0.f); point_cloud.emplace_back(2.5f, 1.7f, 0.f); for (int i = 0; i != 50; ++i) { const transform::Rigid2f expected_pose( {2. * distribution(prng), 2. * distribution(prng)}, 0.5 * distribution(prng)); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ Eigen::Vector3f(expected_pose.translation().x(), expected_pose.translation().y(), 0.f), sensor::TransformPointCloud( point_cloud, transform::Embed3D(expected_pose.cast())), {}}, &probability_grid); FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); transform::Rigid2d pose_estimate; float score; EXPECT_TRUE(fast_correlative_scan_matcher.Match( transform::Rigid2d::Identity(), point_cloud, kMinScore, &score, &pose_estimate)); EXPECT_LT(kMinScore, score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.03f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } } TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(6); sensor::PointCloud unperturbed_point_cloud; unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f); unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f); unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f); unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f); unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f); unperturbed_point_cloud.emplace_back(2.0f, 1.8f, 0.f); for (int i = 0; i != 20; ++i) { const transform::Rigid2f perturbation( {10. * distribution(prng), 10. * distribution(prng)}, 1.6 * distribution(prng)); const sensor::PointCloud point_cloud = sensor::TransformPointCloud( unperturbed_point_cloud, transform::Embed3D(perturbation)); const transform::Rigid2f expected_pose = transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)}, 0.5 * distribution(prng)) * perturbation.inverse(); ProbabilityGrid probability_grid( MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200))); probability_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ transform::Embed3D(expected_pose * perturbation).translation(), sensor::TransformPointCloud(point_cloud, transform::Embed3D(expected_pose)), {}}, &probability_grid); FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid, options); transform::Rigid2d pose_estimate; float score; EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap( point_cloud, kMinScore, &score, &pose_estimate)); EXPECT_LT(kMinScore, score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.03f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } } } // namespace } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/fast_global_localizer.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { bool PerformGlobalLocalization( const float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, const std::vector< cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& matchers, const cartographer::sensor::PointCloud& point_cloud, transform::Rigid2d* const best_pose_estimate, float* const best_score) { CHECK(best_pose_estimate != nullptr) << "Need a non-null output_pose_estimate!"; CHECK(best_score != nullptr) << "Need a non-null best_score!"; *best_score = cutoff; transform::Rigid2d pose_estimate; const sensor::PointCloud filtered_point_cloud = voxel_filter.Filter(point_cloud); bool success = false; if (matchers.size() == 0) { LOG(WARNING) << "Map not yet large enough to localize in!"; return false; } for (auto& matcher : matchers) { float score = -1; transform::Rigid2d pose_estimate; if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score, &pose_estimate)) { CHECK_GT(score, *best_score) << "MatchFullSubmap lied!"; *best_score = score; *best_pose_estimate = pose_estimate; success = true; } } return success; } } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/fast_global_localizer.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ #include #include "Eigen/Geometry" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { // Perform global localization against the provided 'matchers'. The 'cutoff' // specifies the minimum correlation that will be accepted. // This function does not take ownership of the pointers passed in // 'matchers'; they are passed as a vector of raw pointers to give maximum // flexibility to callers. // // Returns true in the case of successful localization. The output parameters // should not be trusted if the function returns false. The 'cutoff' and // 'best_score' are in the range [0.0, 1.0]. bool PerformGlobalLocalization( float cutoff, const cartographer::sensor::AdaptiveVoxelFilter& voxel_filter, const std::vector< cartographer::mapping_2d::scan_matching::FastCorrelativeScanMatcher*>& matchers, const cartographer::sensor::PointCloud& point_cloud, transform::Rigid2d* best_pose_estimate, float* best_score); } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_FAST_GLOBAL_LOCALIZER_H_ ================================================ FILE: mapping_2d/scan_matching/occupied_space_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "ceres/ceres.h" #include "ceres/cubic_interpolation.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { // Computes the cost of inserting occupied space described by the point cloud // into the map. The cost increases with the amount of free space that would be // replaced by occupied space. class OccupiedSpaceCostFunctor { public: // Creates an OccupiedSpaceCostFunctor using the specified map, resolution // level, and point cloud. OccupiedSpaceCostFunctor(const double scaling_factor, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), probability_grid_(probability_grid) {} OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; template bool operator()(const T* const pose, T* residual) const { Eigen::Matrix translation(pose[0], pose[1]); Eigen::Rotation2D rotation(pose[2]); Eigen::Matrix rotation_matrix = rotation.toRotationMatrix(); Eigen::Matrix transform; transform << rotation_matrix, translation, T(0.), T(0.), T(1.); const GridArrayAdapter adapter(probability_grid_); ceres::BiCubicInterpolator interpolator(adapter); const MapLimits& limits = probability_grid_.limits(); for (size_t i = 0; i < point_cloud_.size(); ++i) { // Note that this is a 2D point. The third component is a scaling factor. const Eigen::Matrix point((T(point_cloud_[i].x())), (T(point_cloud_[i].y())), T(1.)); const Eigen::Matrix world = transform * point; interpolator.Evaluate( (limits.max().x() - world[0]) / limits.resolution() - 0.5 + T(kPadding), (limits.max().y() - world[1]) / limits.resolution() - 0.5 + T(kPadding), &residual[i]); residual[i] = scaling_factor_ * (1. - residual[i]); } return true; } private: static constexpr int kPadding = INT_MAX / 4; class GridArrayAdapter { public: enum { DATA_DIMENSION = 1 }; explicit GridArrayAdapter(const ProbabilityGrid& probability_grid) : probability_grid_(probability_grid) {} void GetValue(const int row, const int column, double* const value) const { if (row < kPadding || column < kPadding || row >= NumRows() - kPadding || column >= NumCols() - kPadding) { *value = mapping::kMinProbability; } else { *value = static_cast(probability_grid_.GetProbability( Eigen::Array2i(column - kPadding, row - kPadding))); } } int NumRows() const { return probability_grid_.limits().cell_limits().num_y_cells + 2 * kPadding; } int NumCols() const { return probability_grid_.limits().cell_limits().num_x_cells + 2 * kPadding; } private: const ProbabilityGrid& probability_grid_; }; const double scaling_factor_; const sensor::PointCloud& point_cloud_; const ProbabilityGrid& probability_grid_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ ================================================ FILE: mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 10 message CeresScanMatcherOptions { // Scaling parameters for each cost functor. optional double occupied_space_weight = 1; optional double translation_weight = 2; optional double rotation_weight = 3; // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ optional common.proto.CeresSolverOptions ceres_solver_options = 9; } ================================================ FILE: mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.scan_matching.proto; message FastCorrelativeScanMatcherOptions { // Minimum linear search window in which the best possible scan alignment // will be found. optional double linear_search_window = 3; // Minimum angular search window in which the best possible scan alignment // will be found. optional double angular_search_window = 4; // Number of precomputed grids to use. optional int32 branch_and_bound_depth = 2; } ================================================ FILE: mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_2d.scan_matching.proto; message RealTimeCorrelativeScanMatcherOptions { // Minimum linear search window in which the best possible scan alignment // will be found. optional double linear_search_window = 1; // Minimum angular search window in which the best possible scan alignment // will be found. optional double angular_search_window = 2; // Weights applied to each part of the score. optional double translation_delta_cost_weight = 3; optional double rotation_delta_cost_weight = 4; } ================================================ FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include #include #include #include #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::RealTimeCorrelativeScanMatcherOptions options; options.set_linear_search_window( parameter_dictionary->GetDouble("linear_search_window")); options.set_angular_search_window( parameter_dictionary->GetDouble("angular_search_window")); options.set_translation_delta_cost_weight( parameter_dictionary->GetDouble("translation_delta_cost_weight")); options.set_rotation_delta_cost_weight( parameter_dictionary->GetDouble("rotation_delta_cost_weight")); CHECK_GE(options.translation_delta_cost_weight(), 0.); CHECK_GE(options.rotation_delta_cost_weight(), 0.); return options; } RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( const proto::RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} std::vector RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchCandidates( const SearchParameters& search_parameters) const { int num_candidates = 0; for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { const int num_linear_x_candidates = (search_parameters.linear_bounds[scan_index].max_x - search_parameters.linear_bounds[scan_index].min_x + 1); const int num_linear_y_candidates = (search_parameters.linear_bounds[scan_index].max_y - search_parameters.linear_bounds[scan_index].min_y + 1); num_candidates += num_linear_x_candidates * num_linear_y_candidates; } std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != search_parameters.num_scans; ++scan_index) { for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x; x_index_offset <= search_parameters.linear_bounds[scan_index].max_x; ++x_index_offset) { for (int y_index_offset = search_parameters.linear_bounds[scan_index].min_y; y_index_offset <= search_parameters.linear_bounds[scan_index].max_y; ++y_index_offset) { candidates.emplace_back(scan_index, x_index_offset, y_index_offset, search_parameters); } } } CHECK_EQ(candidates.size(), num_candidates); return candidates; } double RealTimeCorrelativeScanMatcher::Match( const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation(); const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud( point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf( initial_rotation.cast().angle(), Eigen::Vector3f::UnitZ()))); const SearchParameters search_parameters( options_.linear_search_window(), options_.angular_search_window(), rotated_point_cloud, probability_grid.limits().resolution()); const std::vector rotated_scans = GenerateRotatedScans(rotated_point_cloud, search_parameters); const std::vector discrete_scans = DiscretizeScans( probability_grid.limits(), rotated_scans, Eigen::Translation2f(initial_pose_estimate.translation().x(), initial_pose_estimate.translation().y())); std::vector candidates = GenerateExhaustiveSearchCandidates(search_parameters); ScoreCandidates(probability_grid, discrete_scans, search_parameters, &candidates); const Candidate& best_candidate = *std::max_element(candidates.begin(), candidates.end()); *pose_estimate = transform::Rigid2d( {initial_pose_estimate.translation().x() + best_candidate.x, initial_pose_estimate.translation().y() + best_candidate.y}, initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation)); return best_candidate.score; } void RealTimeCorrelativeScanMatcher::ScoreCandidates( const ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* const candidates) const { for (Candidate& candidate : *candidates) { candidate.score = 0.f; for (const Eigen::Array2i& xy_index : discrete_scans[candidate.scan_index]) { const Eigen::Array2i proposed_xy_index( xy_index.x() + candidate.x_index_offset, xy_index.y() + candidate.y_index_offset); const float probability = probability_grid.GetProbability(proposed_xy_index); candidate.score += probability; } candidate.score /= static_cast(discrete_scans[candidate.scan_index].size()); candidate.score *= std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) * options_.translation_delta_cost_weight() + std::abs(candidate.orientation) * options_.rotation_delta_cost_weight())); CHECK_GT(candidate.score, 0.f); } } } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ // This is an implementation of the algorithm described in "Real-Time // Correlative Scan Matching" by Olson. // // The correlative scan matching algorithm is exhaustively evaluating the scan // matching search space. As described by the paper, the basic steps are: // // 1) Evaluate the probability p(z|xi, m) over the entire 3D search window using // the low-resolution table. // 2) Find the best voxel in the low-resolution 3D space that has not already // been considered. Denote this value as Li. If Li < Hbest, terminate: Hbest is // the best scan matching alignment. // 3) Evaluate the search volume inside voxel i using the high resolution table. // Suppose the log-likelihood of this voxel is Hi. Note that Hi <= Li since the // low-resolution map overestimates the log likelihoods. If Hi > Hbest, set // Hbest = Hi. // // This can be made even faster by transforming the scan exactly once over some // discretized range. #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ #include #include #include #include "Eigen/Core" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary); // An implementation of "Real-Time Correlative Scan Matching" by Olson. class RealTimeCorrelativeScanMatcher { public: explicit RealTimeCorrelativeScanMatcher( const proto::RealTimeCorrelativeScanMatcherOptions& options); RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = delete; RealTimeCorrelativeScanMatcher& operator=( const RealTimeCorrelativeScanMatcher&) = delete; // Aligns 'point_cloud' within the 'probability_grid' given an // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. double Match(const transform::Rigid2d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const ProbabilityGrid& probability_grid, transform::Rigid2d* pose_estimate) const; // Computes the score for each Candidate in a collection. The cost is computed // as the sum of probabilities, different from the Ceres CostFunctions: // http://ceres-solver.org/modeling.html // // Visible for testing. void ScoreCandidates(const ProbabilityGrid& probability_grid, const std::vector& discrete_scans, const SearchParameters& search_parameters, std::vector* candidates) const; private: std::vector GenerateExhaustiveSearchCandidates( const SearchParameters& search_parameters) const; const proto::RealTimeCorrelativeScanMatcherOptions options_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ ================================================ FILE: mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include #include #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { namespace { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: RealTimeCorrelativeScanMatcherTest() : probability_grid_( MapLimits(0.05, Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) { { auto parameter_dictionary = common::MakeDictionary( "return { " "insert_free_space = true, " "hit_probability = 0.7, " "miss_probability = 0.4, " "}"); range_data_inserter_ = common::make_unique( CreateRangeDataInserterOptions(parameter_dictionary.get())); } point_cloud_.emplace_back(0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.025f, 0.175f, 0.f); point_cloud_.emplace_back(-0.075f, 0.175f, 0.f); point_cloud_.emplace_back(-0.125f, 0.175f, 0.f); point_cloud_.emplace_back(-0.125f, 0.125f, 0.f); point_cloud_.emplace_back(-0.125f, 0.075f, 0.f); point_cloud_.emplace_back(-0.125f, 0.025f, 0.f); probability_grid_.StartUpdate(); range_data_inserter_->Insert( sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}, &probability_grid_); { auto parameter_dictionary = common::MakeDictionary( "return {" "linear_search_window = 0.6, " "angular_search_window = 0.16, " "translation_delta_cost_weight = 0., " "rotation_delta_cost_weight = 0., " "}"); real_time_correlative_scan_matcher_ = common::make_unique( CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary.get())); } } ProbabilityGrid probability_grid_; std::unique_ptr range_data_inserter_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; }; TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePerfectHighResolutionCandidate) { const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = DiscretizeScans( probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); std::vector candidates; candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(0, candidates[0].y_index_offset); // Every point should align perfectly. EXPECT_NEAR(0.7, candidates[0].score, 1e-2); } TEST_F(RealTimeCorrelativeScanMatcherTest, ScorePartiallyCorrectHighResolutionCandidate) { const std::vector scans = GenerateRotatedScans(point_cloud_, SearchParameters(0, 0, 0., 0.)); const std::vector discrete_scans = DiscretizeScans( probability_grid_.limits(), scans, Eigen::Translation2f::Identity()); std::vector candidates; candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.)); real_time_correlative_scan_matcher_->ScoreCandidates( probability_grid_, discrete_scans, SearchParameters(0, 0, 0., 0.), &candidates); EXPECT_EQ(0, candidates[0].scan_index); EXPECT_EQ(0, candidates[0].x_index_offset); EXPECT_EQ(1, candidates[0].y_index_offset); // 3 points should align perfectly. EXPECT_LT(0.7 * 3. / 7., candidates[0].score); EXPECT_GT(0.7, candidates[0].score); } } // namespace } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/scan_matching/rotation_delta_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ #include "Eigen/Core" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { // Computes the cost of rotating the initial pose estimate. Cost increases with // the solution's distance from the initial estimate. class RotationDeltaCostFunctor { public: // Constructs a new RotationDeltaCostFunctor for the given 'angle'. explicit RotationDeltaCostFunctor(const double scaling_factor, const double angle) : scaling_factor_(scaling_factor), angle_(angle) {} RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; template bool operator()(const T* const pose, T* residual) const { residual[0] = scaling_factor_ * (pose[2] - angle_); return true; } private: const double scaling_factor_; const double angle_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ ================================================ FILE: mapping_2d/scan_matching/translation_delta_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ #include "Eigen/Core" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_2d { namespace scan_matching { // Computes the cost of translating the initial pose estimate. Cost increases // with the solution's distance from the initial estimate. class TranslationDeltaCostFunctor { public: // Constructs a new TranslationDeltaCostFunctor from the given // 'initial_pose_estimate' (x, y, theta). explicit TranslationDeltaCostFunctor( const double scaling_factor, const transform::Rigid2d& initial_pose_estimate) : scaling_factor_(scaling_factor), x_(initial_pose_estimate.translation().x()), y_(initial_pose_estimate.translation().y()) {} TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = delete; template bool operator()(const T* const pose, T* residual) const { residual[0] = scaling_factor_ * (pose[0] - x_); residual[1] = scaling_factor_ * (pose[1] - y_); return true; } private: const double scaling_factor_; const double x_; const double y_; }; } // namespace scan_matching } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ ================================================ FILE: mapping_2d/sparse_pose_graph/constraint_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include #include #include #include #include #include #include #include #include "Eigen/Eigenvalues" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) { return transform::Project2D(submap.local_pose); } ConstraintBuilder::ConstraintBuilder( const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* const thread_pool) : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), ceres_scan_matcher_(options.ceres_scan_matcher_options()) {} ConstraintBuilder::~ConstraintBuilder() { common::MutexLocker locker(&mutex_); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(pending_computations_.size(), 0); CHECK_EQ(submap_queued_work_items_.size(), 0); CHECK(when_done_ == nullptr); } void ConstraintBuilder::MaybeAddConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose) { if (initial_relative_pose.translation().norm() > options_.max_constraint_distance()) { return; } if (sampler_.Pulse()) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ point_cloud, initial_relative_pose, constraint); FinishComputation(current_computation); }); } } void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud, mapping::TrajectoryConnectivity* const trajectory_connectivity) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, point_cloud, transform::Rigid2d::Identity(), constraint); FinishComputation(current_computation); }); } void ConstraintBuilder::NotifyEndOfScan() { common::MutexLocker locker(&mutex_); ++current_computation_; } void ConstraintBuilder::WhenDone( const std::function callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); when_done_ = common::make_unique>(callback); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; thread_pool_->Schedule( [this, current_computation] { FinishComputation(current_computation); }); } void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap, const std::function work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); } else { submap_queued_work_items_[submap_id].push_back(work_item); if (submap_queued_work_items_[submap_id].size() == 1) { thread_pool_->Schedule( [=]() { ConstructSubmapScanMatcher(submap_id, submap); }); } } } void ConstraintBuilder::ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap) { auto submap_scan_matcher = common::make_unique( *submap, options_.fast_correlative_scan_matcher_options()); common::MutexLocker locker(&mutex_); submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)}; for (const std::function& work_item : submap_queued_work_items_[submap_id]) { thread_pool_->Schedule(work_item); } submap_queued_work_items_.erase(submap_id); } const ConstraintBuilder::SubmapScanMatcher* ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); const SubmapScanMatcher* submap_scan_matcher = &submap_scan_matchers_[submap_id]; CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr); return submap_scan_matcher; } void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) { const transform::Rigid2d initial_pose = ComputeSubmapPose(*submap) * initial_relative_pose; const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); const sensor::PointCloud filtered_point_cloud = adaptive_voxel_filter_.Filter(*point_cloud); // The 'constraint_transform' (i <- j) is computed from: // - a 'filtered_point_cloud' in j, // - the initial guess 'initial_pose' for (map <- j), // - the result 'pose_estimate' of Match() (map <- j). // - the ComputeSubmapPose() (map <- i) float score = 0.; transform::Rigid2d pose_estimate = transform::Rigid2d::Identity(); if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( filtered_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); trajectory_connectivity->Connect(node_id.trajectory_id, submap_id.trajectory_id); } else { return; } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { return; } } { common::MutexLocker locker(&mutex_); score_histogram_.Add(score); } // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud, *submap_scan_matcher->probability_grid, &pose_estimate, &unused_summary); const transform::Rigid2d constraint_transform = ComputeSubmapPose(*submap).inverse() * pose_estimate; constraint->reset(new Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), options_.loop_closure_translation_weight(), options_.loop_closure_rotation_weight()}, Constraint::INTER_SUBMAP}); if (options_.log_matches()) { std::ostringstream info; info << "Node " << node_id << " with " << filtered_point_cloud.size() << " points on submap " << submap_id << std::fixed; if (match_full_submap) { info << " matches"; } else { const transform::Rigid2d difference = initial_pose.inverse() * pose_estimate; info << " differs by translation " << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << std::abs(difference.normalized_angle()); } info << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } } void ConstraintBuilder::FinishComputation(const int computation_index) { Result result; std::unique_ptr> callback; { common::MutexLocker locker(&mutex_); if (--pending_computations_[computation_index] == 0) { pending_computations_.erase(computation_index); } if (pending_computations_.empty()) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { for (const std::unique_ptr& constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); } } if (options_.log_matches()) { LOG(INFO) << constraints_.size() << " computations resulted in " << result.size() << " additional constraints."; LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); } constraints_.clear(); callback = std::move(when_done_); when_done_.reset(); } } } if (callback != nullptr) { (*callback)(result); } } int ConstraintBuilder::GetNumFinishedScans() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; } return pending_computations_.begin()->first; } } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/sparse_pose_graph/constraint_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #include #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { // Returns (map <- submap) where 'submap' is a coordinate system at the origin // of the Submap. transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap); // Asynchronously computes constraints. // // Intermingle an arbitrary number of calls to MaybeAddConstraint() or // MaybeAddGlobalConstraint, then call WhenDone(). After all computations are // done the 'callback' will be called with the result and another // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. // // This class is thread-safe. class ConstraintBuilder { public: using Constraint = mapping::SparsePoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* thread_pool); ~ConstraintBuilder(); ConstraintBuilder(const ConstraintBuilder&) = delete; ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'point_cloud' for 'node_id'. The 'initial_pose' is // relative to the 'submap'. // // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddConstraint(const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'point_cloud' for 'node_id'. This performs full-submap // matching. // // The 'trajectory_connectivity' is updated if the full-submap match succeeds. // // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud, mapping::TrajectoryConnectivity* trajectory_connectivity); // Must be called after all computations related to one node have been added. void NotifyEndOfScan(); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. void WhenDone(std::function callback); // Returns the number of consecutive finished scans. int GetNumFinishedScans(); private: struct SubmapScanMatcher { const ProbabilityGrid* probability_grid; std::unique_ptr fast_correlative_scan_matcher; }; // Either schedules the 'work_item', or if needed, schedules the scan matcher // construction and queues the 'work_item'. void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const ProbabilityGrid* submap, std::function work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id, const ProbabilityGrid* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. const SubmapScanMatcher* GetSubmapScanMatcher( const mapping::SubmapId& submap_id) EXCLUDES(mutex_); // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'point_cloud' do not change anymore. // If 'match_full_submap' is true, and global localization succeeds, will // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); // Decrements the 'pending_computations_' count. If all computations are done, // runs the 'when_done_' callback and resets the state. void FinishComputation(int computation_index) EXCLUDES(mutex_); const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; common::ThreadPool* thread_pool_; common::Mutex mutex_; // 'callback' set by WhenDone(). std::unique_ptr> when_done_ GUARDED_BY(mutex_); // Index of the scan in reaction to which computations are currently // added. This is always the highest scan index seen so far, even when older // scans are matched against a new submap. int current_computation_ GUARDED_BY(mutex_) = 0; // For each added scan, maps to the number of pending computations that were // added for it. std::map pending_computations_ GUARDED_BY(mutex_); // Constraints currently being computed in the background. A deque is used to // keep pointers valid when adding more entries. std::deque> constraints_ GUARDED_BY(mutex_); // Map of already constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); // Map by 'submap_id' of scan matchers under construction, and the work // to do once construction is done. std::map>> submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; // Histogram of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); }; } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ ================================================ FILE: mapping_2d/sparse_pose_graph/optimization_problem.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include #include #include #include #include #include #include #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" #include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { namespace { // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the // orientation. std::array FromPose(const transform::Rigid2d& pose) { return {{pose.translation().x(), pose.translation().y(), pose.normalized_angle()}}; } // Converts a pose as represented for Ceres back to an transform::Rigid2d pose. transform::Rigid2d ToPose(const std::array& values) { return transform::Rigid2d({values[0], values[1]}, values[2]); } } // namespace OptimizationProblem::OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& options) : options_(options) {} OptimizationProblem::~OptimizationProblem() {} void OptimizationProblem::AddImuData(const int trajectory_id, const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { CHECK_GE(trajectory_id, 0); imu_data_.resize( std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose) { CHECK_GE(trajectory_id, 0); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); node_data_[trajectory_id].push_back( NodeData{time, initial_point_cloud_pose, point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, const transform::Rigid2d& submap_pose) { CHECK_GE(trajectory_id, 0); submap_data_.resize( std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); submap_data_[trajectory_id].push_back(SubmapData{submap_pose}); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } void OptimizationProblem::Solve(const std::vector& constraints) { if (node_data_.empty()) { // Nothing to optimize. return; } ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. std::vector>> C_submaps(submap_data_.size()); std::vector>> C_nodes(node_data_.size()); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; submap_index != submap_data_[trajectory_id].size(); ++submap_index) { if (trajectory_id == 0 && submap_index == 0) { // Fix the pose of the first submap of the first trajectory. C_submaps[trajectory_id].push_back( FromPose(transform::Rigid2d::Identity())); problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); problem.SetParameterBlockConstant( C_submaps[trajectory_id].back().data()); } else { C_submaps[trajectory_id].push_back( FromPose(submap_data_[trajectory_id][submap_index].pose)); problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); } } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { C_nodes[trajectory_id].resize(node_data_[trajectory_id].size()); for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { C_nodes[trajectory_id][node_index] = FromPose(node_data_[trajectory_id][node_index].point_cloud_pose); problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3); } } // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(constraint.pose)), // Only loop closure constraints should have a loss function. constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) : nullptr, C_submaps.at(constraint.submap_id.trajectory_id) .at(constraint.submap_id.submap_index) .data(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index) .data()); } // Add penalties for changes between consecutive scans. for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { for (size_t node_index = 1; node_index < node_data_[trajectory_id].size(); ++node_index) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(Constraint::Pose{ transform::Embed3D(node_data_[trajectory_id][node_index - 1] .initial_point_cloud_pose.inverse() * node_data_[trajectory_id][node_index] .initial_point_cloud_pose), options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_scan_rotation_penalty_factor()})), nullptr /* loss function */, C_nodes[trajectory_id][node_index - 1].data(), C_nodes[trajectory_id][node_index].data()); } } // Solve. ceres::Solver::Summary summary; ceres::Solve( common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary); if (options_.log_solver_summary()) { LOG(INFO) << summary.FullReport(); } // Store the result. for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; submap_index != submap_data_[trajectory_id].size(); ++submap_index) { submap_data_[trajectory_id][submap_index].pose = ToPose(C_submaps[trajectory_id][submap_index]); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { node_data_[trajectory_id][node_index].point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]); } } } const std::vector>& OptimizationProblem::node_data() const { return node_data_; } const std::vector>& OptimizationProblem::submap_data() const { return submap_data_; } } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/sparse_pose_graph/optimization_problem.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping_3d/imu_integration.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { struct NodeData { common::Time time; transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d point_cloud_pose; }; struct SubmapData { transform::Rigid2d pose; }; // Implements the SPA loop closure method. class OptimizationProblem { public: using Constraint = mapping::SparsePoseGraph::Constraint; explicit OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& options); ~OptimizationProblem(); OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void SetMaxNumIterations(int32 max_num_iterations); // Computes the optimized poses. void Solve(const std::vector& constraints); const std::vector>& node_data() const; const std::vector>& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; std::vector> node_data_; std::vector> submap_data_; }; } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ ================================================ FILE: mapping_2d/sparse_pose_graph/spa_cost_function.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" namespace cartographer { namespace mapping_2d { namespace sparse_pose_graph { class SpaCostFunction { public: using Constraint = mapping::SparsePoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} // Computes the error between the scan-to-submap alignment 'zbar_ij' and the // difference of submap pose 'c_i' and scan pose 'c_j' which are both in an // arbitrary common frame. template static std::array ComputeUnscaledError( const transform::Rigid2d& zbar_ij, const T* const c_i, const T* const c_j) { const T cos_theta_i = cos(c_i[2]); const T sin_theta_i = sin(c_i[2]); const T delta_x = c_j[0] - c_i[0]; const T delta_y = c_j[1] - c_i[1]; const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, -sin_theta_i * delta_x + cos_theta_i * delta_y, c_j[2] - c_i[2]}; return {{T(zbar_ij.translation().x()) - h[0], T(zbar_ij.translation().y()) - h[1], common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) - h[2])}}; } // Computes the error scaled by 'translation_weight' and 'rotation_weight', // storing it in 'e'. template static void ComputeScaledError(const Constraint::Pose& pose, const T* const c_i, const T* const c_j, T* const e) { const std::array e_ij = ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j); e[0] = e_ij[0] * T(pose.translation_weight); e[1] = e_ij[1] * T(pose.translation_weight); e[2] = e_ij[2] * T(pose.rotation_weight); } template bool operator()(const T* const c_i, const T* const c_j, T* e) const { ComputeScaledError(pose_, c_i, c_j, e); return true; } private: const Constraint::Pose pose_; }; } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ ================================================ FILE: mapping_2d/sparse_pose_graph.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/sparse_pose_graph.h" #include #include #include #include #include #include #include #include #include #include #include "Eigen/Eigenvalues" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/voxel_filter.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { SparsePoseGraph::SparsePoseGraph( const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options()), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} SparsePoseGraph::~SparsePoseGraph() { WaitForAllComputations(); common::MutexLocker locker(&mutex_); CHECK(scan_queue_ == nullptr); } void SparsePoseGraph::GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) { CHECK(!insertion_submaps.empty()); const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]); const int trajectory_id = first_submap_id.trajectory_id; CHECK_GE(trajectory_id, 0); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. CHECK_EQ(first_submap_id.submap_index, 0); if (static_cast(trajectory_id) >= submap_data.size() || submap_data[trajectory_id].empty()) { optimization_problem_.AddSubmap(trajectory_id, transform::Rigid2d::Identity()); } return; } CHECK_EQ(2, insertion_submaps.size()); const int next_submap_index = submap_data.at(trajectory_id).size(); // CHECK that we have a index for the second submap. const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]); CHECK_EQ(second_submap_id.trajectory_id, trajectory_id); CHECK_LE(second_submap_id.submap_index, next_submap_index); // Extrapolate if necessary. if (second_submap_id.submap_index == next_submap_index) { const auto& first_submap_pose = submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]) .inverse() * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[1])); } } void SparsePoseGraph::AddScan( common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, const int trajectory_id, const mapping::Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory_id, tracking_to_pose}); trajectory_nodes_.Append(trajectory_id, mapping::TrajectoryNode{ &constant_node_data_.back(), optimized_pose, }); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); if (submap_ids_.count(insertion_submaps.back()) == 0) { const mapping::SubmapId submap_id = submap_states_.Append(trajectory_id, SubmapState()); submap_ids_.emplace(insertion_submaps.back(), submap_id); submap_states_.at(submap_id).submap = insertion_submaps.back(); } const mapping::Submap* const finished_submap = insertion_submaps.front()->finished_probability_grid != nullptr ? insertion_submaps.front() : nullptr; // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { global_localization_samplers_[trajectory_id] = common::make_unique( options_.global_sampling_ratio()); } AddWorkItem([=]() REQUIRES(mutex_) { ComputeConstraintsForScan(matching_submap, insertion_submaps, finished_submap, pose); }); } void SparsePoseGraph::AddWorkItem(std::function work_item) { if (scan_queue_ == nullptr) { work_item(); } else { scan_queue_->push_back(work_item); } } void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, angular_velocity); }); } void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { // Only globally match against submaps not in this trajectory. if (node_id.trajectory_id != submap_id.trajectory_id && global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_.at(submap_id).submap, node_id, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && reverse_connected_components_.count(submap_id.trajectory_id) > 0 && reverse_connected_components_.at(node_id.trajectory_id) == reverse_connected_components_.at(submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || scan_and_submap_trajectories_connected) { const transform::Rigid2d initial_relative_pose = optimization_problem_.submap_data() .at(submap_id.trajectory_id) .at(submap_id.submap_index) .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) .at(node_id.node_index) .point_cloud_pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_states_.at(submap_id).submap, node_id, &trajectory_nodes_.at(node_id).constant_data->range_data_2d.returns, initial_relative_pose); } } } void SparsePoseGraph::ComputeConstraintsForOldScans( const mapping::Submap* submap) { const auto submap_id = GetSubmapId(submap); const auto& submap_state = submap_states_.at(submap_id); const auto& node_data = optimization_problem_.node_data(); for (size_t trajectory_id = 0; trajectory_id != node_data.size(); ++trajectory_id) { for (size_t node_index = 0; node_index != node_data[trajectory_id].size(); ++node_index) { const mapping::NodeId node_id{static_cast(trajectory_id), static_cast(node_index)}; if (submap_state.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } } } } void SparsePoseGraph::ComputeConstraintsForScan( const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data() .at(matching_id.trajectory_id) .at(matching_id.submap_index) .pose * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; const mapping::NodeId node_id{ matching_id.trajectory_id, static_cast(matching_id.trajectory_id) < optimization_problem_.node_data().size() ? static_cast(optimization_problem_.node_data() .at(matching_id.trajectory_id) .size()) : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id).finished); submap_states_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; constraints_.push_back(Constraint{submap_id, node_id, {transform::Embed3D(constraint_transform), options_.matcher_translation_weight(), options_.matcher_rotation_weight()}, Constraint::INTRA_SUBMAP}); } for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); ++trajectory_id) { for (int submap_index = 0; submap_index < submap_states_.num_indices(trajectory_id); ++submap_index) { const mapping::SubmapId submap_id{trajectory_id, submap_index}; if (submap_states_.at(submap_id).finished) { CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0); ComputeConstraint(node_id, submap_id); } } } if (finished_submap != nullptr) { const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap); SubmapState& finished_submap_state = submap_states_.at(finished_submap_id); CHECK(!finished_submap_state.finished); // We have a new completed submap, so we look into adding constraints for // old scans. ComputeConstraintsForOldScans(finished_submap); finished_submap_state.finished = true; } constraint_builder_.NotifyEndOfScan(); ++num_scans_since_last_loop_closure_; if (options_.optimize_every_n_scans() > 0 && num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { CHECK(!run_loop_closure_); run_loop_closure_ = true; // If there is a 'scan_queue_' already, some other thread will take care. if (scan_queue_ == nullptr) { scan_queue_ = common::make_unique>>(); HandleScanQueue(); } } } void SparsePoseGraph::HandleScanQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); common::MutexLocker locker(&mutex_); num_scans_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { if (scan_queue_->empty()) { LOG(INFO) << "We caught up. Hooray!"; scan_queue_.reset(); return; } scan_queue_->front()(); scan_queue_->pop_front(); } // We have to optimize again. HandleScanQueue(); }); } void SparsePoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); const int num_finished_scans_at_start = constraint_builder_.GetNumFinishedScans(); while (!locker.AwaitWithTimeout( [this]() REQUIRES(mutex_) { return constraint_builder_.GetNumFinishedScans() == num_trajectory_nodes_; }, common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) << 100. * (constraint_builder_.GetNumFinishedScans() - num_finished_scans_at_start) / (num_trajectory_nodes_ - num_finished_scans_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( [this, ¬ification]( const sparse_pose_graph::ConstraintBuilder::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; }); locker.Await([¬ification]() { return notification; }); } void SparsePoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( options_.max_num_final_iterations()); RunOptimization(); optimization_problem_.SetMaxNumIterations( options_.optimization_problem_options() .ceres_solver_options() .max_num_iterations()); } void SparsePoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } optimization_problem_.Solve(constraints_); common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { int node_index = 0; const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); for (; node_index != static_cast(node_data[trajectory_id].size()); ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = transform::Embed3D( node_data[trajectory_id][node_index].point_cloud_pose); } // Extrapolate all point cloud poses that were added later. const auto new_submap_transforms = ExtrapolateSubmapTransforms( optimization_problem_.submap_data(), trajectory_id); const auto old_submap_transforms = ExtrapolateSubmapTransforms( optimized_submap_transforms_, trajectory_id); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); const transform::Rigid3d extrapolation_transform = new_submap_transforms.back() * old_submap_transforms.back().inverse(); for (; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = extrapolation_transform * trajectory_nodes_.at(node_id).pose; } } optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { for (const int trajectory_id : connected_components_[i]) { reverse_connected_components_.emplace(trajectory_id, i); } } } std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); return trajectory_nodes_.data(); } std::vector SparsePoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); if (submap_states_.num_trajectories() <= trajectory_id || submap_states_.num_indices(trajectory_id) == 0) { return transform::Rigid3d::Identity(); } const auto extrapolated_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * submap_states_ .at(mapping::SubmapId{ trajectory_id, static_cast(extrapolated_submap_transforms.size()) - 1}) .submap->local_pose.inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { common::MutexLocker locker(&mutex_); return connected_components_; } std::vector SparsePoseGraph::GetSubmapTransforms( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, const int trajectory_id) const { if (trajectory_id >= submap_states_.num_trajectories()) { return {transform::Rigid3d::Identity()}; } // Submaps for which we have optimized poses. std::vector result; for (int submap_index = 0; submap_index != submap_states_.num_indices(trajectory_id); ++submap_index) { const mapping::SubmapId submap_id{trajectory_id, submap_index}; const auto& state = submap_states_.at(submap_id); if (static_cast(trajectory_id) < submap_transforms.size() && result.size() < submap_transforms.at(trajectory_id).size()) { // Submaps for which we have optimized poses. result.push_back( Embed3D(submap_transforms.at(trajectory_id).at(result.size()).pose)); } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. const mapping::SubmapId previous_submap_id{ trajectory_id, static_cast(result.size()) - 1}; result.push_back( result.back() * submap_states_.at(previous_submap_id).submap->local_pose.inverse() * state.submap->local_pose); } } if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } return result; } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/sparse_pose_graph.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #include #include #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_2d { // Implements the loop closure method called Sparse Pose Adjustment (SPA) from // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." // Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference // on (pp. 22--29). IEEE, 2010. // // It is extended for submapping: // Each scan has been matched against one or more submaps (adding a constraint // for each match), both poses of scans and of submaps are to be optimized. // All constraints are between a submap i and a scan j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool); ~SparsePoseGraph() override; SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; // Adds a new 'range_data_in_pose' observation at 'time', and a 'pose' // that will later be optimized. The 'tracking_to_pose' is remembered so // that the optimized pose can be embedded into 3D. The 'pose' was determined // by scan matching against the 'matching_submap' and the scan was inserted // into the 'insertion_submaps'. void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose, const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose, int trajectory_id, const mapping::Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms(int trajectory_id) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: struct SubmapState { const mapping::Submap* submap = nullptr; // IDs of the scans that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. std::set node_ids; // Whether in the current state of the background thread this submap is // finished. When this transitions to true, all scans are tried to match // against this submap. Likewise, all new scans are matched against submaps // which are finished. bool finished = false; }; // Handles a new work item. void AddWorkItem(std::function work_item) REQUIRES(mutex_); mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const REQUIRES(mutex_) { const auto iterator = submap_ids_.find(submap); CHECK(iterator != submap_ids_.end()); return iterator->second; } // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. void GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose) REQUIRES(mutex_); // Computes constraints for a scan and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) REQUIRES(mutex_); // Adds constraints for older scans whenever a new submap is finished. void ComputeConstraintsForOldScans(const mapping::Submap* submap) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have // been computed, that will also do all work that queue up in 'scan_queue_'. void HandleScanQueue() REQUIRES(mutex_); // Waits until we caught up (i.e. nothing is waiting to be scheduled), and // all computations have finished. void WaitForAllComputations() EXCLUDES(mutex_); // Runs the optimization. Callers have to make sure, that there is only one // optimization being run at a time. void RunOptimization() EXCLUDES(mutex_); // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, int trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; // If it exists, further scans must be added to this queue, and will be // considered later. std::unique_ptr>> scan_queue_ GUARDED_BY(mutex_); // How our various trajectories are related. mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); // We globally localize a fraction of the scans from each trajectory. std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); // Number of scans added since last loop closure. int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. std::map submap_ids_ GUARDED_BY(mutex_); mapping::NestedVectorsById submap_states_ GUARDED_BY(mutex_); // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID. std::map reverse_connected_components_; // Data that are currently being shown. // // Deque to keep references valid for the background computation when adding // new data. std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ ================================================ FILE: mapping_2d/sparse_pose_graph_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/sparse_pose_graph.h" #include #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/mapping_2d/submaps.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_2d { namespace { class SparsePoseGraphTest : public ::testing::Test { protected: SparsePoseGraphTest() : thread_pool_(1) { // Builds a wavy, irregularly circular point cloud that is unique // rotationally. This gives us good rotational texture and avoids any // possibility of the CeresScanMatcher preferring free space (> // kMinProbability) to unknown space (== kMinProbability). for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) { const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f); point_cloud_.emplace_back(r * std::sin(t), r * std::cos(t), 0.f); } { auto parameter_dictionary = common::MakeDictionary(R"text( return { resolution = 0.05, half_length = 21., num_range_data = 1, output_debug_images = false, range_data_inserter = { insert_free_space = true, hit_probability = 0.53, miss_probability = 0.495, }, })text"); submaps_ = common::make_unique( CreateSubmapsOptions(parameter_dictionary.get())); } { auto parameter_dictionary = common::MakeDictionary(R"text( return { optimize_every_n_scans = 1000, constraint_builder = { sampling_ratio = 1., max_constraint_distance = 6., adaptive_voxel_filter = { max_length = 1e-2, min_num_points = 1000, max_range = 50., }, min_score = 0.5, global_localization_min_score = 0.6, loop_closure_translation_weight = 1., loop_closure_rotation_weight = 1., log_matches = true, fast_correlative_scan_matcher = { linear_search_window = 3., angular_search_window = 0.1, branch_and_bound_depth = 3, }, ceres_scan_matcher = { occupied_space_weight = 20., translation_weight = 10., rotation_weight = 1., ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, num_threads = 1, }, }, fast_correlative_scan_matcher_3d = { branch_and_bound_depth = 3, full_resolution_depth = 3, rotational_histogram_size = 30, min_rotational_score = 0.1, linear_xy_search_window = 4., linear_z_search_window = 4., angular_search_window = 0.1, }, ceres_scan_matcher_3d = { occupied_space_weight_0 = 20., translation_weight = 10., rotation_weight = 1., only_optimize_yaw = true, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 50, num_threads = 1, }, }, }, matcher_translation_weight = 1., matcher_rotation_weight = 1., optimization_problem = { acceleration_weight = 1., rotation_weight = 1e2, huber_scale = 1., consecutive_scan_translation_penalty_factor = 0., consecutive_scan_rotation_penalty_factor = 0., log_solver_summary = true, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 200, num_threads = 1, }, }, max_num_final_iterations = 200, global_sampling_ratio = 0.01, })text"); sparse_pose_graph_ = common::make_unique( mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), &thread_pool_); } current_pose_ = transform::Rigid2d::Identity(); } void MoveRelativeWithNoise(const transform::Rigid2d& movement, const transform::Rigid2d& noise) { current_pose_ = current_pose_ * movement; const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( point_cloud_, transform::Embed3D(current_pose_.inverse().cast())); const mapping::Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); std::vector insertion_submaps; for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } const sensor::RangeData range_data{ Eigen::Vector3f::Zero(), new_point_cloud, {}}; const transform::Rigid2d pose_estimate = noise * current_pose_; constexpr int kTrajectoryId = 0; submaps_->InsertRangeData(TransformRangeData( range_data, transform::Embed3D(pose_estimate.cast()))); sparse_pose_graph_->AddScan( common::FromUniversal(0), transform::Rigid3d::Identity(), range_data, pose_estimate, kTrajectoryId, matching_submap, insertion_submaps); } void MoveRelative(const transform::Rigid2d& movement) { MoveRelativeWithNoise(movement, transform::Rigid2d::Identity()); } sensor::PointCloud point_cloud_; std::unique_ptr submaps_; common::ThreadPool thread_pool_; std::unique_ptr sparse_pose_graph_; transform::Rigid2d current_pose_; }; TEST_F(SparsePoseGraphTest, EmptyMap) { sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); EXPECT_THAT(nodes.size(), ::testing::Eq(0)); } TEST_F(SparsePoseGraphTest, NoMovement) { MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity()); sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(nodes.size(), ::testing::Eq(1)); EXPECT_THAT(nodes[0].size(), ::testing::Eq(3)); EXPECT_THAT(nodes[0][0].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); EXPECT_THAT(nodes[0][1].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); EXPECT_THAT(nodes[0][2].pose, transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); } TEST_F(SparsePoseGraphTest, NoOverlappingScans) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; for (int i = 0; i != 4; ++i) { MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.)); poses.emplace_back(current_pose_); } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(nodes.size(), ::testing::Eq(1)); for (int i = 0; i != 4; ++i) { EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) << i; } } TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingScans) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector poses; for (int i = 0; i != 5; ++i) { MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.)); poses.emplace_back(current_pose_); } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(nodes.size(), ::testing::Eq(1)); for (int i = 0; i != 5; ++i) { EXPECT_THAT(poses[i], IsNearly(transform::Project2D(nodes[0][i].pose), 1e-2)) << i; } } TEST_F(SparsePoseGraphTest, OverlappingScans) { std::mt19937 rng(0); std::uniform_real_distribution distribution(-1., 1.); std::vector ground_truth; std::vector poses; for (int i = 0; i != 5; ++i) { const double noise_x = 0.1 * distribution(rng); const double noise_y = 0.1 * distribution(rng); const double noise_orientation = 0.1 * distribution(rng); transform::Rigid2d noise({noise_x, noise_y}, noise_orientation); MoveRelativeWithNoise( transform::Rigid2d({0.15 * distribution(rng), 0.4}, 0.), noise); ground_truth.emplace_back(current_pose_); poses.emplace_back(noise * current_pose_); } sparse_pose_graph_->RunFinalOptimization(); const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); ASSERT_THAT(nodes.size(), ::testing::Eq(1)); transform::Rigid2d true_movement = ground_truth.front().inverse() * ground_truth.back(); transform::Rigid2d movement_before = poses.front().inverse() * poses.back(); transform::Rigid2d error_before = movement_before.inverse() * true_movement; transform::Rigid3d optimized_movement = nodes[0].front().pose.inverse() * nodes[0].back().pose; transform::Rigid2d optimized_error = transform::Project2D(optimized_movement).inverse() * true_movement; EXPECT_THAT(std::abs(optimized_error.normalized_angle()), ::testing::Lt(std::abs(error_before.normalized_angle()))); EXPECT_THAT(optimized_error.translation().norm(), ::testing::Lt(error_before.translation().norm())); } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/submaps.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/submaps.h" #include #include #include #include #include #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "glog/logging.h" #include "webp/encode.h" namespace cartographer { namespace mapping_2d { namespace { void WriteDebugImage(const string& filename, const ProbabilityGrid& probability_grid) { constexpr int kUnknown = 128; const mapping_2d::CellLimits& cell_limits = probability_grid.limits().cell_limits(); const int width = cell_limits.num_x_cells; const int height = cell_limits.num_y_cells; std::vector rgb; for (const Eigen::Array2i& xy_index : mapping_2d::XYIndexRangeIterator( probability_grid.limits().cell_limits())) { CHECK(probability_grid.limits().Contains(xy_index)); const uint8_t value = probability_grid.IsKnown(xy_index) ? common::RoundToInt( (1. - probability_grid.GetProbability(xy_index)) * 255 + 0) : kUnknown; rgb.push_back(value); rgb.push_back(value); rgb.push_back(value); } uint8_t* output = nullptr; size_t output_size = WebPEncodeLosslessRGB(rgb.data(), width, height, 3 * width, &output); std::unique_ptr output_deleter(output, std::free); std::ofstream output_file(filename, std::ios::out | std::ios::binary); output_file.write(reinterpret_cast(output), output_size); output_file.close(); CHECK(output_file) << "Writing " << filename << " failed."; } } // namespace ProbabilityGrid ComputeCroppedProbabilityGrid( const ProbabilityGrid& probability_grid) { Eigen::Array2i offset; CellLimits limits; probability_grid.ComputeCroppedLimits(&offset, &limits); const double resolution = probability_grid.limits().resolution(); const Eigen::Vector2d max = probability_grid.limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x()); ProbabilityGrid cropped_grid(MapLimits(resolution, max, limits)); cropped_grid.StartUpdate(); for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(limits)) { if (probability_grid.IsKnown(xy_index + offset)) { cropped_grid.SetProbability( xy_index, probability_grid.GetProbability(xy_index + offset)); } } return cropped_grid; } proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::SubmapsOptions options; options.set_resolution(parameter_dictionary->GetDouble("resolution")); options.set_half_length(parameter_dictionary->GetDouble("half_length")); options.set_num_range_data( parameter_dictionary->GetNonNegativeInt("num_range_data")); options.set_output_debug_images( parameter_dictionary->GetBool("output_debug_images")); *options.mutable_range_data_inserter_options() = CreateRangeDataInserterOptions( parameter_dictionary->GetDictionary("range_data_inserter").get()); CHECK_GT(options.num_range_data(), 0); return options; } Submap::Submap(const MapLimits& limits, const Eigen::Vector2f& origin) : mapping::Submap(transform::Rigid3d::Translation( Eigen::Vector3d(origin.x(), origin.y(), 0.))), probability_grid(limits) {} Submaps::Submaps(const proto::SubmapsOptions& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one likelihood field which we can return, // and will create it at the origin in absence of a better choice. AddSubmap(Eigen::Vector2f::Zero()); } void Submaps::InsertRangeData(const sensor::RangeData& range_data) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); CHECK(submap->finished_probability_grid == nullptr); range_data_inserter_.Insert(range_data, &submap->probability_grid); ++submap->num_range_data; } const Submap* const last_submap = Get(size() - 1); if (last_submap->num_range_data == options_.num_range_data()) { AddSubmap(range_data.origin.head<2>()); } } const Submap* Submaps::Get(int index) const { CHECK_GE(index, 0); CHECK_LT(index, size()); return submaps_[index].get(); } int Submaps::size() const { return submaps_.size(); } void Submaps::SubmapToProto( const int index, const transform::Rigid3d&, mapping::proto::SubmapQuery::Response* const response) const { AddProbabilityGridToResponse(Get(index)->local_pose, Get(index)->probability_grid, response); } void Submaps::FinishSubmap(int index) { // Crop the finished Submap before inserting a new Submap to reduce peak // memory usage a bit. Submap* submap = submaps_[index].get(); CHECK(submap->finished_probability_grid == nullptr); submap->probability_grid = ComputeCroppedProbabilityGrid(submap->probability_grid); submap->finished_probability_grid = &submap->probability_grid; if (options_.output_debug_images()) { // Output the Submap that won't be changed from now on. WriteDebugImage("submap" + std::to_string(index) + ".webp", submap->probability_grid); } } void Submaps::AddSubmap(const Eigen::Vector2f& origin) { if (size() > 1) { FinishSubmap(size() - 2); } const int num_cells_per_dimension = common::RoundToInt(2. * options_.half_length() / options_.resolution()) + 1; submaps_.push_back(common::make_unique( MapLimits(options_.resolution(), origin.cast() + options_.half_length() * Eigen::Vector2d::Ones(), CellLimits(num_cells_per_dimension, num_cells_per_dimension)), origin)); LOG(INFO) << "Added submap " << size(); } } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/submaps.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ #define CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ #include #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/map_limits.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/proto/submaps_options.pb.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_2d { ProbabilityGrid ComputeCroppedProbabilityGrid( const ProbabilityGrid& probability_grid); proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary); struct Submap : public mapping::Submap { Submap(const MapLimits& limits, const Eigen::Vector2f& origin); ProbabilityGrid probability_grid; }; // A container of Submaps. class Submaps : public mapping::Submaps { public: explicit Submaps(const proto::SubmapsOptions& options); Submaps(const Submaps&) = delete; Submaps& operator=(const Submaps&) = delete; const Submap* Get(int index) const override; int size() const override; void SubmapToProto( int index, const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* response) const override; // Inserts 'range_data' into the Submap collection. void InsertRangeData(const sensor::RangeData& range_data); private: void FinishSubmap(int index); void AddSubmap(const Eigen::Vector2f& origin); const proto::SubmapsOptions options_; std::vector> submaps_; RangeDataInserter range_data_inserter_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_SUBMAPS_H_ ================================================ FILE: mapping_2d/submaps_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/submaps.h" #include #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/port.h" #include "cartographer/transform/transform.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_2d { namespace { TEST(SubmapsTest, TheRightNumberOfScansAreInserted) { constexpr int kNumRangeData = 10; auto parameter_dictionary = common::MakeDictionary( "return {" "resolution = 0.05, " "half_length = 10., " "num_range_data = " + std::to_string(kNumRangeData) + ", " "output_debug_images = false, " "range_data_inserter = {" "insert_free_space = true, " "hit_probability = 0.53, " "miss_probability = 0.495, " "}," "}"); Submaps submaps{CreateSubmapsOptions(parameter_dictionary.get())}; for (int i = 0; i != 1000; ++i) { submaps.InsertRangeData({Eigen::Vector3f::Zero(), {}, {}}); const int matching = submaps.matching_index(); // Except for the first, maps should only be returned after enough scans. if (matching != 0) { EXPECT_LE(kNumRangeData, submaps.Get(matching)->num_range_data); } } for (int i = 0; i != submaps.size() - 2; ++i) { // Submaps should not be left without the right number of scans in them. EXPECT_EQ(kNumRangeData * 2, submaps.Get(i)->num_range_data); } } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_2d/xy_index.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ #define CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ #include #include #include #include #include "Eigen/Core" #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping_2d/proto/cell_limits.pb.h" #include "glog/logging.h" namespace cartographer { namespace mapping_2d { struct CellLimits { CellLimits() = default; CellLimits(int init_num_x_cells, int init_num_y_cells) : num_x_cells(init_num_x_cells), num_y_cells(init_num_y_cells) {} explicit CellLimits(const proto::CellLimits& cell_limits) : num_x_cells(cell_limits.num_x_cells()), num_y_cells(cell_limits.num_y_cells()) {} int num_x_cells = 0; int num_y_cells = 0; }; inline proto::CellLimits ToProto(const CellLimits& cell_limits) { proto::CellLimits result; result.set_num_x_cells(cell_limits.num_x_cells); result.set_num_y_cells(cell_limits.num_y_cells); return result; } // Iterates in row-major order through a range of xy-indices. class XYIndexRangeIterator : public std::iterator { public: // Constructs a new iterator for the specified range. XYIndexRangeIterator(const Eigen::Array2i& min_xy_index, const Eigen::Array2i& max_xy_index) : min_xy_index_(min_xy_index), max_xy_index_(max_xy_index), xy_index_(min_xy_index) {} // Constructs a new iterator for everything contained in 'cell_limits'. explicit XYIndexRangeIterator(const CellLimits& cell_limits) : XYIndexRangeIterator(Eigen::Array2i::Zero(), Eigen::Array2i(cell_limits.num_x_cells - 1, cell_limits.num_y_cells - 1)) {} XYIndexRangeIterator& operator++() { // This is a necessary evil. Bounds checking is very expensive and needs to // be avoided in production. We have unit tests that exercise this check // in debug mode. DCHECK(*this != end()); if (xy_index_.x() < max_xy_index_.x()) { ++xy_index_.x(); } else { xy_index_.x() = min_xy_index_.x(); ++xy_index_.y(); } return *this; } Eigen::Array2i& operator*() { return xy_index_; } bool operator==(const XYIndexRangeIterator& other) const { return (xy_index_ == other.xy_index_).all(); } bool operator!=(const XYIndexRangeIterator& other) const { return !operator==(other); } XYIndexRangeIterator begin() { return XYIndexRangeIterator(min_xy_index_, max_xy_index_); } XYIndexRangeIterator end() { XYIndexRangeIterator it = begin(); it.xy_index_ = Eigen::Array2i(min_xy_index_.x(), max_xy_index_.y() + 1); return it; } private: Eigen::Array2i min_xy_index_; Eigen::Array2i max_xy_index_; Eigen::Array2i xy_index_; }; } // namespace mapping_2d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_2D_XY_INDEX_H_ ================================================ FILE: mapping_2d/xy_index_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_2d/xy_index.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_2d { namespace { TEST(XYIndexTest, CellLimitsToProto) { const CellLimits limits(1, 2); const auto proto = ToProto(limits); EXPECT_EQ(limits.num_x_cells, proto.num_x_cells()); EXPECT_EQ(limits.num_y_cells, proto.num_y_cells()); } TEST(XYIndexTest, CellLimitsProtoConstructor) { proto::CellLimits limits; limits.set_num_x_cells(1); limits.set_num_y_cells(2); auto native = CellLimits(limits); EXPECT_EQ(limits.num_x_cells(), native.num_x_cells); EXPECT_EQ(limits.num_y_cells(), native.num_y_cells); } TEST(XYIndexTest, XYIndexRangeIterator) { const Eigen::Array2i min(1, 2); const Eigen::Array2i max(3, 4); XYIndexRangeIterator it(min, max); EXPECT_TRUE((min == *it.begin()).all()) << *it.begin(); EXPECT_TRUE((Eigen::Array2i(1, 5) == *it.end()).all()) << *it.end(); EXPECT_TRUE((min == *it).all()) << *it; int num_indices = 0; for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(min, max)) { LOG(INFO) << xy_index; EXPECT_TRUE((xy_index >= min).all()); EXPECT_TRUE((xy_index <= max).all()); ++num_indices; } EXPECT_EQ(9, num_indices); } } // namespace } // namespace mapping_2d } // namespace cartographer ================================================ FILE: mapping_3d/acceleration_cost_function.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { namespace mapping_3d { // Penalizes differences between IMU data and optimized accelerations. class AccelerationCostFunction { public: AccelerationCostFunction(const double scaling_factor, const Eigen::Vector3d& delta_velocity_imu_frame, const double first_delta_time_seconds, const double second_delta_time_seconds) : scaling_factor_(scaling_factor), delta_velocity_imu_frame_(delta_velocity_imu_frame), first_delta_time_seconds_(first_delta_time_seconds), second_delta_time_seconds_(second_delta_time_seconds) {} AccelerationCostFunction(const AccelerationCostFunction&) = delete; AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete; template bool operator()(const T* const middle_rotation, const T* const start_position, const T* const middle_position, const T* const end_position, const T* const gravity_constant, T* residual) const { const Eigen::Matrix imu_delta_velocity = ToEigen(middle_rotation) * delta_velocity_imu_frame_.cast() - *gravity_constant * (0.5 * (first_delta_time_seconds_ + second_delta_time_seconds_) * Eigen::Vector3d::UnitZ()) .cast(); const Eigen::Matrix start_velocity = (Eigen::Map>(middle_position) - Eigen::Map>(start_position)) / T(first_delta_time_seconds_); const Eigen::Matrix end_velocity = (Eigen::Map>(end_position) - Eigen::Map>(middle_position)) / T(second_delta_time_seconds_); const Eigen::Matrix delta_velocity = end_velocity - start_velocity; (Eigen::Map>(residual) = T(scaling_factor_) * (imu_delta_velocity - delta_velocity)); return true; } private: template static Eigen::Quaternion ToEigen(const T* const quaternion) { return Eigen::Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]); } const double scaling_factor_; const Eigen::Vector3d delta_velocity_imu_frame_; const double first_delta_time_seconds_; const double second_delta_time_seconds_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_ ================================================ FILE: mapping_3d/ceres_pose.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/ceres_pose.h" namespace cartographer { namespace mapping_3d { CeresPose::CeresPose( const transform::Rigid3d& rigid, std::unique_ptr translation_parametrization, std::unique_ptr rotation_parametrization, ceres::Problem* problem) : translation_({{rigid.translation().x(), rigid.translation().y(), rigid.translation().z()}}), rotation_({{rigid.rotation().w(), rigid.rotation().x(), rigid.rotation().y(), rigid.rotation().z()}}) { problem->AddParameterBlock(translation_.data(), 3, translation_parametrization.release()); problem->AddParameterBlock(rotation_.data(), 4, rotation_parametrization.release()); } const transform::Rigid3d CeresPose::ToRigid() const { return transform::Rigid3d( Eigen::Map(translation_.data()), Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], rotation_[3])); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/ceres_pose.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ #define CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ #include #include #include "Eigen/Core" #include "cartographer/transform/rigid_transform.h" #include "ceres/ceres.h" namespace cartographer { namespace mapping_3d { class CeresPose { public: CeresPose( const transform::Rigid3d& rigid, std::unique_ptr translation_parametrization, std::unique_ptr rotation_parametrization, ceres::Problem* problem); CeresPose(const CeresPose&) = delete; CeresPose& operator=(const CeresPose&) = delete; const transform::Rigid3d ToRigid() const; double* translation() { return translation_.data(); } double* rotation() { return rotation_.data(); } private: std::array translation_; // Rotation quaternion as (w, x, y, z). std::array rotation_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ ================================================ FILE: mapping_3d/global_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/global_trajectory_builder.h" #include "cartographer/mapping_3d/local_trajectory_builder.h" namespace cartographer { namespace mapping_3d { GlobalTrajectoryBuilder::GlobalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options, const int trajectory_id, SparsePoseGraph* sparse_pose_graph) : trajectory_id_(trajectory_id), sparse_pose_graph_(sparse_pose_graph), local_trajectory_builder_(CreateLocalTrajectoryBuilder(options)) {} GlobalTrajectoryBuilder::~GlobalTrajectoryBuilder() {} const mapping_3d::Submaps* GlobalTrajectoryBuilder::submaps() const { return local_trajectory_builder_->submaps(); } void GlobalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { local_trajectory_builder_->AddImuData(time, linear_acceleration, angular_velocity); sparse_pose_graph_->AddImuData(trajectory_id_, time, linear_acceleration, angular_velocity); } void GlobalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { auto insertion_result = local_trajectory_builder_->AddRangefinderData(time, origin, ranges); if (insertion_result == nullptr) { return; } sparse_pose_graph_->AddScan( insertion_result->time, insertion_result->range_data_in_tracking, insertion_result->pose_observation, trajectory_id_, insertion_result->matching_submap, insertion_result->insertion_submaps); } void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time, const transform::Rigid3d& pose) { local_trajectory_builder_->AddOdometerData(time, pose); } const GlobalTrajectoryBuilder::PoseEstimate& GlobalTrajectoryBuilder::pose_estimate() const { return local_trajectory_builder_->pose_estimate(); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/global_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ #include #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/sparse_pose_graph.h" namespace cartographer { namespace mapping_3d { class GlobalTrajectoryBuilder : public mapping::GlobalTrajectoryBuilderInterface { public: GlobalTrajectoryBuilder(const proto::LocalTrajectoryBuilderOptions& options, int trajectory_id, mapping_3d::SparsePoseGraph* sparse_pose_graph); ~GlobalTrajectoryBuilder() override; GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder&) = delete; GlobalTrajectoryBuilder& operator=(const GlobalTrajectoryBuilder&) = delete; const mapping_3d::Submaps* submaps() const override; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; void AddRangefinderData(common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; const PoseEstimate& pose_estimate() const override; private: const int trajectory_id_; mapping_3d::SparsePoseGraph* const sparse_pose_graph_; std::unique_ptr local_trajectory_builder_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_GLOBAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_3d/hybrid_grid.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ #define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ #include #include #include #include #include #include "Eigen/Core" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/port.h" #include "cartographer/mapping/probability_values.h" #include "cartographer/mapping_3d/proto/hybrid_grid.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { // Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat // z-major index. inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) { DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index; return (((index.z() << bits) + index.y()) << bits) + index.x(); } // Converts a flat z-major 'index' to a 3-dimensional index with each dimension // from 0 to 2^'bits' - 1. inline Eigen::Array3i To3DIndex(const int index, const int bits) { DCHECK_LT(index, 1 << (3 * bits)); const int mask = (1 << bits) - 1; return Eigen::Array3i(index & mask, (index >> bits) & mask, (index >> bits) >> bits); } // A function to compare value to the default value. (Allows specializations). template bool IsDefaultValue(const TValueType& v) { return v == TValueType(); } // Specialization to compare a std::vector to the default value. template bool IsDefaultValue(const std::vector& v) { return v.empty(); } // A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of // type 'ValueType' in contiguous memory. Indices in each dimension are 0-based. template class FlatGrid { public: using ValueType = TValueType; // Creates a new flat grid with all values being default constructed. FlatGrid() { for (ValueType& value : cells_) { value = ValueType(); } } FlatGrid(const FlatGrid&) = delete; FlatGrid& operator=(const FlatGrid&) = delete; // Returns the number of voxels per dimension. static int grid_size() { return 1 << kBits; } // Returns the value stored at 'index', each dimension of 'index' being // between 0 and grid_size() - 1. ValueType value(const Eigen::Array3i& index) const { return cells_[ToFlatIndex(index, kBits)]; } // Returns a pointer to a value to allow changing it. ValueType* mutable_value(const Eigen::Array3i& index) { return &cells_[ToFlatIndex(index, kBits)]; } // An iterator for iterating over all values not comparing equal to the // default constructed value. class Iterator { public: Iterator() : current_(nullptr), end_(nullptr) {} explicit Iterator(const FlatGrid& flat_grid) : current_(flat_grid.cells_.data()), end_(flat_grid.cells_.data() + flat_grid.cells_.size()) { while (!Done() && IsDefaultValue(*current_)) { ++current_; } } void Next() { DCHECK(!Done()); do { ++current_; } while (!Done() && IsDefaultValue(*current_)); } bool Done() const { return current_ == end_; } Eigen::Array3i GetCellIndex() const { DCHECK(!Done()); const int index = (1 << (3 * kBits)) - (end_ - current_); return To3DIndex(index, kBits); } const ValueType& GetValue() const { DCHECK(!Done()); return *current_; } private: const ValueType* current_; const ValueType* end_; }; private: std::array cells_; }; // A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type // 'WrappedGrid'. Wrapped grids are constructed on first access via // 'mutable_value()'. template class NestedGrid { public: using ValueType = typename WrappedGrid::ValueType; // Returns the number of voxels per dimension. static int grid_size() { return WrappedGrid::grid_size() << kBits; } // Returns the value stored at 'index', each dimension of 'index' being // between 0 and grid_size() - 1. ValueType value(const Eigen::Array3i& index) const { const Eigen::Array3i meta_index = GetMetaIndex(index); const WrappedGrid* const meta_cell = meta_cells_[ToFlatIndex(meta_index, kBits)].get(); if (meta_cell == nullptr) { return ValueType(); } const Eigen::Array3i inner_index = index - meta_index * WrappedGrid::grid_size(); return meta_cell->value(inner_index); } // Returns a pointer to the value at 'index' to allow changing it. If // necessary a new wrapped grid is constructed to contain that value. ValueType* mutable_value(const Eigen::Array3i& index) { const Eigen::Array3i meta_index = GetMetaIndex(index); std::unique_ptr& meta_cell = meta_cells_[ToFlatIndex(meta_index, kBits)]; if (meta_cell == nullptr) { meta_cell = common::make_unique(); } const Eigen::Array3i inner_index = index - meta_index * WrappedGrid::grid_size(); return meta_cell->mutable_value(inner_index); } // An iterator for iterating over all values not comparing equal to the // default constructed value. class Iterator { public: Iterator() : current_(nullptr), end_(nullptr), nested_iterator_() {} explicit Iterator(const NestedGrid& nested_grid) : current_(nested_grid.meta_cells_.data()), end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()), nested_iterator_() { AdvanceToValidNestedIterator(); } void Next() { DCHECK(!Done()); nested_iterator_.Next(); if (!nested_iterator_.Done()) { return; } ++current_; AdvanceToValidNestedIterator(); } bool Done() const { return current_ == end_; } Eigen::Array3i GetCellIndex() const { DCHECK(!Done()); const int index = (1 << (3 * kBits)) - (end_ - current_); return To3DIndex(index, kBits) * WrappedGrid::grid_size() + nested_iterator_.GetCellIndex(); } const ValueType& GetValue() const { DCHECK(!Done()); return nested_iterator_.GetValue(); } private: void AdvanceToValidNestedIterator() { for (; !Done(); ++current_) { if (*current_ != nullptr) { nested_iterator_ = typename WrappedGrid::Iterator(**current_); if (!nested_iterator_.Done()) { break; } } } } const std::unique_ptr* current_; const std::unique_ptr* end_; typename WrappedGrid::Iterator nested_iterator_; }; private: // Returns the Eigen::Array3i (meta) index of the meta cell containing // 'index'. Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { DCHECK((index >= 0).all()) << index; const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); DCHECK((meta_index < (1 << kBits)).all()) << index; return meta_index; } std::array, 1 << (3 * kBits)> meta_cells_; }; // A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped // grids are constructed on first access via 'mutable_value()'. If necessary, // the grid grows to twice the size in each dimension. The range of indices is // (almost) symmetric around the origin, i.e. negative indices are allowed. template class DynamicGrid { public: using ValueType = typename WrappedGrid::ValueType; DynamicGrid() : bits_(1), meta_cells_(8) {} DynamicGrid(DynamicGrid&&) = default; DynamicGrid& operator=(DynamicGrid&&) = default; // Returns the current number of voxels per dimension. int grid_size() const { return WrappedGrid::grid_size() << bits_; } // Returns the value stored at 'index'. ValueType value(const Eigen::Array3i& index) const { const Eigen::Array3i shifted_index = index + (grid_size() >> 1); // The cast to unsigned is for performance to check with 3 comparisons // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. if ((shifted_index.cast() >= grid_size()).any()) { return ValueType(); } const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); const WrappedGrid* const meta_cell = meta_cells_[ToFlatIndex(meta_index, bits_)].get(); if (meta_cell == nullptr) { return ValueType(); } const Eigen::Array3i inner_index = shifted_index - meta_index * WrappedGrid::grid_size(); return meta_cell->value(inner_index); } // Returns a pointer to the value at 'index' to allow changing it, dynamically // growing the DynamicGrid and constructing new WrappedGrids as needed. ValueType* mutable_value(const Eigen::Array3i& index) { const Eigen::Array3i shifted_index = index + (grid_size() >> 1); // The cast to unsigned is for performance to check with 3 comparisons // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size. if ((shifted_index.cast() >= grid_size()).any()) { Grow(); return mutable_value(index); } const Eigen::Array3i meta_index = GetMetaIndex(shifted_index); std::unique_ptr& meta_cell = meta_cells_[ToFlatIndex(meta_index, bits_)]; if (meta_cell == nullptr) { meta_cell = common::make_unique(); } const Eigen::Array3i inner_index = shifted_index - meta_index * WrappedGrid::grid_size(); return meta_cell->mutable_value(inner_index); } // An iterator for iterating over all values not comparing equal to the // default constructed value. class Iterator { public: explicit Iterator(const DynamicGrid& dynamic_grid) : bits_(dynamic_grid.bits_), current_(dynamic_grid.meta_cells_.data()), end_(dynamic_grid.meta_cells_.data() + dynamic_grid.meta_cells_.size()), nested_iterator_() { AdvanceToValidNestedIterator(); } void Next() { DCHECK(!Done()); nested_iterator_.Next(); if (!nested_iterator_.Done()) { return; } ++current_; AdvanceToValidNestedIterator(); } bool Done() const { return current_ == end_; } Eigen::Array3i GetCellIndex() const { DCHECK(!Done()); const int outer_index = (1 << (3 * bits_)) - (end_ - current_); const Eigen::Array3i shifted_index = To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() + nested_iterator_.GetCellIndex(); return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size()); } const ValueType& GetValue() const { DCHECK(!Done()); return nested_iterator_.GetValue(); } void AdvanceToEnd() { current_ = end_; } const std::pair operator*() const { return std::pair(GetCellIndex(), GetValue()); } Iterator& operator++() { Next(); return *this; } bool operator!=(const Iterator& it) const { return it.current_ != current_; } private: void AdvanceToValidNestedIterator() { for (; !Done(); ++current_) { if (*current_ != nullptr) { nested_iterator_ = typename WrappedGrid::Iterator(**current_); if (!nested_iterator_.Done()) { break; } } } } int bits_; const std::unique_ptr* current_; const std::unique_ptr* const end_; typename WrappedGrid::Iterator nested_iterator_; }; private: // Returns the Eigen::Array3i (meta) index of the meta cell containing // 'index'. Eigen::Array3i GetMetaIndex(const Eigen::Array3i& index) const { DCHECK((index >= 0).all()) << index; const Eigen::Array3i meta_index = index / WrappedGrid::grid_size(); DCHECK((meta_index < (1 << bits_)).all()) << index; return meta_index; } // Grows this grid by a factor of 2 in each of the 3 dimensions. void Grow() { const int new_bits = bits_ + 1; CHECK_LE(new_bits, 8); std::vector> new_meta_cells_( 8 * meta_cells_.size()); for (int z = 0; z != (1 << bits_); ++z) { for (int y = 0; y != (1 << bits_); ++y) { for (int x = 0; x != (1 << bits_); ++x) { const Eigen::Array3i original_meta_index(x, y, z); const Eigen::Array3i new_meta_index = original_meta_index + (1 << (bits_ - 1)); new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] = std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]); } } } meta_cells_ = std::move(new_meta_cells_); bits_ = new_bits; } int bits_; std::vector> meta_cells_; }; template using Grid = DynamicGrid, 3>>; // Represents a 3D grid as a wide, shallow tree. template class HybridGridBase : public Grid { public: using Iterator = typename Grid::Iterator; // Creates a new tree-based probability grid with voxels having edge length // 'resolution' around the origin which becomes the center of the cell at // index (0, 0, 0). explicit HybridGridBase(const float resolution) : resolution_(resolution) {} float resolution() const { return resolution_; } // Returns the index of the cell containing the 'point'. Indices are integer // vectors identifying cells, for this the coordinates are rounded to the next // multiple of the resolution. Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const { Eigen::Array3f index = point.array() / resolution_; return Eigen::Array3i(common::RoundToInt(index.x()), common::RoundToInt(index.y()), common::RoundToInt(index.z())); } // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1). static Eigen::Array3i GetOctant(const int i) { DCHECK_GE(i, 0); DCHECK_LT(i, 8); return Eigen::Array3i(static_cast(i & 1), static_cast(i & 2), static_cast(i & 4)); } // Returns the center of the cell at 'index'. Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const { return index.matrix().cast() * resolution_; } // Iterator functions for range-based for loops. Iterator begin() const { return Iterator(*this); } Iterator end() const { Iterator it(*this); it.AdvanceToEnd(); return it; } private: // Edge length of each voxel. const float resolution_; }; // A grid containing probability values stored using 15 bits, and an update // marker per voxel. class HybridGrid : public HybridGridBase { public: explicit HybridGrid(const float resolution) : HybridGridBase(resolution) {} explicit HybridGrid(const proto::HybridGrid& proto) : HybridGrid(proto.resolution()) { CHECK_EQ(proto.values_size(), proto.x_indices_size()); CHECK_EQ(proto.values_size(), proto.y_indices_size()); CHECK_EQ(proto.values_size(), proto.z_indices_size()); for (int i = 0; i < proto.values_size(); ++i) { // SetProbability does some error checking for us. SetProbability(Eigen::Vector3i(proto.x_indices(i), proto.y_indices(i), proto.z_indices(i)), mapping::ValueToProbability(proto.values(i))); } } // Sets the probability of the cell at 'index' to the given 'probability'. void SetProbability(const Eigen::Array3i& index, const float probability) { *mutable_value(index) = mapping::ProbabilityToValue(probability); } // Starts the next update sequence. void StartUpdate() { while (!update_indices_.empty()) { DCHECK_GE(*update_indices_.back(), mapping::kUpdateMarker); *update_indices_.back() -= mapping::kUpdateMarker; update_indices_.pop_back(); } } // Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds() // to the probability of the cell at 'index' if the cell has not already been // updated. Multiple updates of the same cell will be ignored until // StartUpdate() is called. Returns true if the cell was updated. // // If this is the first call to ApplyOdds() for the specified cell, its value // will be set to probability corresponding to 'odds'. bool ApplyLookupTable(const Eigen::Array3i& index, const std::vector& table) { DCHECK_EQ(table.size(), mapping::kUpdateMarker); uint16* const cell = mutable_value(index); if (*cell >= mapping::kUpdateMarker) { return false; } update_indices_.push_back(cell); *cell = table[*cell]; DCHECK_GE(*cell, mapping::kUpdateMarker); return true; } // Returns the probability of the cell with 'index'. float GetProbability(const Eigen::Array3i& index) const { return mapping::ValueToProbability(value(index)); } // Returns true if the probability at the specified 'index' is known. bool IsKnown(const Eigen::Array3i& index) const { return value(index) != 0; } private: // Markers at changed cells. std::vector update_indices_; }; inline proto::HybridGrid ToProto(const HybridGrid& grid) { proto::HybridGrid result; result.set_resolution(grid.resolution()); for (const auto it : grid) { result.add_x_indices(it.first.x()); result.add_y_indices(it.first.y()); result.add_z_indices(it.first.z()); result.add_values(it.second); } return result; } } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_ ================================================ FILE: mapping_3d/hybrid_grid_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/hybrid_grid.h" #include #include #include #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace { TEST(HybridGridTest, ApplyOdds) { HybridGrid hybrid_grid(1.f); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 0))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 0))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 1))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 1))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 1))); EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 1))); hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f); hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 0, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f); hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f); hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(0, 1, 0), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f))); EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f); // Tests adding odds to an unknown cell. hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); // Tests that further updates are ignored if StartUpdate() isn't called. hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4); hybrid_grid.StartUpdate(); hybrid_grid.ApplyLookupTable( Eigen::Array3i(1, 1, 1), mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f))); EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f); } TEST(HybridGridTest, GetProbability) { HybridGrid hybrid_grid(1.f); hybrid_grid.SetProbability( hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)), mapping::kMaxProbability); EXPECT_NEAR(hybrid_grid.GetProbability( hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))), mapping::kMaxProbability, 1e-6); for (const Eigen::Array3i& index : {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)), hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)), hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) { EXPECT_FALSE(hybrid_grid.IsKnown(index)); } } MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); } TEST(HybridGridTest, GetCellIndex) { HybridGrid hybrid_grid(2.f); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 0.f, 0.f)), AllCwiseEqual(Eigen::Array3i(0, 0, 0))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 26.f, 10.f)), AllCwiseEqual(Eigen::Array3i(0, 13, 5))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 0.f, 10.f)), AllCwiseEqual(Eigen::Array3i(7, 0, 5))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 26.f, 0.f)), AllCwiseEqual(Eigen::Array3i(7, 13, 0))); // Check around the origin. EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(8.5f, 11.5f, 0.5f)), AllCwiseEqual(Eigen::Array3i(4, 6, 0))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.5f, 12.5f, 1.5f)), AllCwiseEqual(Eigen::Array3i(4, 6, 1))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(6.5f, 14.5f, 2.5f)), AllCwiseEqual(Eigen::Array3i(3, 7, 1))); EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(5.5f, 13.5f, 3.5f)), AllCwiseEqual(Eigen::Array3i(3, 7, 2))); } TEST(HybridGridTest, GetCenterOfCell) { HybridGrid hybrid_grid(2.f); const Eigen::Array3i index(3, 2, 1); const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index); EXPECT_NEAR(6.f, center.x(), 1e-6); EXPECT_NEAR(4.f, center.y(), 1e-6); EXPECT_NEAR(2.f, center.z(), 1e-6); EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index)); } class RandomHybridGridTest : public ::testing::Test { public: RandomHybridGridTest() : hybrid_grid_(2.f), values_() { std::mt19937 rng(1285120005); std::uniform_real_distribution value_distribution( mapping::kMinProbability, mapping::kMaxProbability); std::uniform_int_distribution xyz_distribution(-3000, 2999); for (int i = 0; i < 10000; ++i) { const auto x = xyz_distribution(rng); const auto y = xyz_distribution(rng); const auto z = xyz_distribution(rng); values_.emplace(std::make_tuple(x, y, z), value_distribution(rng)); } for (const auto& pair : values_) { const Eigen::Array3i cell_index(std::get<0>(pair.first), std::get<1>(pair.first), std::get<2>(pair.first)); hybrid_grid_.SetProbability(cell_index, pair.second); } } protected: HybridGrid hybrid_grid_; using ValueMap = std::map, float>; ValueMap values_; }; TEST_F(RandomHybridGridTest, TestIteration) { for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) { const Eigen::Array3i cell_index = it.GetCellIndex(); const float iterator_probability = mapping::ValueToProbability(it.GetValue()); EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index)); const std::tuple key = std::make_tuple(cell_index[0], cell_index[1], cell_index[2]); EXPECT_TRUE(values_.count(key)); EXPECT_NEAR(values_[key], iterator_probability, 1e-4); values_.erase(key); } // Test that range based loop is equivalent to using the iterator. auto it = HybridGrid::Iterator(hybrid_grid_); for (const auto& cell : hybrid_grid_) { ASSERT_FALSE(it.Done()); EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex())); EXPECT_EQ(cell.second, it.GetValue()); it.Next(); } // Now 'values_' must not contain values. for (const auto& pair : values_) { const Eigen::Array3i cell_index(std::get<0>(pair.first), std::get<1>(pair.first), std::get<2>(pair.first)); ADD_FAILURE() << cell_index << " Probability: " << pair.second; } } TEST_F(RandomHybridGridTest, ToProto) { const auto proto = ToProto(hybrid_grid_); EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution()); ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size()); ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size()); ASSERT_EQ(proto.x_indices_size(), proto.values_size()); ValueMap proto_map; for (int i = 0; i < proto.x_indices_size(); ++i) { proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i), proto.z_indices(i))] = proto.values(i); } // Get hybrid_grid_ into the same format. ValueMap hybrid_grid_map; for (const auto i : hybrid_grid_) { hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] = i.second; } EXPECT_EQ(proto_map, hybrid_grid_map); } namespace { struct EigenComparator { bool operator()(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) { return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) < std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z()); } }; } // namespace TEST_F(RandomHybridGridTest, FromProto) { const HybridGrid constructed_grid(ToProto(hybrid_grid_)); std::map member_map( hybrid_grid_.begin(), hybrid_grid_.end()); std::map constructed_map( constructed_grid.begin(), constructed_grid.end()); EXPECT_EQ(member_map, constructed_map); } } // namespace } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/imu_integration.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/imu_integration.h" namespace cartographer { namespace mapping_3d { IntegrateImuResult IntegrateImu( const std::deque& imu_data, const common::Time start_time, const common::Time end_time, std::deque::const_iterator* it) { return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(), start_time, end_time, it); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/imu_integration.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ #define CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/time.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { struct ImuData { common::Time time; Eigen::Vector3d linear_acceleration; Eigen::Vector3d angular_velocity; }; template struct IntegrateImuResult { Eigen::Matrix delta_velocity; Eigen::Quaternion delta_rotation; }; // Returns velocity delta in map frame. IntegrateImuResult IntegrateImu( const std::deque& imu_data, const common::Time start_time, const common::Time end_time, std::deque::const_iterator* it); template IntegrateImuResult IntegrateImu( const std::deque& imu_data, const Eigen::Transform& linear_acceleration_calibration, const Eigen::Transform& angular_velocity_calibration, const common::Time start_time, const common::Time end_time, std::deque::const_iterator* it) { CHECK_LE(start_time, end_time); CHECK(*it != imu_data.cend()); CHECK_LE((*it)->time, start_time); if ((*it) + 1 != imu_data.cend()) { CHECK_GT(((*it) + 1)->time, start_time); } common::Time current_time = start_time; IntegrateImuResult result = {Eigen::Matrix::Zero(), Eigen::Quaterniond::Identity().cast()}; while (current_time < end_time) { common::Time next_imu_data = common::Time::max(); if ((*it) + 1 != imu_data.cend()) { next_imu_data = ((*it) + 1)->time; } common::Time next_time = std::min(next_imu_data, end_time); const T delta_t(common::ToSeconds(next_time - current_time)); const Eigen::Matrix delta_angle = (angular_velocity_calibration * (*it)->angular_velocity.cast()) * delta_t; result.delta_rotation *= transform::AngleAxisVectorToRotationQuaternion(delta_angle); result.delta_velocity += result.delta_rotation * ((linear_acceleration_calibration * (*it)->linear_acceleration.cast()) * delta_t); current_time = next_time; if (current_time == next_imu_data) { ++(*it); } } return result; } } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_IMU_INTEGRATION_H_ ================================================ FILE: mapping_3d/kalman_local_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/kalman_filter/proto/pose_tracker_options.pb.h" #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/proto/submaps_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { KalmanLocalTrajectoryBuilder::KalmanLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), submaps_(common::make_unique(options.submaps_options())), scan_matcher_pose_estimate_(transform::Rigid3d::Identity()), motion_filter_(options.motion_filter_options()), real_time_correlative_scan_matcher_( common::make_unique( options_.kalman_local_trajectory_builder_options() .real_time_correlative_scan_matcher_options())), ceres_scan_matcher_(common::make_unique( options_.ceres_scan_matcher_options())), num_accumulated_(0), first_pose_prediction_(transform::Rigid3f::Identity()), accumulated_range_data_{Eigen::Vector3f::Zero(), {}, {}} {} KalmanLocalTrajectoryBuilder::~KalmanLocalTrajectoryBuilder() {} const mapping_3d::Submaps* KalmanLocalTrajectoryBuilder::submaps() const { return submaps_.get(); } void KalmanLocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { if (!pose_tracker_) { pose_tracker_ = common::make_unique( options_.kalman_local_trajectory_builder_options() .pose_tracker_options(), time); } pose_tracker_->AddImuLinearAccelerationObservation(time, linear_acceleration); pose_tracker_->AddImuAngularVelocityObservation(time, angular_velocity); transform::Rigid3d pose_estimate; kalman_filter::PoseCovariance unused_covariance_estimate; pose_tracker_->GetPoseEstimateMeanAndCovariance(time, &pose_estimate, &unused_covariance_estimate); } std::unique_ptr KalmanLocalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { if (!pose_tracker_) { LOG(INFO) << "PoseTracker not yet initialized."; return nullptr; } transform::Rigid3d pose_prediction; kalman_filter::PoseCovariance unused_covariance_prediction; pose_tracker_->GetPoseEstimateMeanAndCovariance( time, &pose_prediction, &unused_covariance_prediction); if (num_accumulated_ == 0) { first_pose_prediction_ = pose_prediction.cast(); accumulated_range_data_ = sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}; } const transform::Rigid3f tracking_delta = first_pose_prediction_.inverse() * pose_prediction.cast(); const sensor::RangeData range_data_in_first_tracking = sensor::TransformRangeData(sensor::RangeData{origin, ranges, {}}, tracking_delta); for (const Eigen::Vector3f& hit : range_data_in_first_tracking.returns) { const Eigen::Vector3f delta = hit - range_data_in_first_tracking.origin; const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { accumulated_range_data_.returns.push_back(hit); } else { // We insert a ray cropped to 'max_range' as a miss for hits beyond the // maximum range. This way the free space up to the maximum range will // be updated. accumulated_range_data_.misses.push_back( range_data_in_first_tracking.origin + options_.max_range() / range * delta); } } } ++num_accumulated_; if (num_accumulated_ >= options_.scans_per_accumulation()) { num_accumulated_ = 0; return AddAccumulatedRangeData( time, sensor::TransformRangeData(accumulated_range_data_, tracking_delta.inverse())); } return nullptr; } std::unique_ptr KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const sensor::RangeData& range_data_in_tracking) { const sensor::RangeData filtered_range_data = { range_data_in_tracking.origin, sensor::VoxelFiltered(range_data_in_tracking.returns, options_.voxel_filter_size()), sensor::VoxelFiltered(range_data_in_tracking.misses, options_.voxel_filter_size())}; if (filtered_range_data.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; } transform::Rigid3d pose_prediction; kalman_filter::PoseCovariance unused_covariance_prediction; pose_tracker_->GetPoseEstimateMeanAndCovariance( time, &pose_prediction, &unused_covariance_prediction); const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); transform::Rigid3d initial_ceres_pose = matching_submap->local_pose.inverse() * pose_prediction; sensor::AdaptiveVoxelFilter adaptive_voxel_filter( options_.high_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud filtered_point_cloud_in_tracking = adaptive_voxel_filter.Filter(filtered_range_data.returns); if (options_.kalman_local_trajectory_builder_options() .use_online_correlative_scan_matching()) { // We take a copy since we use 'intial_ceres_pose' as an output argument. const transform::Rigid3d initial_pose = initial_ceres_pose; real_time_correlative_scan_matcher_->Match( initial_pose, filtered_point_cloud_in_tracking, matching_submap->high_resolution_hybrid_grid, &initial_ceres_pose); } transform::Rigid3d pose_observation_in_submap; ceres::Solver::Summary summary; sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( options_.low_resolution_adaptive_voxel_filter_options()); const sensor::PointCloud low_resolution_point_cloud_in_tracking = low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns); ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose, {{&filtered_point_cloud_in_tracking, &matching_submap->high_resolution_hybrid_grid}, {&low_resolution_point_cloud_in_tracking, &matching_submap->low_resolution_hybrid_grid}}, &pose_observation_in_submap, &summary); const transform::Rigid3d pose_observation = matching_submap->local_pose * pose_observation_in_submap; pose_tracker_->AddPoseObservation( time, pose_observation, options_.kalman_local_trajectory_builder_options() .scan_matcher_variance() * kalman_filter::PoseCovariance::Identity()); kalman_filter::PoseCovariance unused_covariance_estimate; pose_tracker_->GetPoseEstimateMeanAndCovariance( time, &scan_matcher_pose_estimate_, &unused_covariance_estimate); last_pose_estimate_ = { time, scan_matcher_pose_estimate_, sensor::TransformPointCloud(filtered_range_data.returns, pose_observation.cast())}; return InsertIntoSubmap(time, filtered_range_data, pose_observation); } void KalmanLocalTrajectoryBuilder::AddOdometerData( const common::Time time, const transform::Rigid3d& pose) { if (!pose_tracker_) { pose_tracker_.reset(new kalman_filter::PoseTracker( options_.kalman_local_trajectory_builder_options() .pose_tracker_options(), time)); } pose_tracker_->AddOdometerPoseObservation( time, pose, kalman_filter::BuildPoseCovariance( options_.kalman_local_trajectory_builder_options() .odometer_translational_variance(), options_.kalman_local_trajectory_builder_options() .odometer_rotational_variance())); } const KalmanLocalTrajectoryBuilder::PoseEstimate& KalmanLocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } std::unique_ptr KalmanLocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); std::vector insertion_submaps; for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } submaps_->InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), pose_tracker_->gravity_orientation()); return std::unique_ptr( new InsertionResult{time, range_data_in_tracking, pose_observation, matching_submap, insertion_submaps}); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/kalman_local_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ #include #include "cartographer/common/time.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_3d/local_trajectory_builder_interface.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping_3d { // Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop // closure. class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { public: explicit KalmanLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); ~KalmanLocalTrajectoryBuilder() override; KalmanLocalTrajectoryBuilder(const KalmanLocalTrajectoryBuilder&) = delete; KalmanLocalTrajectoryBuilder& operator=(const KalmanLocalTrajectoryBuilder&) = delete; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; private: std::unique_ptr AddAccumulatedRangeData( common::Time time, const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation); const proto::LocalTrajectoryBuilderOptions options_; std::unique_ptr submaps_; PoseEstimate last_pose_estimate_; // Pose of the last computed scan match. transform::Rigid3d scan_matcher_pose_estimate_; MotionFilter motion_filter_; std::unique_ptr real_time_correlative_scan_matcher_; std::unique_ptr ceres_scan_matcher_; std::unique_ptr pose_tracker_; int num_accumulated_; transform::Rigid3f first_pose_prediction_; sensor::RangeData accumulated_range_data_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_3d/kalman_local_trajectory_builder_options.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" namespace cartographer { namespace mapping_3d { proto::KalmanLocalTrajectoryBuilderOptions CreateKalmanLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::KalmanLocalTrajectoryBuilderOptions options; options.set_use_online_correlative_scan_matching( parameter_dictionary->GetBool("use_online_correlative_scan_matching")); *options.mutable_real_time_correlative_scan_matcher_options() = mapping_2d::scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary ->GetDictionary("real_time_correlative_scan_matcher") .get()); *options.mutable_pose_tracker_options() = kalman_filter::CreatePoseTrackerOptions( parameter_dictionary->GetDictionary("pose_tracker").get()); options.set_scan_matcher_variance( parameter_dictionary->GetDouble("scan_matcher_variance")); options.set_odometer_translational_variance( parameter_dictionary->GetDouble("odometer_translational_variance")); options.set_odometer_rotational_variance( parameter_dictionary->GetDouble("odometer_rotational_variance")); return options; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/kalman_local_trajectory_builder_options.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_3d { proto::KalmanLocalTrajectoryBuilderOptions CreateKalmanLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ ================================================ FILE: mapping_3d/kalman_local_trajectory_builder_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" #include #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace { class KalmanLocalTrajectoryBuilderTest : public ::testing::Test { protected: struct TrajectoryNode { common::Time time; transform::Rigid3d pose; }; void SetUp() override { GenerateBubbles(); } proto::LocalTrajectoryBuilderOptions CreateTrajectoryBuilderOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { min_range = 0.5, max_range = 50., scans_per_accumulation = 1, voxel_filter_size = 0.05, high_resolution_adaptive_voxel_filter = { max_length = 0.7, min_num_points = 200, max_range = 50., }, low_resolution_adaptive_voxel_filter = { max_length = 0.7, min_num_points = 200, max_range = 50., }, ceres_scan_matcher = { occupied_space_weight_0 = 5., occupied_space_weight_1 = 20., translation_weight = 0.1, rotation_weight = 0.3, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 20, num_threads = 1, }, }, motion_filter = { max_time_seconds = 0.2, max_distance_meters = 0.02, max_angle_radians = 0.001, }, submaps = { high_resolution = 0.2, high_resolution_max_range = 50., low_resolution = 0.5, num_range_data = 45000, range_data_inserter = { hit_probability = 0.7, miss_probability = 0.4, num_free_space_voxels = 0, }, }, use = "KALMAN", kalman_local_trajectory_builder = { use_online_correlative_scan_matching = false, real_time_correlative_scan_matcher = { linear_search_window = 0.2, angular_search_window = math.rad(1.), translation_delta_cost_weight = 1e-1, rotation_delta_cost_weight = 1., }, pose_tracker = { orientation_model_variance = 5e-4, position_model_variance = 0.000654766, velocity_model_variance = 0.053926, imu_gravity_time_constant = 1., imu_gravity_variance = 1e-4, num_odometry_states = 1, }, scan_matcher_variance = 1e-6, odometer_translational_variance = 1e-7, odometer_rotational_variance = 1e-7, }, optimizing_local_trajectory_builder = { high_resolution_grid_weight = 5., low_resolution_grid_weight = 15., velocity_weight = 4e1, translation_weight = 1e2, rotation_weight = 1e3, odometry_translation_weight = 1e4, odometry_rotation_weight = 1e4, }, } )text"); return CreateLocalTrajectoryBuilderOptions(parameter_dictionary.get()); } void GenerateBubbles() { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); for (int i = 0; i != 100; ++i) { const float x = distribution(prng); const float y = distribution(prng); const float z = distribution(prng); bubbles_.push_back(10. * Eigen::Vector3f(x, y, z).normalized()); } } // Computes the earliest intersection of the ray 'from' to 'to' with the // axis-aligned cube with edge length 30 and center at the origin. Assumes // that 'from' is inside the cube. Eigen::Vector3f CollideWithBox(const Eigen::Vector3f& from, const Eigen::Vector3f& to) { float first = 100.f; if (to.x() > from.x()) { first = std::min(first, (15.f - from.x()) / (to.x() - from.x())); } else if (to.x() < from.x()) { first = std::min(first, (-15.f - from.x()) / (to.x() - from.x())); } if (to.y() > from.y()) { first = std::min(first, (15.f - from.y()) / (to.y() - from.y())); } else if (to.y() < from.y()) { first = std::min(first, (-15.f - from.y()) / (to.y() - from.y())); } if (to.z() > from.z()) { first = std::min(first, (15.f - from.z()) / (to.z() - from.z())); } else if (to.z() < from.z()) { first = std::min(first, (-15.f - from.z()) / (to.z() - from.z())); } return first * (to - from) + from; } // Computes the earliest intersection of the ray 'from' to 'to' with all // bubbles. Returns 'to' if no intersection exists. Eigen::Vector3f CollideWithBubbles(const Eigen::Vector3f& from, const Eigen::Vector3f& to) { float first = 1.f; constexpr float kBubbleRadius = 0.5f; for (const Eigen::Vector3f& center : bubbles_) { const float a = (to - from).squaredNorm(); const float beta = (to - from).dot(from - center); const float c = (from - center).squaredNorm() - kBubbleRadius * kBubbleRadius; const float discriminant = beta * beta - a * c; if (discriminant < 0.f) { continue; } const float solution = (-beta - std::sqrt(discriminant)) / a; if (solution < 0.f) { continue; } first = std::min(first, solution); } return first * (to - from) + from; } sensor::RangeData GenerateRangeData(const transform::Rigid3d& pose) { // 360 degree rays at 16 angles. sensor::PointCloud directions_in_rangefinder_frame; for (int r = -8; r != 8; ++r) { for (int s = -250; s != 250; ++s) { directions_in_rangefinder_frame.push_back( Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * Eigen::Vector3f::UnitX()); // Second orthogonal rangefinder. directions_in_rangefinder_frame.push_back( Eigen::AngleAxisf(M_PI / 2., Eigen::Vector3f::UnitX()) * Eigen::AngleAxisf(M_PI * s / 250., Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(M_PI / 12. * r / 8., Eigen::Vector3f::UnitY()) * Eigen::Vector3f::UnitX()); } } // We simulate a 30 m edge length box around the origin, also containing // 50 cm radius spheres. sensor::PointCloud returns_in_world_frame; for (const Eigen::Vector3f& direction_in_world_frame : sensor::TransformPointCloud(directions_in_rangefinder_frame, pose.cast())) { const Eigen::Vector3f origin = pose.cast() * Eigen::Vector3f::Zero(); returns_in_world_frame.push_back(CollideWithBubbles( origin, CollideWithBox(origin, direction_in_world_frame))); } return {Eigen::Vector3f::Zero(), sensor::TransformPointCloud(returns_in_world_frame, pose.inverse().cast()), {}}; } void AddLinearOnlyImuObservation(const common::Time time, const transform::Rigid3d& expected_pose) { const Eigen::Vector3d gravity = expected_pose.rotation().inverse() * Eigen::Vector3d(0., 0., 9.81); local_trajectory_builder_->AddImuData(time, gravity, Eigen::Vector3d::Zero()); } std::vector GenerateCorkscrewTrajectory() { std::vector trajectory; common::Time current_time = common::FromUniversal(12345678); // One second at zero velocity. for (int i = 0; i != 5; ++i) { current_time += common::FromSeconds(0.3); trajectory.push_back( TrajectoryNode{current_time, transform::Rigid3d::Identity()}); } // Corkscrew translation and constant velocity rotation. for (double t = 0.; t <= 0.6; t += 0.006) { current_time += common::FromSeconds(0.3); trajectory.push_back(TrajectoryNode{ current_time, transform::Rigid3d::Translation(Eigen::Vector3d( std::sin(4. * t), 1. - std::cos(4. * t), 1. * t)) * transform::Rigid3d::Rotation(Eigen::AngleAxisd( 0.3 * t, Eigen::Vector3d(1., -1., 2.).normalized()))}); } return trajectory; } void VerifyAccuracy(const std::vector& expected_trajectory, double expected_accuracy) { int num_poses = 0; for (const TrajectoryNode& node : expected_trajectory) { AddLinearOnlyImuObservation(node.time, node.pose); const auto range_data = GenerateRangeData(node.pose); if (local_trajectory_builder_->AddRangefinderData( node.time, range_data.origin, range_data.returns) != nullptr) { const auto pose_estimate = local_trajectory_builder_->pose_estimate(); EXPECT_THAT(pose_estimate.pose, transform::IsNearly(node.pose, 1e-1)); ++num_poses; LOG(INFO) << "num_poses: " << num_poses; } } } std::unique_ptr local_trajectory_builder_; std::vector bubbles_; }; TEST_F(KalmanLocalTrajectoryBuilderTest, MoveInsideCubeUsingOnlyCeresScanMatcher) { local_trajectory_builder_.reset( new KalmanLocalTrajectoryBuilder(CreateTrajectoryBuilderOptions())); VerifyAccuracy(GenerateCorkscrewTrajectory(), 1e-1); } } // namespace } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/local_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/local_trajectory_builder.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" #include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h" namespace cartographer { namespace mapping_3d { std::unique_ptr CreateLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& local_trajectory_builder_options) { switch (local_trajectory_builder_options.use()) { case proto::LocalTrajectoryBuilderOptions::KALMAN: return common::make_unique( local_trajectory_builder_options); case proto::LocalTrajectoryBuilderOptions::OPTIMIZING: return common::make_unique( local_trajectory_builder_options); } LOG(FATAL); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/local_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ #include #include #include "cartographer/mapping_3d/local_trajectory_builder_interface.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_3d { std::unique_ptr CreateLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& local_trajectory_builder_options); } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_3d/local_trajectory_builder_interface.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ #include #include #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { class LocalTrajectoryBuilderInterface { public: using PoseEstimate = mapping::GlobalTrajectoryBuilderInterface::PoseEstimate; struct InsertionResult { common::Time time; sensor::RangeData range_data_in_tracking; transform::Rigid3d pose_observation; const Submap* matching_submap; std::vector insertion_submaps; }; virtual ~LocalTrajectoryBuilderInterface() {} LocalTrajectoryBuilderInterface(const LocalTrajectoryBuilderInterface&) = delete; LocalTrajectoryBuilderInterface& operator=( const LocalTrajectoryBuilderInterface&) = delete; virtual void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) = 0; virtual std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) = 0; virtual void AddOdometerData(common::Time time, const transform::Rigid3d& pose) = 0; virtual const mapping_3d::Submaps* submaps() const = 0; virtual const PoseEstimate& pose_estimate() const = 0; protected: LocalTrajectoryBuilderInterface() {} }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_INTERFACE_H_ ================================================ FILE: mapping_3d/local_trajectory_builder_options.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/kalman_local_trajectory_builder_options.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/voxel_filter.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::LocalTrajectoryBuilderOptions options; options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_scans_per_accumulation( parameter_dictionary->GetInt("scans_per_accumulation")); options.set_voxel_filter_size( parameter_dictionary->GetDouble("voxel_filter_size")); *options.mutable_high_resolution_adaptive_voxel_filter_options() = sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary ->GetDictionary("high_resolution_adaptive_voxel_filter") .get()); *options.mutable_low_resolution_adaptive_voxel_filter_options() = sensor::CreateAdaptiveVoxelFilterOptions( parameter_dictionary ->GetDictionary("low_resolution_adaptive_voxel_filter") .get()); *options.mutable_ceres_scan_matcher_options() = scan_matching::CreateCeresScanMatcherOptions( parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); *options.mutable_motion_filter_options() = CreateMotionFilterOptions( parameter_dictionary->GetDictionary("motion_filter").get()); *options.mutable_submaps_options() = mapping_3d::CreateSubmapsOptions( parameter_dictionary->GetDictionary("submaps").get()); *options.mutable_kalman_local_trajectory_builder_options() = CreateKalmanLocalTrajectoryBuilderOptions( parameter_dictionary->GetDictionary("kalman_local_trajectory_builder") .get()); *options.mutable_optimizing_local_trajectory_builder_options() = CreateOptimizingLocalTrajectoryBuilderOptions( parameter_dictionary ->GetDictionary("optimizing_local_trajectory_builder") .get()); const string use_string = parameter_dictionary->GetString("use"); proto::LocalTrajectoryBuilderOptions::Use use; CHECK(proto::LocalTrajectoryBuilderOptions::Use_Parse(use_string, &use)) << "Unknown local_trajectory_builder kind: " << use_string; options.set_use(use); return options; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/local_trajectory_builder_options.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_3d { proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ ================================================ FILE: mapping_3d/motion_filter.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { proto::MotionFilterOptions CreateMotionFilterOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::MotionFilterOptions options; options.set_max_time_seconds( parameter_dictionary->GetDouble("max_time_seconds")); options.set_max_distance_meters( parameter_dictionary->GetDouble("max_distance_meters")); options.set_max_angle_radians( parameter_dictionary->GetDouble("max_angle_radians")); return options; } MotionFilter::MotionFilter(const proto::MotionFilterOptions& options) : options_(options) {} bool MotionFilter::IsSimilar(const common::Time time, const transform::Rigid3d& pose) { LOG_IF_EVERY_N(INFO, num_total_ >= 500, 500) << "Motion filter reduced the number of scans to " << 100. * num_different_ / num_total_ << "%."; ++num_total_; if (num_total_ > 1 && time - last_time_ <= common::FromSeconds(options_.max_time_seconds()) && (pose.translation() - last_pose_.translation()).norm() <= options_.max_distance_meters() && transform::GetAngle(pose.inverse() * last_pose_) <= options_.max_angle_radians()) { return true; } last_time_ = time; last_pose_ = pose; ++num_different_; return false; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/motion_filter.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ #define CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ #include #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/time.h" #include "cartographer/mapping_3d/proto/motion_filter_options.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { proto::MotionFilterOptions CreateMotionFilterOptions( common::LuaParameterDictionary* parameter_dictionary); // Takes poses as input and filters them to get fewer poses. class MotionFilter { public: explicit MotionFilter(const proto::MotionFilterOptions& options); // If the accumulated motion (linear, rotational, or time) is above the // threshold, returns false. Otherwise the relative motion is accumulated and // true is returned. bool IsSimilar(common::Time time, const transform::Rigid3d& pose); private: const proto::MotionFilterOptions options_; int num_total_ = 0; int num_different_ = 0; common::Time last_time_; transform::Rigid3d last_pose_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_MOTION_FILTER_H_ ================================================ FILE: mapping_3d/motion_filter_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace { class MotionFilterTest : public ::testing::Test { protected: MotionFilterTest() { auto parameter_dictionary = common::MakeDictionary( "return {" "max_time_seconds = 0.5, " "max_distance_meters = 0.2, " "max_angle_radians = 2., " "}"); options_ = CreateMotionFilterOptions(parameter_dictionary.get()); } common::Time SecondsSinceEpoch(int seconds) { return common::FromUniversal(seconds * 10000000); } proto::MotionFilterOptions options_; }; TEST_F(MotionFilterTest, NotInitialized) { MotionFilter motion_filter(options_); EXPECT_FALSE( motion_filter.IsSimilar(common::Time(), transform::Rigid3d::Identity())); } TEST_F(MotionFilterTest, NoChange) { MotionFilter motion_filter(options_); EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); } TEST_F(MotionFilterTest, TimeElapsed) { MotionFilter motion_filter(options_); EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(43), transform::Rigid3d::Identity())); EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(43), transform::Rigid3d::Identity())); } TEST_F(MotionFilterTest, LinearMotion) { MotionFilter motion_filter(options_); EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); EXPECT_FALSE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Translation(Eigen::Vector3d(0.3, 0., 0.)))); EXPECT_TRUE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Translation(Eigen::Vector3d(0.45, 0., 0.)))); EXPECT_FALSE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0., 0.)))); EXPECT_TRUE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Translation(Eigen::Vector3d(0.6, 0.15, 0.)))); } TEST_F(MotionFilterTest, RotationalMotion) { MotionFilter motion_filter(options_); EXPECT_FALSE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); EXPECT_TRUE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( 1.9, Eigen::Vector3d::UnitY())))); EXPECT_FALSE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( 2.1, Eigen::Vector3d::UnitY())))); EXPECT_TRUE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( 4., Eigen::Vector3d::UnitY())))); EXPECT_FALSE(motion_filter.IsSimilar( SecondsSinceEpoch(42), transform::Rigid3d::Rotation(Eigen::AngleAxisd( 5.9, Eigen::Vector3d::UnitY())))); EXPECT_TRUE(motion_filter.IsSimilar(SecondsSinceEpoch(42), transform::Rigid3d::Identity())); } } // namespace } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/optimizing_local_trajectory_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/mapping_3d/translation_cost_function.h" #include "cartographer/transform/transform.h" #include "cartographer/transform/transform_interpolation_buffer.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace { // Computes the cost of differences between two velocities. For the constant // velocity model the residuals are just the vector difference. class VelocityDeltaCostFunctor { public: explicit VelocityDeltaCostFunctor(const double scaling_factor) : scaling_factor_(scaling_factor) {} VelocityDeltaCostFunctor(const VelocityDeltaCostFunctor&) = delete; VelocityDeltaCostFunctor& operator=(const VelocityDeltaCostFunctor&) = delete; template bool operator()(const T* const old_velocity, const T* const new_velocity, T* residual) const { residual[0] = scaling_factor_ * (new_velocity[0] - old_velocity[0]); residual[1] = scaling_factor_ * (new_velocity[1] - old_velocity[1]); residual[2] = scaling_factor_ * (new_velocity[2] - old_velocity[2]); return true; } private: const double scaling_factor_; }; class RelativeTranslationAndYawCostFunction { public: RelativeTranslationAndYawCostFunction(const double translation_scaling_factor, const double rotation_scaling_factor, const transform::Rigid3d& delta) : translation_scaling_factor_(translation_scaling_factor), rotation_scaling_factor_(rotation_scaling_factor), delta_(delta) {} RelativeTranslationAndYawCostFunction( const RelativeTranslationAndYawCostFunction&) = delete; RelativeTranslationAndYawCostFunction& operator=( const RelativeTranslationAndYawCostFunction&) = delete; template bool operator()(const T* const start_translation, const T* const start_rotation, const T* const end_translation, const T* const end_rotation, T* residual) const { const transform::Rigid3 start( Eigen::Map>(start_translation), Eigen::Quaternion(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3])); const transform::Rigid3 end( Eigen::Map>(end_translation), Eigen::Quaternion(end_rotation[0], end_rotation[1], end_rotation[2], end_rotation[3])); const transform::Rigid3 delta = end.inverse() * start; const transform::Rigid3 error = delta.inverse() * delta_.cast(); residual[0] = translation_scaling_factor_ * error.translation().x(); residual[1] = translation_scaling_factor_ * error.translation().y(); residual[2] = translation_scaling_factor_ * error.translation().z(); residual[3] = rotation_scaling_factor_ * transform::GetYaw(error); return true; } private: const double translation_scaling_factor_; const double rotation_scaling_factor_; const transform::Rigid3d delta_; }; } // namespace OptimizingLocalTrajectoryBuilder::OptimizingLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), ceres_solver_options_(common::CreateCeresSolverOptions( options.ceres_scan_matcher_options().ceres_solver_options())), submaps_(common::make_unique(options.submaps_options())), num_accumulated_(0), motion_filter_(options.motion_filter_options()) {} OptimizingLocalTrajectoryBuilder::~OptimizingLocalTrajectoryBuilder() {} const mapping_3d::Submaps* OptimizingLocalTrajectoryBuilder::submaps() const { return submaps_.get(); } void OptimizingLocalTrajectoryBuilder::AddImuData( const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { imu_data_.push_back(ImuData{ time, linear_acceleration, angular_velocity, }); RemoveObsoleteSensorData(); } void OptimizingLocalTrajectoryBuilder::AddOdometerData( const common::Time time, const transform::Rigid3d& pose) { odometer_data_.push_back(OdometerData{time, pose}); RemoveObsoleteSensorData(); } std::unique_ptr OptimizingLocalTrajectoryBuilder::AddRangefinderData( const common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) { CHECK_GT(ranges.size(), 0); // TODO(hrapp): Handle misses. // TODO(hrapp): Where are NaNs in range_data_in_tracking coming from? sensor::PointCloud point_cloud; for (const Eigen::Vector3f& hit : ranges) { const Eigen::Vector3f delta = hit - origin; const float range = delta.norm(); if (range >= options_.min_range()) { if (range <= options_.max_range()) { point_cloud.push_back(hit); } } } auto high_resolution_options = options_.high_resolution_adaptive_voxel_filter_options(); high_resolution_options.set_min_num_points( high_resolution_options.min_num_points() / options_.scans_per_accumulation()); sensor::AdaptiveVoxelFilter high_resolution_adaptive_voxel_filter( high_resolution_options); const sensor::PointCloud high_resolution_filtered_points = high_resolution_adaptive_voxel_filter.Filter(point_cloud); auto low_resolution_options = options_.low_resolution_adaptive_voxel_filter_options(); low_resolution_options.set_min_num_points( low_resolution_options.min_num_points() / options_.scans_per_accumulation()); sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter( low_resolution_options); const sensor::PointCloud low_resolution_filtered_points = low_resolution_adaptive_voxel_filter.Filter(point_cloud); if (batches_.empty()) { // First rangefinder data ever. Initialize to the origin. batches_.push_back( Batch{time, point_cloud, high_resolution_filtered_points, low_resolution_filtered_points, State(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity(), Eigen::Vector3d::Zero())}); } else { const Batch& last_batch = batches_.back(); batches_.push_back(Batch{ time, point_cloud, high_resolution_filtered_points, low_resolution_filtered_points, PredictState(last_batch.state, last_batch.time, time), }); } ++num_accumulated_; RemoveObsoleteSensorData(); return MaybeOptimize(time); } void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() { if (imu_data_.empty()) { batches_.clear(); return; } while (batches_.size() > static_cast(options_.scans_per_accumulation())) { batches_.pop_front(); } while (imu_data_.size() > 1 && (batches_.empty() || imu_data_[1].time <= batches_.front().time)) { imu_data_.pop_front(); } while ( odometer_data_.size() > 1 && (batches_.empty() || odometer_data_[1].time <= batches_.front().time)) { odometer_data_.pop_front(); } } void OptimizingLocalTrajectoryBuilder::TransformStates( const transform::Rigid3d& transform) { for (Batch& batch : batches_) { const transform::Rigid3d new_pose = transform * batch.state.ToRigid(); const auto& velocity = batch.state.velocity; const Eigen::Vector3d new_velocity = transform.rotation() * Eigen::Vector3d(velocity[0], velocity[1], velocity[2]); batch.state = State(new_pose.translation(), new_pose.rotation(), new_velocity); } } std::unique_ptr OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) { // TODO(hrapp): Make the number of optimizations configurable. if (num_accumulated_ < options_.scans_per_accumulation() && num_accumulated_ % 10 != 0) { return nullptr; } ceres::Problem problem; const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); // We transform the states in 'batches_' in place to be in the submap frame as // expected by the OccupiedSpaceCostFunctor. This is reverted after solving // the optimization problem. TransformStates(matching_submap->local_pose.inverse()); for (size_t i = 0; i < batches_.size(); ++i) { Batch& batch = batches_[i]; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new scan_matching::OccupiedSpaceCostFunctor( options_.optimizing_local_trajectory_builder_options() .high_resolution_grid_weight() / std::sqrt(static_cast( batch.high_resolution_filtered_points.size())), batch.high_resolution_filtered_points, matching_submap->high_resolution_hybrid_grid), batch.high_resolution_filtered_points.size()), nullptr, batch.state.translation.data(), batch.state.rotation.data()); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new scan_matching::OccupiedSpaceCostFunctor( options_.optimizing_local_trajectory_builder_options() .low_resolution_grid_weight() / std::sqrt(static_cast( batch.low_resolution_filtered_points.size())), batch.low_resolution_filtered_points, matching_submap->low_resolution_hybrid_grid), batch.low_resolution_filtered_points.size()), nullptr, batch.state.translation.data(), batch.state.rotation.data()); if (i == 0) { problem.SetParameterBlockConstant(batch.state.translation.data()); problem.SetParameterBlockConstant(batch.state.rotation.data()); problem.AddParameterBlock(batch.state.velocity.data(), 3); problem.SetParameterBlockConstant(batch.state.velocity.data()); } else { problem.SetParameterization(batch.state.rotation.data(), new ceres::QuaternionParameterization()); } } auto it = imu_data_.cbegin(); for (size_t i = 1; i < batches_.size(); ++i) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new VelocityDeltaCostFunctor( options_.optimizing_local_trajectory_builder_options() .velocity_weight())), nullptr, batches_[i - 1].state.velocity.data(), batches_[i].state.velocity.data()); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationCostFunction( options_.optimizing_local_trajectory_builder_options() .translation_weight(), common::ToSeconds(batches_[i].time - batches_[i - 1].time))), nullptr, batches_[i - 1].state.translation.data(), batches_[i].state.translation.data(), batches_[i - 1].state.velocity.data()); const IntegrateImuResult result = IntegrateImu(imu_data_, batches_[i - 1].time, batches_[i].time, &it); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RotationCostFunction( options_.optimizing_local_trajectory_builder_options() .rotation_weight(), result.delta_rotation)), nullptr, batches_[i - 1].state.rotation.data(), batches_[i].state.rotation.data()); } if (odometer_data_.size() > 1) { transform::TransformInterpolationBuffer interpolation_buffer; for (const auto& odometer_data : odometer_data_) { interpolation_buffer.Push(odometer_data.time, odometer_data.pose); } for (size_t i = 1; i < batches_.size(); ++i) { // Only add constraints for this range data if we have bracketing data // from the odometer. if (!(interpolation_buffer.earliest_time() <= batches_[i - 1].time && batches_[i].time <= interpolation_buffer.latest_time())) { continue; } const transform::Rigid3d previous_odometer_pose = interpolation_buffer.Lookup(batches_[i - 1].time); const transform::Rigid3d current_odometer_pose = interpolation_buffer.Lookup(batches_[i].time); const transform::Rigid3d delta_pose = current_odometer_pose.inverse() * previous_odometer_pose; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RelativeTranslationAndYawCostFunction( options_.optimizing_local_trajectory_builder_options() .odometry_translation_weight(), options_.optimizing_local_trajectory_builder_options() .odometry_rotation_weight(), delta_pose)), nullptr, batches_[i - 1].state.translation.data(), batches_[i - 1].state.rotation.data(), batches_[i].state.translation.data(), batches_[i].state.rotation.data()); } } ceres::Solver::Summary summary; ceres::Solve(ceres_solver_options_, &problem, &summary); // The optimized states in 'batches_' are in the submap frame and we transform // them in place to be in the local SLAM frame again. TransformStates(matching_submap->local_pose); if (num_accumulated_ < options_.scans_per_accumulation()) { return nullptr; } num_accumulated_ = 0; const transform::Rigid3d optimized_pose = batches_.back().state.ToRigid(); sensor::RangeData accumulated_range_data_in_tracking = { Eigen::Vector3f::Zero(), {}, {}}; for (const auto& batch : batches_) { const transform::Rigid3f transform = (optimized_pose.inverse() * batch.state.ToRigid()).cast(); for (const Eigen::Vector3f& point : batch.points) { accumulated_range_data_in_tracking.returns.push_back(transform * point); } } return AddAccumulatedRangeData(time, optimized_pose, accumulated_range_data_in_tracking); } std::unique_ptr OptimizingLocalTrajectoryBuilder::AddAccumulatedRangeData( const common::Time time, const transform::Rigid3d& optimized_pose, const sensor::RangeData& range_data_in_tracking) { const sensor::RangeData filtered_range_data = { range_data_in_tracking.origin, sensor::VoxelFiltered(range_data_in_tracking.returns, options_.voxel_filter_size()), sensor::VoxelFiltered(range_data_in_tracking.misses, options_.voxel_filter_size())}; if (filtered_range_data.returns.empty()) { LOG(WARNING) << "Dropped empty range data."; return nullptr; } last_pose_estimate_ = { time, optimized_pose, sensor::TransformPointCloud(filtered_range_data.returns, optimized_pose.cast())}; return InsertIntoSubmap(time, filtered_range_data, optimized_pose); } const OptimizingLocalTrajectoryBuilder::PoseEstimate& OptimizingLocalTrajectoryBuilder::pose_estimate() const { return last_pose_estimate_; } std::unique_ptr OptimizingLocalTrajectoryBuilder::InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation) { if (motion_filter_.IsSimilar(time, pose_observation)) { return nullptr; } const Submap* const matching_submap = submaps_->Get(submaps_->matching_index()); std::vector insertion_submaps; for (int insertion_index : submaps_->insertion_indices()) { insertion_submaps.push_back(submaps_->Get(insertion_index)); } // TODO(whess): Use an ImuTracker to track the gravity direction. const Eigen::Quaterniond kFakeGravityOrientation = Eigen::Quaterniond::Identity(); submaps_->InsertRangeData( sensor::TransformRangeData(range_data_in_tracking, pose_observation.cast()), kFakeGravityOrientation); return std::unique_ptr( new InsertionResult{time, range_data_in_tracking, pose_observation, matching_submap, insertion_submaps}); } OptimizingLocalTrajectoryBuilder::State OptimizingLocalTrajectoryBuilder::PredictState(const State& start_state, const common::Time start_time, const common::Time end_time) { auto it = --imu_data_.cend(); while (it->time > start_time) { CHECK(it != imu_data_.cbegin()); --it; } const IntegrateImuResult result = IntegrateImu(imu_data_, start_time, end_time, &it); const Eigen::Quaterniond start_rotation( start_state.rotation[0], start_state.rotation[1], start_state.rotation[2], start_state.rotation[3]); const Eigen::Quaterniond orientation = start_rotation * result.delta_rotation; const double delta_time_seconds = common::ToSeconds(end_time - start_time); // TODO(hrapp): IntegrateImu should integration position as well. const Eigen::Vector3d position = Eigen::Map(start_state.translation.data()) + delta_time_seconds * Eigen::Map(start_state.velocity.data()); const Eigen::Vector3d velocity = Eigen::Map(start_state.velocity.data()) + start_rotation * result.delta_velocity - gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ(); return State(position, orientation, velocity); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/optimizing_local_trajectory_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ #include #include #include #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/local_trajectory_builder_interface.h" #include "cartographer/mapping_3d/motion_filter.h" #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/voxel_filter.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { // Batches up some sensor data and optimizes them in one go to get a locally // consistent trajectory. class OptimizingLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface { public: explicit OptimizingLocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options); ~OptimizingLocalTrajectoryBuilder() override; OptimizingLocalTrajectoryBuilder(const OptimizingLocalTrajectoryBuilder&) = delete; OptimizingLocalTrajectoryBuilder& operator=( const OptimizingLocalTrajectoryBuilder&) = delete; void AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) override; std::unique_ptr AddRangefinderData( common::Time time, const Eigen::Vector3f& origin, const sensor::PointCloud& ranges) override; void AddOdometerData(common::Time time, const transform::Rigid3d& pose) override; const mapping_3d::Submaps* submaps() const override; const PoseEstimate& pose_estimate() const override; private: struct State { std::array translation; std::array rotation; // Rotation quaternion as (w, x, y, z). std::array velocity; State(const Eigen::Vector3d& translation, const Eigen::Quaterniond& rotation, const Eigen::Vector3d& velocity) : translation{{translation.x(), translation.y(), translation.z()}}, rotation{{rotation.w(), rotation.x(), rotation.y(), rotation.z()}}, velocity{{velocity.x(), velocity.y(), velocity.z()}} {} Eigen::Quaterniond ToQuaternion() const { return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3]); } transform::Rigid3d ToRigid() const { return transform::Rigid3d( Eigen::Vector3d(translation[0], translation[1], translation[2]), ToQuaternion()); } }; struct Batch { common::Time time; sensor::PointCloud points; sensor::PointCloud high_resolution_filtered_points; sensor::PointCloud low_resolution_filtered_points; State state; }; struct OdometerData { common::Time time; // Dead-reckoning pose of the odometry. transform::Rigid3d pose; }; State PredictState(const State& start_state, const common::Time start_time, const common::Time end_time); void RemoveObsoleteSensorData(); std::unique_ptr AddAccumulatedRangeData( common::Time time, const transform::Rigid3d& pose_observation, const sensor::RangeData& range_data_in_tracking); std::unique_ptr InsertIntoSubmap( const common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose_observation); void TransformStates(const transform::Rigid3d& transform); std::unique_ptr MaybeOptimize(common::Time time); const proto::LocalTrajectoryBuilderOptions options_; const ceres::Solver::Options ceres_solver_options_; std::unique_ptr submaps_; int num_accumulated_; std::deque batches_; double gravity_constant_ = 9.8; std::deque imu_data_; std::deque odometer_data_; PoseEstimate last_pose_estimate_; MotionFilter motion_filter_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_H_ ================================================ FILE: mapping_3d/optimizing_local_trajectory_builder_options.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h" namespace cartographer { namespace mapping_3d { proto::OptimizingLocalTrajectoryBuilderOptions CreateOptimizingLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::OptimizingLocalTrajectoryBuilderOptions options; options.set_high_resolution_grid_weight( parameter_dictionary->GetDouble("high_resolution_grid_weight")); options.set_low_resolution_grid_weight( parameter_dictionary->GetDouble("low_resolution_grid_weight")); options.set_velocity_weight( parameter_dictionary->GetDouble("velocity_weight")); options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); options.set_odometry_translation_weight( parameter_dictionary->GetDouble("odometry_translation_weight")); options.set_odometry_rotation_weight( parameter_dictionary->GetDouble("odometry_rotation_weight")); return options; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/optimizing_local_trajectory_builder_options.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" namespace cartographer { namespace mapping_3d { proto::OptimizingLocalTrajectoryBuilderOptions CreateOptimizingLocalTrajectoryBuilderOptions( common::LuaParameterDictionary* parameter_dictionary); } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ ================================================ FILE: mapping_3d/proto/hybrid_grid.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.proto; message HybridGrid { optional float resolution = 1; // '{x, y, z}_indices[i]' is the index of 'values[i]'. repeated sint32 x_indices = 3 [packed = true]; repeated sint32 y_indices = 4 [packed = true]; repeated sint32 z_indices = 5 [packed = true]; // The entries in 'values' should be uint16s, not int32s, but protos don't // have a uint16 type. repeated int32 values = 6 [packed = true]; } ================================================ FILE: mapping_3d/proto/kalman_local_trajectory_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; package cartographer.mapping_3d.proto; message KalmanLocalTrajectoryBuilderOptions { // Whether to solve the online scan matching first using the correlative scan // matcher to generate a good starting point for Ceres. optional bool use_online_correlative_scan_matching = 1; optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions real_time_correlative_scan_matcher_options = 2; optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; optional double scan_matcher_variance = 6; optional double odometer_translational_variance = 4; optional double odometer_rotational_variance = 5; } ================================================ FILE: mapping_3d/proto/local_trajectory_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_3d/proto/motion_filter_options.proto"; import "cartographer/sensor/proto/adaptive_voxel_filter_options.proto"; import "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.proto"; import "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto"; import "cartographer/mapping_3d/proto/submaps_options.proto"; import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto"; package cartographer.mapping_3d.proto; message LocalTrajectoryBuilderOptions { enum Use { KALMAN = 0; OPTIMIZING = 1; } // Rangefinder points outside these ranges will be dropped. optional float min_range = 1; optional float max_range = 2; // Number of scans to accumulate into one unwarped, combined scan to use for // scan matching. optional int32 scans_per_accumulation = 3; // Voxel filter that gets applied to the range data immediately after // cropping. optional float voxel_filter_size = 4; // Voxel filter used to compute a sparser point cloud for matching. optional sensor.proto.AdaptiveVoxelFilterOptions high_resolution_adaptive_voxel_filter_options = 5; optional sensor.proto.AdaptiveVoxelFilterOptions low_resolution_adaptive_voxel_filter_options = 12; optional scan_matching.proto.CeresScanMatcherOptions ceres_scan_matcher_options = 6; optional MotionFilterOptions motion_filter_options = 7; optional SubmapsOptions submaps_options = 8; // Which one of the implementation to instantiate and use. optional Use use = 9; optional KalmanLocalTrajectoryBuilderOptions kalman_local_trajectory_builder_options = 10; optional OptimizingLocalTrajectoryBuilderOptions optimizing_local_trajectory_builder_options = 11; } ================================================ FILE: mapping_3d/proto/motion_filter_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.proto; message MotionFilterOptions { // Threshold above which a new scan is inserted based on time. optional double max_time_seconds = 1; // Threshold above which a new scan is inserted based on linear motion. optional double max_distance_meters = 2; // Threshold above which a new scan is inserted based on rotational motion. optional double max_angle_radians = 3; } ================================================ FILE: mapping_3d/proto/optimizing_local_trajectory_builder_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.proto; message OptimizingLocalTrajectoryBuilderOptions { optional double high_resolution_grid_weight = 6; optional double low_resolution_grid_weight = 7; optional double velocity_weight = 1; optional double translation_weight = 2; optional double rotation_weight = 3; optional double odometry_translation_weight = 4; optional double odometry_rotation_weight = 5; } ================================================ FILE: mapping_3d/proto/range_data_inserter_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.proto; message RangeDataInserterOptions { // Probability change for a hit (this will be converted to odds and therefore // must be greater than 0.5). optional double hit_probability = 1; // Probability change for a miss (this will be converted to odds and therefore // must be less than 0.5). optional double miss_probability = 2; // Up to how many free space voxels are updated for scan matching. // 0 disables free space. optional int32 num_free_space_voxels = 3; } ================================================ FILE: mapping_3d/proto/submaps_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; import "cartographer/mapping_3d/proto/range_data_inserter_options.proto"; package cartographer.mapping_3d.proto; message SubmapsOptions { // Resolution of the 'high_resolution' map in meters used for local SLAM and // loop closure. optional double high_resolution = 1; // Maximum range to filter the point cloud to before insertion into the // 'high_resolution' map. optional double high_resolution_max_range = 4; // Resolution of the 'low_resolution' version of the map in meters used for // local SLAM only. optional double low_resolution = 5; // Number of scans before adding a new submap. Each submap will get twice the // number of scans inserted: First for initialization without being matched // against, then while being matched. optional int32 num_range_data = 2; optional RangeDataInserterOptions range_data_inserter_options = 3; } ================================================ FILE: mapping_3d/range_data_inserter.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/range_data_inserter.h" #include "Eigen/Core" #include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace { void InsertMissesIntoGrid(const std::vector& miss_table, const Eigen::Vector3f& origin, const sensor::PointCloud& returns, HybridGrid* hybrid_grid, const int num_free_space_voxels) { const Eigen::Array3i origin_cell = hybrid_grid->GetCellIndex(origin); for (const Eigen::Vector3f& hit : returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); const Eigen::Array3i delta = hit_cell - origin_cell; const int num_samples = delta.cwiseAbs().maxCoeff(); CHECK_LT(num_samples, 1 << 15); // 'num_samples' is the number of samples we equi-distantly place on the // line between 'origin' and 'hit'. (including a fractional part for sub- // voxels) It is chosen so that between two samples we change from one voxel // to the next on the fastest changing dimension. // // Only the last 'num_free_space_voxels' are updated for performance. for (int position = std::max(0, num_samples - num_free_space_voxels); position < num_samples; ++position) { const Eigen::Array3i miss_cell = origin_cell + delta * position / num_samples; hybrid_grid->ApplyLookupTable(miss_cell, miss_table); } } } } // namespace proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary) { proto::RangeDataInserterOptions options; options.set_hit_probability( parameter_dictionary->GetDouble("hit_probability")); options.set_miss_probability( parameter_dictionary->GetDouble("miss_probability")); options.set_num_free_space_voxels( parameter_dictionary->GetInt("num_free_space_voxels")); CHECK_GT(options.hit_probability(), 0.5); CHECK_LT(options.miss_probability(), 0.5); return options; } RangeDataInserter::RangeDataInserter( const proto::RangeDataInserterOptions& options) : options_(options), hit_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options_.hit_probability()))), miss_table_(mapping::ComputeLookupTableToApplyOdds( mapping::Odds(options_.miss_probability()))) {} void RangeDataInserter::Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid) const { CHECK_NOTNULL(hybrid_grid)->StartUpdate(); for (const Eigen::Vector3f& hit : range_data.returns) { const Eigen::Array3i hit_cell = hybrid_grid->GetCellIndex(hit); hybrid_grid->ApplyLookupTable(hit_cell, hit_table_); } // By not starting a new update after hits are inserted, we give hits priority // (i.e. no hits will be ignored because of a miss in the same cell). InsertMissesIntoGrid(miss_table_, range_data.origin, range_data.returns, hybrid_grid, options_.num_free_space_voxels()); } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/range_data_inserter.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" namespace cartographer { namespace mapping_3d { proto::RangeDataInserterOptions CreateRangeDataInserterOptions( common::LuaParameterDictionary* parameter_dictionary); class RangeDataInserter { public: explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); RangeDataInserter(const RangeDataInserter&) = delete; RangeDataInserter& operator=(const RangeDataInserter&) = delete; // Inserts 'range_data' into 'hybrid_grid'. void Insert(const sensor::RangeData& range_data, HybridGrid* hybrid_grid) const; private: const proto::RangeDataInserterOptions options_; const std::vector hit_table_; const std::vector miss_table_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ ================================================ FILE: mapping_3d/range_data_inserter_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/range_data_inserter.h" #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace { class RangeDataInserterTest : public ::testing::Test { protected: RangeDataInserterTest() : hybrid_grid_(1.f) { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 1000, " "}"); options_ = CreateRangeDataInserterOptions(parameter_dictionary.get()); range_data_inserter_.reset(new RangeDataInserter(options_)); } void InsertPointCloud() { const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f); sensor::PointCloud returns = { {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}}; range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}}, &hybrid_grid_); } float GetProbability(float x, float y, float z) const { return hybrid_grid_.GetProbability( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } float IsKnown(float x, float y, float z) const { return hybrid_grid_.IsKnown( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } const proto::RangeDataInserterOptions& options() const { return options_; } private: HybridGrid hybrid_grid_; std::unique_ptr range_data_inserter_; proto::RangeDataInserterOptions options_; }; TEST_F(RangeDataInserterTest, InsertPointCloud) { InsertPointCloud(); EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f), 1e-4); EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), 1e-4); EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f), 1e-4); for (int x = -4; x <= 4; ++x) { for (int y = -4; y <= 4; ++y) { if (x < -3 || x > 0 || y != x + 2) { EXPECT_FALSE(IsKnown(x, y, 4.f)); } else { EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f), 1e-4); } } } } TEST_F(RangeDataInserterTest, ProbabilityProgression) { InsertPointCloud(); EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f), 1e-4); EXPECT_NEAR(options().miss_probability(), GetProbability(-2.f, 0.f, 3.f), 1e-4); EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f), 1e-4); for (int i = 0; i < 1000; ++i) { InsertPointCloud(); } EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3); EXPECT_NEAR(mapping::kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3); EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3); } } // namespace } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/rotation_cost_function.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { namespace mapping_3d { // Penalizes differences between IMU data and optimized orientations. class RotationCostFunction { public: RotationCostFunction(const double scaling_factor, const Eigen::Quaterniond& delta_rotation_imu_frame) : scaling_factor_(scaling_factor), delta_rotation_imu_frame_(delta_rotation_imu_frame) {} RotationCostFunction(const RotationCostFunction&) = delete; RotationCostFunction& operator=(const RotationCostFunction&) = delete; template bool operator()(const T* const start_rotation, const T* const end_rotation, T* residual) const { const Eigen::Quaternion start(start_rotation[0], start_rotation[1], start_rotation[2], start_rotation[3]); const Eigen::Quaternion end(end_rotation[0], end_rotation[1], end_rotation[2], end_rotation[3]); const Eigen::Quaternion error = end.conjugate() * start * delta_rotation_imu_frame_.cast(); residual[0] = scaling_factor_ * error.x(); residual[1] = scaling_factor_ * error.y(); residual[2] = scaling_factor_ * error.z(); return true; } private: const double scaling_factor_; const Eigen::Quaterniond delta_rotation_imu_frame_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_ ================================================ FILE: mapping_3d/scan_matching/ceres_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include #include #include #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" #include "cartographer/mapping_3d/ceres_pose.h" #include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { struct YawOnlyQuaternionPlus { template bool operator()(const T* x, const T* delta, T* x_plus_delta) const { const T clamped_delta = common::Clamp(delta[0], T(-0.5), T(0.5)); T q_delta[4]; q_delta[0] = ceres::sqrt(1. - clamped_delta * clamped_delta); q_delta[1] = T(0.); q_delta[2] = T(0.); q_delta[3] = clamped_delta; ceres::QuaternionProduct(q_delta, x, x_plus_delta); return true; } }; } // namespace proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::CeresScanMatcherOptions options; for (int i = 0;; ++i) { const string lua_identifier = "occupied_space_weight_" + std::to_string(i); if (!parameter_dictionary->HasKey(lua_identifier)) { break; } options.add_occupied_space_weight( parameter_dictionary->GetDouble(lua_identifier)); } options.set_translation_weight( parameter_dictionary->GetDouble("translation_weight")); options.set_rotation_weight( parameter_dictionary->GetDouble("rotation_weight")); options.set_only_optimize_yaw( parameter_dictionary->GetBool("only_optimize_yaw")); *options.mutable_ceres_solver_options() = common::CreateCeresSolverOptionsProto( parameter_dictionary->GetDictionary("ceres_solver_options").get()); return options; } CeresScanMatcher::CeresScanMatcher( const proto::CeresScanMatcherOptions& options) : options_(options), ceres_solver_options_( common::CreateCeresSolverOptions(options.ceres_solver_options())) { ceres_solver_options_.linear_solver_type = ceres::DENSE_QR; } void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, const transform::Rigid3d& initial_pose_estimate, const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* const pose_estimate, ceres::Solver::Summary* const summary) { ceres::Problem problem; CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, options_.only_optimize_yaw() ? std::unique_ptr( common::make_unique>()) : std::unique_ptr( common::make_unique()), &problem); CHECK_EQ(options_.occupied_space_weight_size(), point_clouds_and_hybrid_grids.size()); for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) { CHECK_GT(options_.occupied_space_weight(i), 0.); const sensor::PointCloud& point_cloud = *point_clouds_and_hybrid_grids[i].first; const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new OccupiedSpaceCostFunctor( options_.occupied_space_weight(i) / std::sqrt(static_cast(point_cloud.size())), point_cloud, hybrid_grid), point_cloud.size()), nullptr, ceres_pose.translation(), ceres_pose.rotation()); } CHECK_GT(options_.translation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationDeltaCostFunctor(options_.translation_weight(), previous_pose)), nullptr, ceres_pose.translation()); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RotationDeltaCostFunctor(options_.rotation_weight(), initial_pose_estimate.rotation())), nullptr, ceres_pose.rotation()); ceres::Solve(ceres_solver_options_, &problem, summary); *pose_estimate = ceres_pose.ToRigid(); } } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/ceres_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ #include #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); using PointCloudAndHybridGridPointers = std::pair; // This scan matcher uses Ceres to align scans with an existing map. class CeresScanMatcher { public: explicit CeresScanMatcher(const proto::CeresScanMatcherOptions& options); CeresScanMatcher(const CeresScanMatcher&) = delete; CeresScanMatcher& operator=(const CeresScanMatcher&) = delete; // Aligns 'point_clouds' within the 'hybrid_grids' given an // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver // 'summary'. void Match(const transform::Rigid3d& previous_pose, const transform::Rigid3d& initial_pose_estimate, const std::vector& point_clouds_and_hybrid_grids, transform::Rigid3d* pose_estimate, ceres::Solver::Summary* summary); private: const proto::CeresScanMatcherOptions options_; ceres::Solver::Options ceres_solver_options_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_CERES_SCAN_MATCHER_H_ ================================================ FILE: mapping_3d/scan_matching/ceres_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { class CeresScanMatcherTest : public ::testing::Test { protected: CeresScanMatcherTest() : hybrid_grid_(1.f), expected_pose_( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) { for (const auto& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { point_cloud_.push_back(point); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); } auto parameter_dictionary = common::MakeDictionary(R"text( return { occupied_space_weight_0 = 1., translation_weight = 0.01, rotation_weight = 0.1, only_optimize_yaw = false, ceres_solver_options = { use_nonmonotonic_steps = true, max_num_iterations = 10, num_threads = 1, }, })text"); options_ = CreateCeresScanMatcherOptions(parameter_dictionary.get()); ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); } void TestFromInitialPose(const transform::Rigid3d& initial_pose) { transform::Rigid3d pose; ceres::Solver::Summary summary; ceres_scan_matcher_->Match(initial_pose, initial_pose, {{&point_cloud_, &hybrid_grid_}}, &pose, &summary); EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport(); EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2)); } HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; proto::CeresScanMatcherOptions options_; std::unique_ptr ceres_scan_matcher_; }; TEST_F(CeresScanMatcherTest, PerfectEstimate) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); } TEST_F(CeresScanMatcherTest, AlongX) { ceres_scan_matcher_.reset(new CeresScanMatcher(options_)); TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); } TEST_F(CeresScanMatcherTest, AlongZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); } TEST_F(CeresScanMatcherTest, AlongXYZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); } TEST_F(CeresScanMatcherTest, FullPoseCorrection) { // We try to find the rotation around z... const auto additional_transform = transform::Rigid3d::Rotation( Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.))); point_cloud_ = sensor::TransformPointCloud( point_cloud_, additional_transform.cast()); expected_pose_ = expected_pose_ * additional_transform.inverse(); // ...starting initially with rotation around x. TestFromInitialPose( transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05), Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.)))); } } // namespace } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include #include #include #include #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::FastCorrelativeScanMatcherOptions options; options.set_branch_and_bound_depth( parameter_dictionary->GetInt("branch_and_bound_depth")); options.set_full_resolution_depth( parameter_dictionary->GetInt("full_resolution_depth")); options.set_rotational_histogram_size( parameter_dictionary->GetInt("rotational_histogram_size")); options.set_min_rotational_score( parameter_dictionary->GetDouble("min_rotational_score")); options.set_linear_xy_search_window( parameter_dictionary->GetDouble("linear_xy_search_window")); options.set_linear_z_search_window( parameter_dictionary->GetDouble("linear_z_search_window")); options.set_angular_search_window( parameter_dictionary->GetDouble("angular_search_window")); return options; } class PrecomputationGridStack { public: PrecomputationGridStack( const HybridGrid& hybrid_grid, const proto::FastCorrelativeScanMatcherOptions& options) { CHECK_GE(options.branch_and_bound_depth(), 1); CHECK_GE(options.full_resolution_depth(), 1); precomputation_grids_.reserve(options.branch_and_bound_depth()); precomputation_grids_.push_back(ConvertToPrecomputationGrid(hybrid_grid)); Eigen::Array3i last_width = Eigen::Array3i::Ones(); for (int depth = 1; depth != options.branch_and_bound_depth(); ++depth) { const bool half_resolution = depth >= options.full_resolution_depth(); const Eigen::Array3i next_width = ((1 << depth) * Eigen::Array3i::Ones()); const int full_voxels_per_high_resolution_voxel = 1 << std::max(0, depth - options.full_resolution_depth()); const Eigen::Array3i shift = (next_width - last_width + (full_voxels_per_high_resolution_voxel - 1)) / full_voxels_per_high_resolution_voxel; precomputation_grids_.push_back( PrecomputeGrid(precomputation_grids_.back(), half_resolution, shift)); last_width = next_width; } } const PrecomputationGrid& Get(int depth) const { return precomputation_grids_.at(depth); } int max_depth() const { return precomputation_grids_.size() - 1; } private: std::vector precomputation_grids_; }; FastCorrelativeScanMatcher::FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options) : options_(options), resolution_(hybrid_grid.resolution()), width_in_voxels_(hybrid_grid.grid_size()), precomputation_grid_stack_( common::make_unique(hybrid_grid, options)), rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {} FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {} bool FastCorrelativeScanMatcher::Match( const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, float* const score, transform::Rigid3d* const pose_estimate) const { const SearchParameters search_parameters{ common::RoundToInt(options_.linear_xy_search_window() / resolution_), common::RoundToInt(options_.linear_z_search_window() / resolution_), options_.angular_search_window()}; return MatchWithSearchParameters(search_parameters, initial_pose_estimate, coarse_point_cloud, fine_point_cloud, min_score, score, pose_estimate); } bool FastCorrelativeScanMatcher::MatchFullSubmap( const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, float* const score, transform::Rigid3d* const pose_estimate) const { const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(), gravity_alignment); float max_point_distance = 0.f; for (const Eigen::Vector3f& point : coarse_point_cloud) { max_point_distance = std::max(max_point_distance, point.norm()); } const int linear_window_size = (width_in_voxels_ + 1) / 2 + common::RoundToInt(max_point_distance / resolution_ + 0.5f); const SearchParameters search_parameters{linear_window_size, linear_window_size, M_PI}; return MatchWithSearchParameters(search_parameters, initial_pose_estimate, coarse_point_cloud, fine_point_cloud, min_score, score, pose_estimate); } bool FastCorrelativeScanMatcher::MatchWithSearchParameters( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const float min_score, float* const score, transform::Rigid3d* const pose_estimate) const { CHECK_NOTNULL(score); CHECK_NOTNULL(pose_estimate); const std::vector discrete_scans = GenerateDiscreteScans( search_parameters, coarse_point_cloud, fine_point_cloud, initial_pose_estimate.cast()); const std::vector lowest_resolution_candidates = ComputeLowestResolutionCandidates(search_parameters, discrete_scans); const Candidate best_candidate = BranchAndBound( search_parameters, discrete_scans, lowest_resolution_candidates, precomputation_grid_stack_->max_depth(), min_score); if (best_candidate.score > min_score) { *score = best_candidate.score; *pose_estimate = (transform::Rigid3f( resolution_ * best_candidate.offset.matrix().cast(), Eigen::Quaternionf::Identity()) * discrete_scans[best_candidate.scan_index].pose) .cast(); return true; } return false; } DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose) const { std::vector> cell_indices_per_depth; const PrecomputationGrid& original_grid = precomputation_grid_stack_->Get(0); std::vector full_resolution_cell_indices; for (const Eigen::Vector3f& point : sensor::TransformPointCloud(point_cloud, pose)) { full_resolution_cell_indices.push_back(original_grid.GetCellIndex(point)); } const int full_resolution_depth = std::min(options_.full_resolution_depth(), options_.branch_and_bound_depth()); CHECK_GE(full_resolution_depth, 1); for (int i = 0; i != full_resolution_depth; ++i) { cell_indices_per_depth.push_back(full_resolution_cell_indices); } const int low_resolution_depth = options_.branch_and_bound_depth() - full_resolution_depth; CHECK_GE(low_resolution_depth, 0); const Eigen::Array3i search_window_start( -search_parameters.linear_xy_window_size, -search_parameters.linear_xy_window_size, -search_parameters.linear_z_window_size); for (int i = 0; i != low_resolution_depth; ++i) { const int reduction_exponent = i + 1; const Eigen::Array3i low_resolution_search_window_start( search_window_start[0] >> reduction_exponent, search_window_start[1] >> reduction_exponent, search_window_start[2] >> reduction_exponent); cell_indices_per_depth.emplace_back(); for (const Eigen::Array3i& cell_index : full_resolution_cell_indices) { const Eigen::Array3i cell_at_start = cell_index + search_window_start; const Eigen::Array3i low_resolution_cell_at_start( cell_at_start[0] >> reduction_exponent, cell_at_start[1] >> reduction_exponent, cell_at_start[2] >> reduction_exponent); cell_indices_per_depth.back().push_back( low_resolution_cell_at_start - low_resolution_search_window_start); } } return DiscreteScan{pose, cell_indices_per_depth}; } std::vector FastCorrelativeScanMatcher::GenerateDiscreteScans( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const transform::Rigid3f& initial_pose) const { std::vector result; // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution_; for (const Eigen::Vector3f& point : coarse_point_cloud) { const float range = point.norm(); max_scan_range = std::max(range, max_scan_range); } const float kSafetyMargin = 1.f - 1e-2f; const float angular_step_size = kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) / (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt( search_parameters.angular_search_window / angular_step_size); // TODO(whess): Should there be a small search window for rotations around // x and y? std::vector angles; for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { angles.push_back(rz * angular_step_size); } const std::vector scores = rotational_scan_matcher_.Match( sensor::TransformPointCloud(fine_point_cloud, initial_pose), angles); for (size_t i = 0; i != angles.size(); ++i) { if (scores[i] < options_.min_rotational_score()) { continue; } const Eigen::Vector3f angle_axis(0.f, 0.f, angles[i]); // It's important to apply the 'angle_axis' rotation between the translation // and rotation of the 'initial_pose', so that the rotation is around the // origin of the range data, and yaw is in map frame. const transform::Rigid3f pose( initial_pose.translation(), transform::AngleAxisVectorToRotationQuaternion(angle_axis) * initial_pose.rotation()); result.push_back( DiscretizeScan(search_parameters, coarse_point_cloud, pose)); } return result; } std::vector FastCorrelativeScanMatcher::GenerateLowestResolutionCandidates( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const int num_discrete_scans) const { const int linear_step_size = 1 << precomputation_grid_stack_->max_depth(); const int num_lowest_resolution_linear_xy_candidates = (2 * search_parameters.linear_xy_window_size + linear_step_size) / linear_step_size; const int num_lowest_resolution_linear_z_candidates = (2 * search_parameters.linear_z_window_size + linear_step_size) / linear_step_size; const int num_candidates = num_discrete_scans * common::Power(num_lowest_resolution_linear_xy_candidates, 2) * num_lowest_resolution_linear_z_candidates; std::vector candidates; candidates.reserve(num_candidates); for (int scan_index = 0; scan_index != num_discrete_scans; ++scan_index) { for (int z = -search_parameters.linear_z_window_size; z <= search_parameters.linear_z_window_size; z += linear_step_size) { for (int y = -search_parameters.linear_xy_window_size; y <= search_parameters.linear_xy_window_size; y += linear_step_size) { for (int x = -search_parameters.linear_xy_window_size; x <= search_parameters.linear_xy_window_size; x += linear_step_size) { candidates.emplace_back(scan_index, Eigen::Array3i(x, y, z)); } } } } CHECK_EQ(candidates.size(), num_candidates); return candidates; } void FastCorrelativeScanMatcher::ScoreCandidates( const int depth, const std::vector& discrete_scans, std::vector* const candidates) const { const int reduction_exponent = std::max(0, depth - options_.full_resolution_depth() + 1); for (Candidate& candidate : *candidates) { int sum = 0; const DiscreteScan& discrete_scan = discrete_scans[candidate.scan_index]; const Eigen::Array3i offset(candidate.offset[0] >> reduction_exponent, candidate.offset[1] >> reduction_exponent, candidate.offset[2] >> reduction_exponent); CHECK_LT(depth, discrete_scan.cell_indices_per_depth.size()); for (const Eigen::Array3i& cell_index : discrete_scan.cell_indices_per_depth[depth]) { const Eigen::Array3i proposed_cell_index = cell_index + offset; sum += precomputation_grid_stack_->Get(depth).value(proposed_cell_index); } candidate.score = PrecomputationGrid::ToProbability( sum / static_cast(discrete_scan.cell_indices_per_depth[depth].size())); } std::sort(candidates->begin(), candidates->end(), std::greater()); } std::vector FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const std::vector& discrete_scans) const { std::vector lowest_resolution_candidates = GenerateLowestResolutionCandidates(search_parameters, discrete_scans.size()); ScoreCandidates(precomputation_grid_stack_->max_depth(), discrete_scans, &lowest_resolution_candidates); return lowest_resolution_candidates; } Candidate FastCorrelativeScanMatcher::BranchAndBound( const FastCorrelativeScanMatcher::SearchParameters& search_parameters, const std::vector& discrete_scans, const std::vector& candidates, const int candidate_depth, float min_score) const { if (candidate_depth == 0) { // Return the best candidate. return *candidates.begin(); } Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero()); best_high_resolution_candidate.score = min_score; for (const Candidate& candidate : candidates) { if (candidate.score <= min_score) { break; } std::vector higher_resolution_candidates; const int half_width = 1 << (candidate_depth - 1); for (int z : {0, half_width}) { if (candidate.offset.z() + z > search_parameters.linear_z_window_size) { break; } for (int y : {0, half_width}) { if (candidate.offset.y() + y > search_parameters.linear_xy_window_size) { break; } for (int x : {0, half_width}) { if (candidate.offset.x() + x > search_parameters.linear_xy_window_size) { break; } higher_resolution_candidates.emplace_back( candidate.scan_index, candidate.offset + Eigen::Array3i(x, y, z)); } } } ScoreCandidates(candidate_depth - 1, discrete_scans, &higher_resolution_candidates); best_high_resolution_candidate = std::max( best_high_resolution_candidate, BranchAndBound(search_parameters, discrete_scans, higher_resolution_candidates, candidate_depth - 1, best_high_resolution_candidate.score)); } return best_high_resolution_candidate; } } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ // This is an implementation of a 3D branch-and-bound algorithm similar to // mapping_2d::FastCorrelativeScanMatcher. #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ #include #include #include "Eigen/Core" #include "cartographer/common/port.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions( common::LuaParameterDictionary* parameter_dictionary); class PrecomputationGridStack; struct DiscreteScan { transform::Rigid3f pose; // Contains a vector of discretized scans for each 'depth'. std::vector> cell_indices_per_depth; }; struct Candidate { Candidate(const int scan_index, const Eigen::Array3i& offset) : scan_index(scan_index), offset(offset) {} // Index into the discrete scans vectors. int scan_index; // Linear offset from the initial pose in cell indices. For lower resolution // candidates this is the lowest offset of the 2^depth x 2^depth x 2^depth // block of possibilities. Eigen::Array3i offset; // Score, higher is better. float score = 0.f; bool operator<(const Candidate& other) const { return score < other.score; } bool operator>(const Candidate& other) const { return score > other.score; } }; class FastCorrelativeScanMatcher { public: FastCorrelativeScanMatcher( const HybridGrid& hybrid_grid, const std::vector& nodes, const proto::FastCorrelativeScanMatcherOptions& options); ~FastCorrelativeScanMatcher(); FastCorrelativeScanMatcher(const FastCorrelativeScanMatcher&) = delete; FastCorrelativeScanMatcher& operator=(const FastCorrelativeScanMatcher&) = delete; // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given an // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality) // is possible, true is returned, and 'score' and 'pose_estimate' are updated // with the result. 'fine_point_cloud' is used to compute the rotational scan // matcher score. bool Match(const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, float* score, transform::Rigid3d* pose_estimate) const; // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which // is expected to be approximately gravity aligned. If a score above // 'min_score' (excluding equality) is possible, true is returned, and 'score' // and 'pose_estimate' are updated with the result. 'fine_point_cloud' is used // to compute the rotational scan matcher score. bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, float* score, transform::Rigid3d* pose_estimate) const; private: struct SearchParameters { const int linear_xy_window_size; // voxels const int linear_z_window_size; // voxels const double angular_search_window; // radians }; bool MatchWithSearchParameters( const SearchParameters& search_parameters, const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, float min_score, float* score, transform::Rigid3d* pose_estimate) const; DiscreteScan DiscretizeScan(const SearchParameters& search_parameters, const sensor::PointCloud& point_cloud, const transform::Rigid3f& pose) const; std::vector GenerateDiscreteScans( const SearchParameters& search_parameters, const sensor::PointCloud& coarse_point_cloud, const sensor::PointCloud& fine_point_cloud, const transform::Rigid3f& initial_pose) const; std::vector GenerateLowestResolutionCandidates( const SearchParameters& search_parameters, int num_discrete_scans) const; void ScoreCandidates(int depth, const std::vector& discrete_scans, std::vector* const candidates) const; std::vector ComputeLowestResolutionCandidates( const SearchParameters& search_parameters, const std::vector& discrete_scans) const; Candidate BranchAndBound(const SearchParameters& search_parameters, const std::vector& discrete_scans, const std::vector& candidates, int candidate_depth, float min_score) const; const proto::FastCorrelativeScanMatcherOptions options_; const float resolution_; const int width_in_voxels_; std::unique_ptr precomputation_grid_stack_; RotationalScanMatcher rotational_scan_matcher_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_FAST_CORRELATIVE_SCAN_MATCHER_H_ ================================================ FILE: mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include #include #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) { auto parameter_dictionary = common::MakeDictionary( "return {" "branch_and_bound_depth = " + std::to_string(branch_and_bound_depth) + ", " "full_resolution_depth = " + std::to_string(branch_and_bound_depth) + ", " "rotational_histogram_size = 30, " "min_rotational_score = 0.1, " "linear_xy_search_window = 0.8, " "linear_z_search_window = 0.8, " "angular_search_window = 0.3, " "}"); return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get()); } mapping_3d::proto::RangeDataInserterOptions CreateRangeDataInserterTestOptions() { auto parameter_dictionary = common::MakeDictionary( "return { " "hit_probability = 0.7, " "miss_probability = 0.4, " "num_free_space_voxels = 5, " "}"); return CreateRangeDataInserterOptions(parameter_dictionary.get()); } TEST(FastCorrelativeScanMatcherTest, CorrectPose) { std::mt19937 prng(42); std::uniform_real_distribution distribution(-1.f, 1.f); RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions()); constexpr float kMinScore = 0.1f; const auto options = CreateFastCorrelativeScanMatcherTestOptions(5); sensor::PointCloud point_cloud{ Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f), Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f), Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f), Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f), Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f), Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)}; for (int i = 0; i != 20; ++i) { const float x = 0.7f * distribution(prng); const float y = 0.7f * distribution(prng); const float z = 0.7f * distribution(prng); const float theta = 0.2f * distribution(prng); const auto expected_pose = transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) * transform::Rigid3f::Rotation( Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ())); HybridGrid hybrid_grid(0.05f); hybrid_grid.StartUpdate(); range_data_inserter.Insert( sensor::RangeData{ expected_pose.translation(), sensor::TransformPointCloud(point_cloud, expected_pose), {}}, &hybrid_grid); FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {}, options); transform::Rigid3d pose_estimate; float score; EXPECT_TRUE(fast_correlative_scan_matcher.Match( transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore, &score, &pose_estimate)); EXPECT_LT(kMinScore, score); EXPECT_THAT(expected_pose, transform::IsNearly(pose_estimate.cast(), 0.05f)) << "Actual: " << transform::ToProto(pose_estimate).DebugString() << "\nExpected: " << transform::ToProto(expected_pose).DebugString(); } } } // namespace } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/interpolated_grid.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ #include #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { // Interpolates between HybridGrid probability voxels. We use the tricubic // interpolation which interpolates the values and has vanishing derivative at // these points. // // This class is templated to work with the autodiff that Ceres provides. // For this reason, it is also important that the interpolation scheme be // continuously differentiable. class InterpolatedGrid { public: explicit InterpolatedGrid(const HybridGrid& hybrid_grid) : hybrid_grid_(hybrid_grid) {} InterpolatedGrid(const InterpolatedGrid&) = delete; InterpolatedGrid& operator=(const InterpolatedGrid&) = delete; // Returns the interpolated probability at (x, y, z) of the HybridGrid // used to perform the interpolation. // // This is a piecewise, continuously differentiable function. We use the // scalar part of Jet parameters to select our interval below. It is the // tensor product volume of piecewise cubic polynomials that interpolate // the values, and have vanishing derivative at the interval boundaries. template T GetProbability(const T& x, const T& y, const T& z) const { double x1, y1, z1, x2, y2, z2; ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2); const Eigen::Array3i index1 = hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1)); const double q111 = hybrid_grid_.GetProbability(index1); const double q112 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1)); const double q121 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0)); const double q122 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1)); const double q211 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0)); const double q212 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1)); const double q221 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0)); const double q222 = hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1)); const T normalized_x = (x - x1) / (x2 - x1); const T normalized_y = (y - y1) / (y2 - y1); const T normalized_z = (z - z1) / (z2 - z1); // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive. const T normalized_xx = normalized_x * normalized_x; const T normalized_xxx = normalized_x * normalized_xx; const T normalized_yy = normalized_y * normalized_y; const T normalized_yyy = normalized_y * normalized_yy; const T normalized_zz = normalized_z * normalized_z; const T normalized_zzz = normalized_z * normalized_zz; // We first interpolate in z, then y, then x. All 7 times this uses the same // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2). // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0 // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1. const T q11 = (q111 - q112) * normalized_zzz * 2. + (q112 - q111) * normalized_zz * 3. + q111; const T q12 = (q121 - q122) * normalized_zzz * 2. + (q122 - q121) * normalized_zz * 3. + q121; const T q21 = (q211 - q212) * normalized_zzz * 2. + (q212 - q211) * normalized_zz * 3. + q211; const T q22 = (q221 - q222) * normalized_zzz * 2. + (q222 - q221) * normalized_zz * 3. + q221; const T q1 = (q11 - q12) * normalized_yyy * 2. + (q12 - q11) * normalized_yy * 3. + q11; const T q2 = (q21 - q22) * normalized_yyy * 2. + (q22 - q21) * normalized_yy * 3. + q21; return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. + q1; } private: template void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z, double* x1, double* y1, double* z1, double* x2, double* y2, double* z2) const { const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z); *x1 = lower.x(); *y1 = lower.y(); *z1 = lower.z(); *x2 = lower.x() + hybrid_grid_.resolution(); *y2 = lower.y() + hybrid_grid_.resolution(); *z2 = lower.z() + hybrid_grid_.resolution(); } // Center of the next lower voxel, i.e., not necessarily the voxel containing // (x, y, z). For each dimension, the largest voxel index so that the // corresponding center is at most the given coordinate. Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y, const double z) const { // Center of the cell containing (x, y, z). Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); // Move to the next lower voxel center. if (center.x() > x) { center.x() -= hybrid_grid_.resolution(); } if (center.y() > y) { center.y() -= hybrid_grid_.resolution(); } if (center.z() > z) { center.z() -= hybrid_grid_.resolution(); } return center; } // Uses the scalar part of a Ceres Jet. template Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y, const T& jet_z) const { return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a); } const HybridGrid& hybrid_grid_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_ ================================================ FILE: mapping_3d/scan_matching/interpolated_grid_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/interpolated_grid.h" #include "Eigen/Core" #include "cartographer/mapping_3d/hybrid_grid.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { class InterpolatedGridTest : public ::testing::Test { protected: InterpolatedGridTest() : hybrid_grid_(0.1f), interpolated_grid_(hybrid_grid_) { for (const auto& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { hybrid_grid_.SetProbability(hybrid_grid_.GetCellIndex(point), 1.); } } float GetHybridGridProbability(float x, float y, float z) const { return hybrid_grid_.GetProbability( hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z))); } HybridGrid hybrid_grid_; InterpolatedGrid interpolated_grid_; }; TEST_F(InterpolatedGridTest, InterpolatesGridPoints) { for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { EXPECT_NEAR(GetHybridGridProbability(x, y, z), interpolated_grid_.GetProbability(x, y, z), 1e-6); } } } } TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) { const double kSampleStep = hybrid_grid_.resolution() / 10.; for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) { for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) { for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) { const float start_probability = GetHybridGridProbability(x, y, z); const float next_probability = GetHybridGridProbability(x + hybrid_grid_.resolution(), y, z); const float grid_difference = next_probability - start_probability; if (std::abs(grid_difference) < 1e-6f) { continue; } for (double sample = kSampleStep; sample < hybrid_grid_.resolution() - 2 * kSampleStep; sample += kSampleStep) { EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability( x + sample + kSampleStep, y, z) - interpolated_grid_.GetProbability( x + sample, y, z))); } } } } } } // namespace } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/occupied_space_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ #include "Eigen/Core" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/scan_matching/interpolated_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { // Computes the cost of inserting occupied space described by the point cloud // into the map. The cost increases with the amount of free space that would be // replaced by occupied space. class OccupiedSpaceCostFunctor { public: // Creates an OccupiedSpaceCostFunctor using the specified grid, 'rotation' to // add to all poses, and point cloud. OccupiedSpaceCostFunctor(const double scaling_factor, const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid) : scaling_factor_(scaling_factor), point_cloud_(point_cloud), interpolated_grid_(hybrid_grid) {} OccupiedSpaceCostFunctor(const OccupiedSpaceCostFunctor&) = delete; OccupiedSpaceCostFunctor& operator=(const OccupiedSpaceCostFunctor&) = delete; template bool operator()(const T* const translation, const T* const rotation, T* const residual) const { const transform::Rigid3 transform( Eigen::Map>(translation), Eigen::Quaternion(rotation[0], rotation[1], rotation[2], rotation[3])); return Evaluate(transform, residual); } template bool Evaluate(const transform::Rigid3& transform, T* const residual) const { for (size_t i = 0; i < point_cloud_.size(); ++i) { const Eigen::Matrix world = transform * point_cloud_[i].cast(); const T probability = interpolated_grid_.GetProbability(world[0], world[1], world[2]); residual[i] = scaling_factor_ * (1. - probability); } return true; } private: const double scaling_factor_; const sensor::PointCloud& point_cloud_; const InterpolatedGrid interpolated_grid_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTOR_H_ ================================================ FILE: mapping_3d/scan_matching/precomputation_grid.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" #include #include "Eigen/Core" #include "cartographer/common/math.h" #include "cartographer/mapping/probability_values.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { // C++11 defines that integer division rounds towards zero. For index math, we // actually need it to round towards negative infinity. Luckily bit shifts have // that property. inline int DivideByTwoRoundingTowardsNegativeInfinity(const int value) { return value >> 1; } // Computes the half resolution index corresponding to the full resolution // 'cell_index'. Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) { return Eigen::Array3i( DivideByTwoRoundingTowardsNegativeInfinity(cell_index[0]), DivideByTwoRoundingTowardsNegativeInfinity(cell_index[1]), DivideByTwoRoundingTowardsNegativeInfinity(cell_index[2])); } } // namespace PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) { PrecomputationGrid result(hybrid_grid.resolution()); for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { const int cell_value = common::RoundToInt( (mapping::ValueToProbability(it.GetValue()) - mapping::kMinProbability) * (255.f / (mapping::kMaxProbability - mapping::kMinProbability))); CHECK_GE(cell_value, 0); CHECK_LE(cell_value, 255); *result.mutable_value(it.GetCellIndex()) = cell_value; } return result; } PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, const bool half_resolution, const Eigen::Array3i& shift) { PrecomputationGrid result(grid.resolution()); for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) { for (int i = 0; i != 8; ++i) { // We use this value to update 8 values in the resulting grid, at // position (x - {0, 'shift'}, y - {0, 'shift'}, z - {0, 'shift'}). // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, // this results in precomputation grids analogous to the 2D case. const Eigen::Array3i cell_index = it.GetCellIndex() - shift * PrecomputationGrid::GetOctant(i); auto* const cell_value = result.mutable_value( half_resolution ? CellIndexAtHalfResolution(cell_index) : cell_index); *cell_value = std::max(it.GetValue(), *cell_value); } } return result; } } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/precomputation_grid.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { class PrecomputationGrid : public HybridGridBase { public: explicit PrecomputationGrid(const float resolution) : HybridGridBase(resolution) {} // Maps values from [0, 255] to [kMinProbability, kMaxProbability]. static float ToProbability(float value) { return mapping::kMinProbability + value * ((mapping::kMaxProbability - mapping::kMinProbability) / 255.f); } }; // Converts a HybridGrid to a PrecomputationGrid representing the same data, // but only using 8 bit instead of 2 x 16 bit. PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid); // Returns a grid of the same resolution containing the maximum value of // original voxels in 'grid'. This maximum is over the 8 voxels that have // any combination of index components optionally increased by 'shift'. // If 'shift' is 2 ** (depth - 1), where depth 0 is the original grid, and this // is using the precomputed grid of one depth before, this results in // precomputation grids analogous to the 2D case. PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid, bool half_resolution, const Eigen::Array3i& shift); } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_PRECOMPUTATION_GRID_H_ ================================================ FILE: mapping_3d/scan_matching/precomputation_grid_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/precomputation_grid.h" #include #include #include #include "cartographer/mapping_3d/hybrid_grid.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) { HybridGrid hybrid_grid(2.f); std::mt19937 rng(23847); std::uniform_int_distribution coordinate_distribution(-50, 49); std::uniform_real_distribution value_distribution( mapping::kMinProbability, mapping::kMaxProbability); for (int i = 0; i < 1000; ++i) { const auto x = coordinate_distribution(rng); const auto y = coordinate_distribution(rng); const auto z = coordinate_distribution(rng); const Eigen::Array3i cell_index(x, y, z); hybrid_grid.SetProbability(cell_index, value_distribution(rng)); } std::vector precomputed_grids; for (int depth = 0; depth <= 3; ++depth) { if (depth == 0) { precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid)); } else { precomputed_grids.push_back( PrecomputeGrid(precomputed_grids.back(), false, (1 << (depth - 1)) * Eigen::Array3i::Ones())); } const int width = 1 << depth; for (int i = 0; i < 100; ++i) { const auto x = coordinate_distribution(rng); const auto y = coordinate_distribution(rng); const auto z = coordinate_distribution(rng); float max_probability = 0.; for (int dx = 0; dx < width; ++dx) { for (int dy = 0; dy < width; ++dy) { for (int dz = 0; dz < width; ++dz) { max_probability = std::max( max_probability, hybrid_grid.GetProbability( Eigen::Array3i(x + dx, y + dy, z + dz))); } } } EXPECT_NEAR(max_probability, PrecomputationGrid::ToProbability( precomputed_grids.back().value(Eigen::Array3i(x, y, z))), 1e-2); } } } } // namespace } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.scan_matching.proto; import "cartographer/common/proto/ceres_solver_options.proto"; // NEXT ID: 7 message CeresScanMatcherOptions { // Scaling parameters for each cost functor. repeated double occupied_space_weight = 1; optional double translation_weight = 2; optional double rotation_weight = 3; // Whether only to allow changes to yaw, keeping roll/pitch constant. optional bool only_optimize_yaw = 5; // Configure the Ceres solver. See the Ceres documentation for more // information: https://code.google.com/p/ceres-solver/ optional common.proto.CeresSolverOptions ceres_solver_options = 6; } ================================================ FILE: mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.mapping_3d.scan_matching.proto; message FastCorrelativeScanMatcherOptions { // Number of precomputed grids to use. optional int32 branch_and_bound_depth = 2; // Number of full resolution grids to use, additional grids will reduce the // resolution by half each. optional int32 full_resolution_depth = 8; // Number of histogram buckets for the rotational scan matcher. optional int32 rotational_histogram_size = 3; // Minimum score for the rotational scan matcher. optional double min_rotational_score = 4; // Linear search window in the plane orthogonal to gravity in which the best // possible scan alignment will be found. optional double linear_xy_search_window = 5; // Linear search window in the gravity direction in which the best possible // scan alignment will be found. optional double linear_z_search_window = 6; // Minimum angular search window in which the best possible scan alignment // will be found. optional double angular_search_window = 7; } ================================================ FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { RealTimeCorrelativeScanMatcher::RealTimeCorrelativeScanMatcher( const mapping_2d::scan_matching::proto:: RealTimeCorrelativeScanMatcherOptions& options) : options_(options) {} float RealTimeCorrelativeScanMatcher::Match( const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const { CHECK_NOTNULL(pose_estimate); float best_score = -1.f; for (const transform::Rigid3f& transform : GenerateExhaustiveSearchTransforms( hybrid_grid.resolution(), point_cloud)) { const transform::Rigid3f candidate = initial_pose_estimate.cast() * transform; const float score = ScoreCandidate( hybrid_grid, sensor::TransformPointCloud(point_cloud, candidate), transform); if (score > best_score) { best_score = score; *pose_estimate = candidate.cast(); } } return best_score; } std::vector RealTimeCorrelativeScanMatcher::GenerateExhaustiveSearchTransforms( const float resolution, const sensor::PointCloud& point_cloud) const { std::vector result; const int linear_window_size = common::RoundToInt(options_.linear_search_window() / resolution); // We set this value to something on the order of resolution to make sure that // the std::acos() below is defined. float max_scan_range = 3.f * resolution; for (const Eigen::Vector3f& point : point_cloud) { const float range = point.norm(); max_scan_range = std::max(range, max_scan_range); } const float kSafetyMargin = 1.f - 1e-3f; const float angular_step_size = kSafetyMargin * std::acos(1.f - common::Pow2(resolution) / (2.f * common::Pow2(max_scan_range))); const int angular_window_size = common::RoundToInt(options_.angular_search_window() / angular_step_size); for (int z = -linear_window_size; z <= linear_window_size; ++z) { for (int y = -linear_window_size; y <= linear_window_size; ++y) { for (int x = -linear_window_size; x <= linear_window_size; ++x) { for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) { for (int ry = -angular_window_size; ry <= angular_window_size; ++ry) { for (int rx = -angular_window_size; rx <= angular_window_size; ++rx) { const Eigen::Vector3f angle_axis(rx * angular_step_size, ry * angular_step_size, rz * angular_step_size); result.emplace_back( Eigen::Vector3f(x * resolution, y * resolution, z * resolution), transform::AngleAxisVectorToRotationQuaternion(angle_axis)); } } } } } } return result; } float RealTimeCorrelativeScanMatcher::ScoreCandidate( const HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const { float score = 0.f; for (const Eigen::Vector3f& point : transformed_point_cloud) { score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point)); } score /= static_cast(transformed_point_cloud.size()); const float angle = transform::GetAngle(transform); score *= std::exp(-common::Pow2(transform.translation().norm() * options_.translation_delta_cost_weight() + angle * options_.rotation_delta_cost_weight())); CHECK_GT(score, 0.f); return score; } } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ // A voxel accurate scan matcher, exhaustively evaluating the scan matching // search space. #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ #include #include "Eigen/Core" #include "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { class RealTimeCorrelativeScanMatcher { public: explicit RealTimeCorrelativeScanMatcher( const mapping_2d::scan_matching::proto:: RealTimeCorrelativeScanMatcherOptions& options); RealTimeCorrelativeScanMatcher(const RealTimeCorrelativeScanMatcher&) = delete; RealTimeCorrelativeScanMatcher& operator=( const RealTimeCorrelativeScanMatcher&) = delete; // Aligns 'point_cloud' within the 'hybrid_grid' given an // 'initial_pose_estimate' then updates 'pose_estimate' with the result and // returns the score. float Match(const transform::Rigid3d& initial_pose_estimate, const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid, transform::Rigid3d* pose_estimate) const; private: std::vector GenerateExhaustiveSearchTransforms( float resolution, const sensor::PointCloud& point_cloud) const; float ScoreCandidate(const HybridGrid& hybrid_grid, const sensor::PointCloud& transformed_point_cloud, const transform::Rigid3f& transform) const; const mapping_2d::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions options_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_REAL_TIME_CORRELATIVE_SCAN_MATCHER_H_ ================================================ FILE: mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher.h" #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "cartographer/transform/transform.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { class RealTimeCorrelativeScanMatcherTest : public ::testing::Test { protected: RealTimeCorrelativeScanMatcherTest() : hybrid_grid_(0.1f), expected_pose_(Eigen::Vector3d(-1., 0., 0.), Eigen::Quaterniond::Identity()) { for (const auto& point : {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f), Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f), Eigen::Vector3f(-7.f, 3.f, 1.f)}) { point_cloud_.push_back(point); hybrid_grid_.SetProbability( hybrid_grid_.GetCellIndex(expected_pose_.cast() * point), 1.); } auto parameter_dictionary = common::MakeDictionary(R"text( return { linear_search_window = 0.3, angular_search_window = math.rad(1.), translation_delta_cost_weight = 1e-1, rotation_delta_cost_weight = 1., })text"); real_time_correlative_scan_matcher_.reset( new RealTimeCorrelativeScanMatcher( mapping_2d::scan_matching:: CreateRealTimeCorrelativeScanMatcherOptions( parameter_dictionary.get()))); } void TestFromInitialPose(const transform::Rigid3d& initial_pose) { transform::Rigid3d pose; const float score = real_time_correlative_scan_matcher_->Match( initial_pose, point_cloud_, hybrid_grid_, &pose); LOG(INFO) << "Score: " << score; EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3)); } HybridGrid hybrid_grid_; transform::Rigid3d expected_pose_; sensor::PointCloud point_cloud_; std::unique_ptr real_time_correlative_scan_matcher_; }; TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))); } TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.))); } TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2))); } TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) { TestFromInitialPose( transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2))); } TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.)))); } TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.)))); } TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) { TestFromInitialPose(transform::Rigid3d( Eigen::Vector3d(-1., 0., 0.), Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.)))); } } // namespace } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/rotation_delta_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ #include #include "Eigen/Core" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/rotation.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { // Computes the cost of rotating the pose estimate. Cost increases with the // solution's distance from the rotation estimate. class RotationDeltaCostFunctor { public: // Constructs a new RotationDeltaCostFunctor from the given // 'rotation_estimate'. explicit RotationDeltaCostFunctor(const double scaling_factor, const Eigen::Quaterniond& initial_rotation) : scaling_factor_(scaling_factor) { initial_rotation_inverse_[0] = initial_rotation.w(); initial_rotation_inverse_[1] = -initial_rotation.x(); initial_rotation_inverse_[2] = -initial_rotation.y(); initial_rotation_inverse_[3] = -initial_rotation.z(); } RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete; RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete; template bool operator()(const T* const rotation_quaternion, T* residual) const { T delta[4]; T initial_rotation_inverse[4] = { T(initial_rotation_inverse_[0]), T(initial_rotation_inverse_[1]), T(initial_rotation_inverse_[2]), T(initial_rotation_inverse_[3])}; ceres::QuaternionProduct(initial_rotation_inverse, rotation_quaternion, delta); // Will compute the squared norm of the imaginary component of the delta // quaternion which is sin(phi/2)^2. residual[0] = scaling_factor_ * delta[1]; residual[1] = scaling_factor_ * delta[2]; residual[2] = scaling_factor_ * delta[3]; return true; } private: const double scaling_factor_; double initial_rotation_inverse_[4]; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_ ================================================ FILE: mapping_3d/scan_matching/rotational_scan_matcher.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/scan_matching/rotational_scan_matcher.h" #include #include #include "cartographer/common/math.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { namespace { constexpr float kMinDistance = 0.2f; constexpr float kMaxDistance = 0.9f; constexpr float kSliceHeight = 0.2f; void AddValueToHistogram(float angle, const float value, Eigen::VectorXf* histogram) { // Map the angle to [0, pi), i.e. a vector and its inverse are considered to // represent the same angle. while (angle > static_cast(M_PI)) { angle -= static_cast(M_PI); } while (angle < 0.f) { angle += static_cast(M_PI); } const float zero_to_one = angle / static_cast(M_PI); const int bucket = common::Clamp( common::RoundToInt(histogram->size() * zero_to_one - 0.5f), 0, histogram->size() - 1); (*histogram)(bucket) += value; } Eigen::Vector3f ComputeCentroid(const sensor::PointCloud& slice) { CHECK(!slice.empty()); Eigen::Vector3f sum = Eigen::Vector3f::Zero(); for (const Eigen::Vector3f& point : slice) { sum += point; } return sum / static_cast(slice.size()); } struct AngleValuePair { float angle; float value; }; void AddPointCloudSliceToValueVector( const sensor::PointCloud& slice, std::vector* value_vector) { if (slice.empty()) { return; } // We compute the angle of the ray from a point to the centroid of the whole // point cloud. If it is orthogonal to the angle we compute between points, we // will add the angle between points to the histogram with the maximum weight. // This is to reject, e.g., the angles observed on the ceiling and floor. const Eigen::Vector3f centroid = ComputeCentroid(slice); Eigen::Vector3f last_point = slice.front(); for (const Eigen::Vector3f& point : slice) { const Eigen::Vector2f delta = (point - last_point).head<2>(); const Eigen::Vector2f direction = (point - centroid).head<2>(); const float distance = delta.norm(); if (distance < kMinDistance || direction.norm() < kMinDistance) { continue; } if (distance > kMaxDistance) { last_point = point; continue; } const float angle = common::atan2(delta); const float value = std::max( 0.f, 1.f - std::abs(delta.normalized().dot(direction.normalized()))); value_vector->push_back(AngleValuePair{angle, value}); } } // A function to sort the points in each slice by angle around the centroid. // This is because the returns from different rangefinders are interleaved in // the data. sensor::PointCloud SortSlice(const sensor::PointCloud& slice) { struct SortableAnglePointPair { bool operator<(const SortableAnglePointPair& rhs) const { return angle < rhs.angle; } float angle; Eigen::Vector3f point; }; const Eigen::Vector3f centroid = ComputeCentroid(slice); std::vector by_angle; by_angle.reserve(slice.size()); for (const Eigen::Vector3f& point : slice) { const Eigen::Vector2f delta = (point - centroid).head<2>(); if (delta.norm() < kMinDistance) { continue; } by_angle.push_back(SortableAnglePointPair{common::atan2(delta), point}); } std::sort(by_angle.begin(), by_angle.end()); sensor::PointCloud result; for (const auto& pair : by_angle) { result.push_back(pair.point); } return result; } std::vector GetValuesForHistogram( const sensor::PointCloud& point_cloud) { std::map slices; for (const Eigen::Vector3f& point : point_cloud) { slices[common::RoundToInt(point.z() / kSliceHeight)].push_back(point); } std::vector result; for (const auto& slice : slices) { AddPointCloudSliceToValueVector(SortSlice(slice.second), &result); } return result; } void AddValuesToHistogram(const std::vector& value_vector, const float rotation, Eigen::VectorXf* histogram) { for (const AngleValuePair& pair : value_vector) { AddValueToHistogram(pair.angle + rotation, pair.value, histogram); } } } // namespace RotationalScanMatcher::RotationalScanMatcher( const std::vector& nodes, const int histogram_size) : histogram_(Eigen::VectorXf::Zero(histogram_size)) { for (const mapping::TrajectoryNode& node : nodes) { AddValuesToHistogram( GetValuesForHistogram(sensor::TransformPointCloud( node.constant_data->range_data_3d.returns.Decompress(), node.pose.cast())), 0.f, &histogram_); } } std::vector RotationalScanMatcher::Match( const sensor::PointCloud& point_cloud, const std::vector& angles) const { std::vector result; result.reserve(angles.size()); const std::vector value_vector = GetValuesForHistogram(point_cloud); for (const float angle : angles) { Eigen::VectorXf scan_histogram = Eigen::VectorXf::Zero(histogram_.size()); AddValuesToHistogram(value_vector, angle, &scan_histogram); result.push_back(MatchHistogram(scan_histogram)); } return result; } float RotationalScanMatcher::MatchHistogram( const Eigen::VectorXf& scan_histogram) const { // We compute the dot product of normalized histograms as a measure of // similarity. const float scan_histogram_norm = scan_histogram.norm(); const float histogram_norm = histogram_.norm(); const float normalization = scan_histogram_norm * histogram_norm; if (normalization < 1e-3f) { return 1.f; } return histogram_.dot(scan_histogram) / normalization; } } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/scan_matching/rotational_scan_matcher.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ #include #include "Eigen/Geometry" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/sensor/point_cloud.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { class RotationalScanMatcher { public: explicit RotationalScanMatcher( const std::vector& nodes, int histogram_size); RotationalScanMatcher(const RotationalScanMatcher&) = delete; RotationalScanMatcher& operator=(const RotationalScanMatcher&) = delete; // Scores how well a 'point_cloud' can be understood as rotated by certain // 'angles' relative to the 'nodes'. Each angle results in a score between // 0 (worst) and 1 (best). std::vector Match(const sensor::PointCloud& point_cloud, const std::vector& angles) const; private: float MatchHistogram(const Eigen::VectorXf& scan_histogram) const; Eigen::VectorXf histogram_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_ROTATIONAL_SCAN_MATCHER_H_ ================================================ FILE: mapping_3d/scan_matching/translation_delta_cost_functor.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ #include "Eigen/Core" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace mapping_3d { namespace scan_matching { // Computes the cost of translating the initial pose estimate. Cost increases // with the solution's distance from the initial estimate. class TranslationDeltaCostFunctor { public: // Constructs a new TranslationDeltaCostFunctor from the given // 'initial_pose_estimate'. explicit TranslationDeltaCostFunctor( const double scaling_factor, const transform::Rigid3d& initial_pose_estimate) : scaling_factor_(scaling_factor), x_(initial_pose_estimate.translation().x()), y_(initial_pose_estimate.translation().y()), z_(initial_pose_estimate.translation().z()) {} TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete; TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) = delete; template bool operator()(const T* const translation, T* residual) const { residual[0] = scaling_factor_ * (translation[0] - x_); residual[1] = scaling_factor_ * (translation[1] - y_); residual[2] = scaling_factor_ * (translation[2] - z_); return true; } private: const double scaling_factor_; const double x_; const double y_; const double z_; }; } // namespace scan_matching } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_ ================================================ FILE: mapping_3d/sparse_pose_graph/constraint_builder.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" #include #include #include #include #include #include #include #include #include "Eigen/Eigenvalues" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/thread_pool.h" #include "cartographer/kalman_filter/pose_tracker.h" #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h" #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { ConstraintBuilder::ConstraintBuilder( const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* const thread_pool) : options_(options), thread_pool_(thread_pool), sampler_(options.sampling_ratio()), adaptive_voxel_filter_(options.adaptive_voxel_filter_options()), ceres_scan_matcher_(options.ceres_scan_matcher_options_3d()) {} ConstraintBuilder::~ConstraintBuilder() { common::MutexLocker locker(&mutex_); CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called"; CHECK_EQ(pending_computations_.size(), 0); CHECK_EQ(submap_queued_work_items_.size(), 0); CHECK(when_done_ == nullptr); } void ConstraintBuilder::MaybeAddConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, const sensor::CompressedPointCloud* const compressed_point_cloud, const std::vector& submap_nodes, const transform::Rigid3d& initial_pose) { if (initial_pose.translation().norm() > options_.max_constraint_distance()) { return; } if (sampler_.Pulse()) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, node_id, false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ compressed_point_cloud, initial_pose, constraint); FinishComputation(current_computation); }); } } void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, const sensor::CompressedPointCloud* const compressed_point_cloud, const std::vector& submap_nodes, const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* const trajectory_connectivity) { common::MutexLocker locker(&mutex_); constraints_.emplace_back(); auto* const constraint = &constraints_.back(); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint( submap_id, submap, node_id, true, /* match_full_submap */ trajectory_connectivity, compressed_point_cloud, transform::Rigid3d::Rotation(gravity_alignment), constraint); FinishComputation(current_computation); }); } void ConstraintBuilder::NotifyEndOfScan() { common::MutexLocker locker(&mutex_); ++current_computation_; } void ConstraintBuilder::WhenDone( const std::function callback) { common::MutexLocker locker(&mutex_); CHECK(when_done_ == nullptr); when_done_ = common::make_unique>(callback); ++pending_computations_[current_computation_]; const int current_computation = current_computation_; thread_pool_->Schedule( [this, current_computation] { FinishComputation(current_computation); }); } void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, const HybridGrid* const submap, const std::function work_item) { if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher != nullptr) { thread_pool_->Schedule(work_item); } else { submap_queued_work_items_[submap_id].push_back(work_item); if (submap_queued_work_items_[submap_id].size() == 1) { thread_pool_->Schedule([=]() { ConstructSubmapScanMatcher(submap_id, submap_nodes, submap); }); } } } void ConstraintBuilder::ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, const HybridGrid* const submap) { auto submap_scan_matcher = common::make_unique( *submap, submap_nodes, options_.fast_correlative_scan_matcher_options_3d()); common::MutexLocker locker(&mutex_); submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)}; for (const std::function& work_item : submap_queued_work_items_[submap_id]) { thread_pool_->Schedule(work_item); } submap_queued_work_items_.erase(submap_id); } const ConstraintBuilder::SubmapScanMatcher* ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { common::MutexLocker locker(&mutex_); const SubmapScanMatcher* submap_scan_matcher = &submap_scan_matchers_[submap_id]; CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr); return submap_scan_matcher; } void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) { const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress(); const sensor::PointCloud filtered_point_cloud = adaptive_voxel_filter_.Filter(point_cloud); // The 'constraint_transform' (submap 'i' <- scan 'j') is computed from the // initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a // 'filtered_point_cloud' in 'j'. float score = 0.; transform::Rigid3d pose_estimate; if (match_full_submap) { if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap( initial_pose.rotation(), filtered_point_cloud, point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(submap_id.trajectory_id, 0); trajectory_connectivity->Connect(node_id.trajectory_id, submap_id.trajectory_id); } else { return; } } else { if (submap_scan_matcher->fast_correlative_scan_matcher->Match( initial_pose, filtered_point_cloud, point_cloud, options_.min_score(), &score, &pose_estimate)) { // We've reported a successful local match. CHECK_GT(score, options_.min_score()); } else { return; } } { common::MutexLocker locker(&mutex_); score_histogram_.Add(score); } // Use the CSM estimate as both the initial and previous pose. This has the // effect that, in the absence of better information, we prefer the original // CSM estimate. ceres::Solver::Summary unused_summary; transform::Rigid3d constraint_transform; ceres_scan_matcher_.Match( pose_estimate, pose_estimate, {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}}, &constraint_transform, &unused_summary); constraint->reset(new OptimizationProblem::Constraint{ submap_id, node_id, {constraint_transform, options_.loop_closure_translation_weight(), options_.loop_closure_rotation_weight()}, OptimizationProblem::Constraint::INTER_SUBMAP}); if (options_.log_matches()) { std::ostringstream info; info << "Node " << node_id << " with " << filtered_point_cloud.size() << " points on submap " << submap_id << std::fixed; if (match_full_submap) { info << " matches"; } else { const transform::Rigid3d difference = initial_pose.inverse() * constraint_transform; info << " differs by translation " << std::setprecision(2) << difference.translation().norm() << " rotation " << std::setprecision(3) << transform::GetAngle(difference); } info << " with score " << std::setprecision(1) << 100. * score << "%."; LOG(INFO) << info.str(); } } void ConstraintBuilder::FinishComputation(const int computation_index) { Result result; std::unique_ptr> callback; { common::MutexLocker locker(&mutex_); if (--pending_computations_[computation_index] == 0) { pending_computations_.erase(computation_index); } if (pending_computations_.empty()) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { for (const std::unique_ptr& constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); } } if (options_.log_matches()) { LOG(INFO) << constraints_.size() << " computations resulted in " << result.size() << " additional constraints."; LOG(INFO) << "Score histogram:\n" << score_histogram_.ToString(10); } constraints_.clear(); callback = std::move(when_done_); when_done_.reset(); } } } if (callback != nullptr) { (*callback)(result); } } int ConstraintBuilder::GetNumFinishedScans() { common::MutexLocker locker(&mutex_); if (pending_computations_.empty()) { return current_computation_; } return pending_computations_.begin()->first; } } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/sparse_pose_graph/constraint_builder.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ #include #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/histogram.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping/trajectory_node.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/voxel_filter.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { // Asynchronously computes constraints. // // Intermingle an arbitrary number of calls to MaybeAddConstraint() or // MaybeAddGlobalConstraint, then call WhenDone(). After all computations are // done the 'callback' will be called with the result and another // MaybeAdd(Global)Constraint()/WhenDone() cycle can follow. // // This class is thread-safe. class ConstraintBuilder { public: using Constraint = mapping::SparsePoseGraph::Constraint; using Result = std::vector; ConstraintBuilder( const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions& options, common::ThreadPool* thread_pool); ~ConstraintBuilder(); ConstraintBuilder(const ConstraintBuilder&) = delete; ConstraintBuilder& operator=(const ConstraintBuilder&) = delete; // Schedules exploring a new constraint between 'submap' identified by // 'submap_id', and the 'point_cloud' for 'node_id'. The 'intial_pose' is // relative to the 'submap'. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. void MaybeAddConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, const sensor::CompressedPointCloud* compressed_point_cloud, const std::vector& submap_nodes, const transform::Rigid3d& initial_pose); // Schedules exploring a new constraint between 'submap' identified by // 'submap_id' and the 'compressed_point_cloud' for 'node_id'. // This performs full-submap matching. // // The 'gravity_alignment' is the rotation to apply to the point cloud data // to make it approximately gravity aligned. The 'trajectory_connectivity' is // updated if the full-submap match succeeds. // // The pointees of 'submap' and 'compressed_point_cloud' must stay valid until // all computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, const sensor::CompressedPointCloud* compressed_point_cloud, const std::vector& submap_nodes, const Eigen::Quaterniond& gravity_alignment, mapping::TrajectoryConnectivity* trajectory_connectivity); // Must be called after all computations related to one node have been added. void NotifyEndOfScan(); // Registers the 'callback' to be called with the results, after all // computations triggered by MaybeAddConstraint() have finished. void WhenDone(std::function callback); // Returns the number of consecutive finished scans. int GetNumFinishedScans(); private: struct SubmapScanMatcher { const HybridGrid* hybrid_grid; std::unique_ptr fast_correlative_scan_matcher; }; // Either schedules the 'work_item', or if needed, schedules the scan matcher // construction and queues the 'work_item'. void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, const HybridGrid* submap, std::function work_item) REQUIRES(mutex_); // Constructs the scan matcher for a 'submap', then schedules its work items. void ConstructSubmapScanMatcher( const mapping::SubmapId& submap_id, const std::vector& submap_nodes, const HybridGrid* submap) EXCLUDES(mutex_); // Returns the scan matcher for a submap, which has to exist. const SubmapScanMatcher* GetSubmapScanMatcher( const mapping::SubmapId& submap_id) EXCLUDES(mutex_); // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'point_cloud' do not change anymore. // If 'match_full_submap' is true, and global localization succeeds, will // connect 'node_id.trajectory_id' and 'submap_id.trajectory_id' in // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* submap, const mapping::NodeId& node_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* compressed_point_cloud, const transform::Rigid3d& initial_pose, std::unique_ptr* constraint) EXCLUDES(mutex_); // Decrements the 'pending_computations_' count. If all computations are done, // runs the 'when_done_' callback and resets the state. void FinishComputation(int computation_index) EXCLUDES(mutex_); const mapping::sparse_pose_graph::proto::ConstraintBuilderOptions options_; common::ThreadPool* thread_pool_; common::Mutex mutex_; // 'callback' set by WhenDone(). std::unique_ptr> when_done_ GUARDED_BY(mutex_); // Index of the scan in reaction to which computations are currently // added. This is always the highest scan index seen so far, even when older // scans are matched against a new submap. int current_computation_ GUARDED_BY(mutex_) = 0; // For each added scan, maps to the number of pending computations that were // added for it. std::map pending_computations_ GUARDED_BY(mutex_); // Constraints currently being computed in the background. A deque is used to // keep pointers valid when adding more entries. std::deque> constraints_ GUARDED_BY(mutex_); // Map of already constructed scan matchers by 'submap_id'. std::map submap_scan_matchers_ GUARDED_BY(mutex_); // Map by 'submap_id' of scan matchers under construction, and the work // to do once construction is done. std::map>> submap_queued_work_items_ GUARDED_BY(mutex_); common::FixedRatioSampler sampler_; const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_; scan_matching::CeresScanMatcher ceres_scan_matcher_; // Histogram of scan matcher scores. common::Histogram score_histogram_ GUARDED_BY(mutex_); }; } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ ================================================ FILE: mapping_3d/sparse_pose_graph/optimization_problem.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include #include #include #include #include #include #include #include "Eigen/Core" #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" #include "cartographer/mapping_3d/acceleration_cost_function.h" #include "cartographer/mapping_3d/ceres_pose.h" #include "cartographer/mapping_3d/rotation_cost_function.h" #include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" #include "ceres/rotation.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { namespace { struct ConstantYawQuaternionPlus { template bool operator()(const T* x, const T* delta, T* x_plus_delta) const { const T delta_norm = ceres::sqrt(common::Pow2(delta[0]) + common::Pow2(delta[1])); const T sin_delta_over_delta = delta_norm < 1e-6 ? T(1.) : ceres::sin(delta_norm) / delta_norm; T q_delta[4]; q_delta[0] = delta_norm < 1e-6 ? T(1.) : ceres::cos(delta_norm); q_delta[1] = sin_delta_over_delta * delta[0]; q_delta[2] = sin_delta_over_delta * delta[1]; q_delta[3] = T(0.); // We apply the 'delta' which is interpreted as an angle-axis rotation // vector in the xy-plane of the submap frame. This way we can align to // gravity because rotations around the z-axis in the submap frame do not // change gravity alignment, while disallowing random rotations of the map // that have nothing to do with gravity alignment (i.e. we disallow steps // just changing "yaw" of the complete map). ceres::QuaternionProduct(x, q_delta, x_plus_delta); return true; } }; } // namespace OptimizationProblem::OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z) : options_(options), fix_z_(fix_z) {} OptimizationProblem::~OptimizationProblem() {} void OptimizationProblem::AddImuData(const int trajectory_id, const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { CHECK_GE(trajectory_id, 0); imu_data_.resize( std::max(imu_data_.size(), static_cast(trajectory_id) + 1)); imu_data_[trajectory_id].push_back( ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( const int trajectory_id, const common::Time time, const transform::Rigid3d& point_cloud_pose) { CHECK_GE(trajectory_id, 0); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, const transform::Rigid3d& submap_pose) { CHECK_GE(trajectory_id, 0); submap_data_.resize( std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); submap_data_[trajectory_id].push_back(SubmapData{submap_pose}); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { options_.mutable_ceres_solver_options()->set_max_num_iterations( max_num_iterations); } void OptimizationProblem::Solve(const std::vector& constraints) { if (node_data_.empty()) { // Nothing to optimize. return; } ceres::Problem::Options problem_options; ceres::Problem problem(problem_options); const auto translation_parameterization = [this]() -> std::unique_ptr { return fix_z_ == FixZ::kYes ? common::make_unique( 3, std::vector{2}) : nullptr; }; // Set the starting point. CHECK(!submap_data_.empty()); CHECK(!submap_data_[0].empty()); // TODO(hrapp): Move ceres data into SubmapData. std::vector> C_submaps(submap_data_.size()); std::vector> C_nodes(node_data_.size()); for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; submap_index != submap_data_[trajectory_id].size(); ++submap_index) { if (trajectory_id == 0 && submap_index == 0) { // Tie the first submap of the first trajectory to the origin. C_submaps[trajectory_id].emplace_back( transform::Rigid3d::Identity(), translation_parameterization(), common::make_unique>(), &problem); problem.SetParameterBlockConstant( C_submaps[trajectory_id].back().translation()); } else { C_submaps[trajectory_id].emplace_back( submap_data_[trajectory_id][submap_index].pose, translation_parameterization(), common::make_unique(), &problem); } } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { C_nodes[trajectory_id].emplace_back( node_data_[trajectory_id][node_index].point_cloud_pose, translation_parameterization(), common::make_unique(), &problem); } } // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new SpaCostFunction(constraint.pose)), // Only loop closure constraints should have a loss function. constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) : nullptr, C_submaps.at(constraint.submap_id.trajectory_id) .at(constraint.submap_id.submap_index) .rotation(), C_submaps.at(constraint.submap_id.trajectory_id) .at(constraint.submap_id.submap_index) .translation(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index) .rotation(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index) .translation()); } // Add constraints based on IMU observations of angular velocities and // linear acceleration. for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { const std::deque& imu_data = imu_data_.at(trajectory_id); CHECK(!imu_data.empty()); // TODO(whess): Add support for empty trajectories. const auto& node_data = node_data_[trajectory_id]; CHECK(!node_data.empty()); // Skip IMU data before the first node of this trajectory. auto it = imu_data.cbegin(); while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) { ++it; } for (size_t node_index = 1; node_index < node_data.size(); ++node_index) { auto it2 = it; const IntegrateImuResult result = IntegrateImu(imu_data, node_data[node_index - 1].time, node_data[node_index].time, &it); if (node_index + 1 < node_data.size()) { const common::Time first_time = node_data[node_index - 1].time; const common::Time second_time = node_data[node_index].time; const common::Time third_time = node_data[node_index + 1].time; const common::Duration first_duration = second_time - first_time; const common::Duration second_duration = third_time - second_time; const common::Time first_center = first_time + first_duration / 2; const common::Time second_center = second_time + second_duration / 2; const IntegrateImuResult result_to_first_center = IntegrateImu(imu_data, first_time, first_center, &it2); const IntegrateImuResult result_center_to_center = IntegrateImu(imu_data, first_center, second_center, &it2); // 'delta_velocity' is the change in velocity from the point in time // halfway between the first and second poses to halfway between second // and third pose. It is computed from IMU data and still contains a // delta due to gravity. The orientation of this vector is in the IMU // frame at the second pose. const Eigen::Vector3d delta_velocity = (result.delta_rotation.inverse() * result_to_first_center.delta_rotation) * result_center_to_center.delta_velocity; problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new AccelerationCostFunction( options_.acceleration_weight(), delta_velocity, common::ToSeconds(first_duration), common::ToSeconds(second_duration))), nullptr, C_nodes[trajectory_id].at(node_index).rotation(), C_nodes[trajectory_id].at(node_index - 1).translation(), C_nodes[trajectory_id].at(node_index).translation(), C_nodes[trajectory_id].at(node_index + 1).translation(), &gravity_constant_); } problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new RotationCostFunction(options_.rotation_weight(), result.delta_rotation)), nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(), C_nodes[trajectory_id].at(node_index).rotation()); } } // Solve. ceres::Solver::Summary summary; ceres::Solve( common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary); if (options_.log_solver_summary()) { LOG(INFO) << summary.FullReport(); LOG(INFO) << "Gravity was: " << gravity_constant_; } // Store the result. for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { for (size_t submap_index = 0; submap_index != submap_data_[trajectory_id].size(); ++submap_index) { submap_data_[trajectory_id][submap_index].pose = C_submaps[trajectory_id][submap_index].ToRigid(); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); ++trajectory_id) { for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); ++node_index) { node_data_[trajectory_id][node_index].point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid(); } } } const std::vector>& OptimizationProblem::node_data() const { return node_data_; } const std::vector>& OptimizationProblem::submap_data() const { return submap_data_; } } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/sparse_pose_graph/optimization_problem.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping_3d/imu_integration.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { struct NodeData { common::Time time; transform::Rigid3d point_cloud_pose; }; struct SubmapData { transform::Rigid3d pose; }; // Implements the SPA loop closure method. class OptimizationProblem { public: using Constraint = mapping::SparsePoseGraph::Constraint; enum class FixZ { kYes, kNo }; OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z); ~OptimizationProblem(); OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid3d& point_cloud_pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void SetMaxNumIterations(int32 max_num_iterations); // Computes the optimized poses. void Solve(const std::vector& constraints); const std::vector>& node_data() const; const std::vector>& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::vector> imu_data_; std::vector> node_data_; std::vector> submap_data_; double gravity_constant_ = 9.8; }; } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_ ================================================ FILE: mapping_3d/sparse_pose_graph/optimization_problem_test.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { namespace { class OptimizationProblemTest : public ::testing::Test { protected: OptimizationProblemTest() : optimization_problem_(CreateOptions(), OptimizationProblem::FixZ::kNo), rng_(45387) {} mapping::sparse_pose_graph::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { acceleration_weight = 1e-4, rotation_weight = 1e-2, huber_scale = 1., consecutive_scan_translation_penalty_factor = 1e-2, consecutive_scan_rotation_penalty_factor = 1e-2, log_solver_summary = true, ceres_solver_options = { use_nonmonotonic_steps = false, max_num_iterations = 200, num_threads = 4, }, })text"); return mapping::sparse_pose_graph::CreateOptimizationProblemOptions( parameter_dictionary.get()); } transform::Rigid3d RandomTransform(double translation_size, double rotation_size) { std::uniform_real_distribution translation_distribution( -translation_size, translation_size); const double x = translation_distribution(rng_); const double y = translation_distribution(rng_); const double z = translation_distribution(rng_); std::uniform_real_distribution rotation_distribution(-rotation_size, rotation_size); const double rx = rotation_distribution(rng_); const double ry = rotation_distribution(rng_); const double rz = rotation_distribution(rng_); return transform::Rigid3d(Eigen::Vector3d(x, y, z), transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(rx, ry, rz))); } transform::Rigid3d RandomYawOnlyTransform(double translation_size, double rotation_size) { std::uniform_real_distribution translation_distribution( -translation_size, translation_size); const double x = translation_distribution(rng_); const double y = translation_distribution(rng_); const double z = translation_distribution(rng_); std::uniform_real_distribution rotation_distribution(-rotation_size, rotation_size); const double rz = rotation_distribution(rng_); return transform::Rigid3d(Eigen::Vector3d(x, y, z), transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(0., 0., rz))); } OptimizationProblem optimization_problem_; std::mt19937 rng_; }; transform::Rigid3d AddNoise(const transform::Rigid3d& transform, const transform::Rigid3d& noise) { const Eigen::Quaterniond noisy_rotation(noise.rotation() * transform.rotation()); return transform::Rigid3d(transform.translation() + noise.translation(), noisy_rotation); } TEST_F(OptimizationProblemTest, ReducesNoise) { constexpr int kNumNodes = 100; const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity(); const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); const int kTrajectoryId = 0; struct NoisyNode { transform::Rigid3d ground_truth_pose; transform::Rigid3d noise; }; std::vector test_data; for (int j = 0; j != kNumNodes; ++j) { test_data.push_back( NoisyNode{RandomTransform(10., 3.), RandomYawOnlyTransform(0.2, 0.3)}); } common::Time now = common::FromUniversal(0); for (const NoisyNode& node : test_data) { const transform::Rigid3d pose = AddNoise(node.ground_truth_pose, node.noise); optimization_problem_.AddImuData(kTrajectoryId, now, Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::Zero()); optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose); now += common::FromSeconds(0.01); } std::vector constraints; for (int j = 0; j != kNumNodes; ++j) { constraints.push_back(OptimizationProblem::Constraint{ mapping::SubmapId{0, 0}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, test_data[j].noise), 1., 1.}}); // We add an additional independent, but equally noisy observation. constraints.push_back(OptimizationProblem::Constraint{ mapping::SubmapId{0, 1}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ AddNoise(test_data[j].ground_truth_pose, RandomYawOnlyTransform(0.2, 0.3)), 1., 1.}}); // We add very noisy data with a low weight to verify it is mostly ignored. constraints.push_back(OptimizationProblem::Constraint{ mapping::SubmapId{0, 2}, mapping::NodeId{0, j}, OptimizationProblem::Constraint::Pose{ kSubmap2Transform.inverse() * test_data[j].ground_truth_pose * RandomTransform(1e3, 3.), 1e-9, 1e-9}}); } double translation_error_before = 0.; double rotation_error_before = 0.; const auto& node_data = optimization_problem_.node_data().at(0); for (int j = 0; j != kNumNodes; ++j) { translation_error_before += (test_data[j].ground_truth_pose.translation() - node_data[j].point_cloud_pose.translation()) .norm(); rotation_error_before += transform::GetAngle(test_data[j].ground_truth_pose.inverse() * node_data[j].point_cloud_pose); } optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform); optimization_problem_.Solve(constraints); double translation_error_after = 0.; double rotation_error_after = 0.; for (int j = 0; j != kNumNodes; ++j) { translation_error_after += (test_data[j].ground_truth_pose.translation() - node_data[j].point_cloud_pose.translation()) .norm(); rotation_error_after += transform::GetAngle(test_data[j].ground_truth_pose.inverse() * node_data[j].point_cloud_pose); } EXPECT_GT(0.8 * translation_error_before, translation_error_after); EXPECT_GT(0.8 * rotation_error_before, rotation_error_after); } } // namespace } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/sparse_pose_graph/spa_cost_function.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" namespace cartographer { namespace mapping_3d { namespace sparse_pose_graph { class SpaCostFunction { public: using Constraint = mapping::SparsePoseGraph::Constraint; explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} // Computes the error between the scan-to-submap alignment 'zbar_ij' and the // difference of submap pose 'c_i' and scan pose 'c_j' which are both in an // arbitrary common frame. template static std::array ComputeUnscaledError( const transform::Rigid3d& zbar_ij, const T* const c_i_rotation, const T* const c_i_translation, const T* const c_j_rotation, const T* const c_j_translation) { const Eigen::Quaternion R_i_inverse(c_i_rotation[0], -c_i_rotation[1], -c_i_rotation[2], -c_i_rotation[3]); const Eigen::Matrix delta(c_j_translation[0] - c_i_translation[0], c_j_translation[1] - c_i_translation[1], c_j_translation[2] - c_i_translation[2]); const Eigen::Matrix h_translation = R_i_inverse * delta; const Eigen::Quaternion h_rotation_inverse = Eigen::Quaternion(c_j_rotation[0], -c_j_rotation[1], -c_j_rotation[2], -c_j_rotation[3]) * Eigen::Quaternion(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2], c_i_rotation[3]); const Eigen::Matrix angle_axis_difference = transform::RotationQuaternionToAngleAxisVector( h_rotation_inverse * zbar_ij.rotation().cast()); return {{T(zbar_ij.translation().x()) - h_translation[0], T(zbar_ij.translation().y()) - h_translation[1], T(zbar_ij.translation().z()) - h_translation[2], angle_axis_difference[0], angle_axis_difference[1], angle_axis_difference[2]}}; } // Computes the error scaled by 'translation_weight' and 'rotation_weight', // storing it in 'e'. template static void ComputeScaledError(const Constraint::Pose& pose, const T* const c_i_rotation, const T* const c_i_translation, const T* const c_j_rotation, const T* const c_j_translation, T* const e) { const std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation, c_j_translation); for (int ij : {0, 1, 2}) { e[ij] = e_ij[ij] * T(pose.translation_weight); } for (int ij : {3, 4, 5}) { e[ij] = e_ij[ij] * T(pose.rotation_weight); } } template bool operator()(const T* const c_i_rotation, const T* const c_i_translation, const T* const c_j_rotation, const T* const c_j_translation, T* const e) const { ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation, c_j_translation, e); return true; } private: const Constraint::Pose pose_; }; } // namespace sparse_pose_graph } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ ================================================ FILE: mapping_3d/sparse_pose_graph.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/sparse_pose_graph.h" #include #include #include #include #include #include #include #include #include #include #include "Eigen/Eigenvalues" #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/voxel_filter.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { SparsePoseGraph::SparsePoseGraph( const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool) : options_(options), optimization_problem_(options_.optimization_problem_options(), sparse_pose_graph::OptimizationProblem::FixZ::kNo), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} SparsePoseGraph::~SparsePoseGraph() { WaitForAllComputations(); common::MutexLocker locker(&mutex_); CHECK(scan_queue_ == nullptr); } void SparsePoseGraph::GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) { CHECK(!insertion_submaps.empty()); const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]); const int trajectory_id = first_submap_id.trajectory_id; CHECK_GE(trajectory_id, 0); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. CHECK_EQ(first_submap_id.submap_index, 0); if (static_cast(trajectory_id) >= submap_data.size() || submap_data[trajectory_id].empty()) { optimization_problem_.AddSubmap(trajectory_id, transform::Rigid3d::Identity()); } return; } CHECK_EQ(2, insertion_submaps.size()); const int next_submap_index = submap_data.at(trajectory_id).size(); // CHECK that we have a index for the second submap. const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]); CHECK_EQ(second_submap_id.trajectory_id, trajectory_id); CHECK_LE(second_submap_id.submap_index, next_submap_index); // Extrapolate if necessary. if (second_submap_id.submap_index == next_submap_index) { const auto& first_submap_pose = submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * insertion_submaps[0]->local_pose.inverse() * insertion_submaps[1]->local_pose); } } void SparsePoseGraph::AddScan( common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, const int trajectory_id, const Submap* const matching_submap, const std::vector& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, sensor::Compress(range_data_in_tracking), trajectory_id, transform::Rigid3d::Identity()}); trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); ++num_trajectory_nodes_; trajectory_connectivity_.Add(trajectory_id); if (submap_ids_.count(insertion_submaps.back()) == 0) { const mapping::SubmapId submap_id = submap_states_.Append(trajectory_id, SubmapState()); submap_ids_.emplace(insertion_submaps.back(), submap_id); submap_states_.at(submap_id).submap = insertion_submaps.back(); } const Submap* const finished_submap = insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { global_localization_samplers_[trajectory_id] = common::make_unique( options_.global_sampling_ratio()); } AddWorkItem([=]() REQUIRES(mutex_) { ComputeConstraintsForScan(matching_submap, insertion_submaps, finished_submap, pose); }); } void SparsePoseGraph::AddWorkItem(std::function work_item) { if (scan_queue_ == nullptr) { work_item(); } else { scan_queue_->push_back(work_item); } } void SparsePoseGraph::AddImuData(const int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { optimization_problem_.AddImuData(trajectory_id, time, linear_acceleration, angular_velocity); }); } void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { const transform::Rigid3d inverse_submap_pose = optimization_problem_.submap_data() .at(submap_id.trajectory_id) .at(submap_id.submap_index) .pose.inverse(); const transform::Rigid3d initial_relative_pose = inverse_submap_pose * optimization_problem_.node_data() .at(node_id.trajectory_id) .at(node_id.node_index) .point_cloud_pose; std::vector submap_nodes; for (const mapping::NodeId& submap_node_id : submap_states_.at(submap_id).node_ids) { submap_nodes.push_back(mapping::TrajectoryNode{ trajectory_nodes_.at(submap_node_id).constant_data, inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose}); } // Only globally match against submaps not in this trajectory. if (node_id.trajectory_id != submap_id.trajectory_id && global_localization_samplers_[node_id.trajectory_id]->Pulse()) { // In this situation, 'initial_relative_pose' is: // // submap <- global map 2 <- global map 1 <- tracking // (agreeing on gravity) // // Since they possibly came from two disconnected trajectories, the only // guaranteed connection between the tracking and the submap frames is // an agreement on the direction of gravity. Therefore, excluding yaw, // 'initial_relative_pose.rotation()' is a good estimate of the relative // orientation of the point cloud in the submap frame. Finding the correct // yaw component will be handled by the matching procedure in the // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_.at(submap_id).submap, node_id, &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, submap_nodes, initial_relative_pose.rotation(), &trajectory_connectivity_); } else { const bool scan_and_submap_trajectories_connected = reverse_connected_components_.count(node_id.trajectory_id) > 0 && reverse_connected_components_.count(submap_id.trajectory_id) > 0 && reverse_connected_components_.at(node_id.trajectory_id) == reverse_connected_components_.at(submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( submap_id, submap_states_.at(submap_id).submap, node_id, &trajectory_nodes_.at(node_id).constant_data->range_data_3d.returns, submap_nodes, initial_relative_pose); } } } void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const auto submap_id = GetSubmapId(submap); const auto& submap_state = submap_states_.at(submap_id); const auto& node_data = optimization_problem_.node_data(); for (size_t trajectory_id = 0; trajectory_id != node_data.size(); ++trajectory_id) { for (size_t node_index = 0; node_index != node_data[trajectory_id].size(); ++node_index) { const mapping::NodeId node_id{static_cast(trajectory_id), static_cast(node_index)}; if (submap_state.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } } } } void SparsePoseGraph::ComputeConstraintsForScan( const Submap* matching_submap, std::vector insertion_submaps, const Submap* finished_submap, const transform::Rigid3d& pose) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid3d optimized_pose = optimization_problem_.submap_data() .at(matching_id.trajectory_id) .at(matching_id.submap_index) .pose * matching_submap->local_pose.inverse() * pose; const mapping::NodeId node_id{ matching_id.trajectory_id, static_cast(matching_id.trajectory_id) < optimization_problem_.node_data().size() ? static_cast(optimization_problem_.node_data() .at(matching_id.trajectory_id) .size()) : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_.at(node_id).constant_data; CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, scan_data->time, optimized_pose); for (const Submap* submap : insertion_submaps) { const mapping::SubmapId submap_id = GetSubmapId(submap); CHECK(!submap_states_.at(submap_id).finished); submap_states_.at(submap_id).node_ids.emplace(node_id); const transform::Rigid3d constraint_transform = submap->local_pose.inverse() * pose; constraints_.push_back( Constraint{submap_id, node_id, {constraint_transform, options_.matcher_translation_weight(), options_.matcher_rotation_weight()}, Constraint::INTRA_SUBMAP}); } for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories(); ++trajectory_id) { for (int submap_index = 0; submap_index < submap_states_.num_indices(trajectory_id); ++submap_index) { const mapping::SubmapId submap_id{trajectory_id, submap_index}; if (submap_states_.at(submap_id).finished) { CHECK_EQ(submap_states_.at(submap_id).node_ids.count(node_id), 0); ComputeConstraint(node_id, submap_id); } } } if (finished_submap != nullptr) { const mapping::SubmapId finished_submap_id = GetSubmapId(finished_submap); SubmapState& finished_submap_state = submap_states_.at(finished_submap_id); CHECK(!finished_submap_state.finished); // We have a new completed submap, so we look into adding constraints for // old scans. ComputeConstraintsForOldScans(finished_submap); finished_submap_state.finished = true; } constraint_builder_.NotifyEndOfScan(); ++num_scans_since_last_loop_closure_; if (options_.optimize_every_n_scans() > 0 && num_scans_since_last_loop_closure_ > options_.optimize_every_n_scans()) { CHECK(!run_loop_closure_); run_loop_closure_ = true; // If there is a 'scan_queue_' already, some other thread will take care. if (scan_queue_ == nullptr) { scan_queue_ = common::make_unique>>(); HandleScanQueue(); } } } void SparsePoseGraph::HandleScanQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); common::MutexLocker locker(&mutex_); num_scans_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { if (scan_queue_->empty()) { LOG(INFO) << "We caught up. Hooray!"; scan_queue_.reset(); return; } scan_queue_->front()(); scan_queue_->pop_front(); } // We have to optimize again. HandleScanQueue(); }); } void SparsePoseGraph::WaitForAllComputations() { bool notification = false; common::MutexLocker locker(&mutex_); const int num_finished_scans_at_start = constraint_builder_.GetNumFinishedScans(); while (!locker.AwaitWithTimeout( [this]() REQUIRES(mutex_) { return constraint_builder_.GetNumFinishedScans() == num_trajectory_nodes_; }, common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) << 100. * (constraint_builder_.GetNumFinishedScans() - num_finished_scans_at_start) / (num_trajectory_nodes_ - num_finished_scans_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; constraint_builder_.WhenDone( [this, ¬ification]( const sparse_pose_graph::ConstraintBuilder::Result& result) { common::MutexLocker locker(&mutex_); constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; }); locker.Await([¬ification]() { return notification; }); } void SparsePoseGraph::RunFinalOptimization() { WaitForAllComputations(); optimization_problem_.SetMaxNumIterations( options_.max_num_final_iterations()); RunOptimization(); optimization_problem_.SetMaxNumIterations( options_.optimization_problem_options() .ceres_solver_options() .max_num_iterations()); } void SparsePoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; } optimization_problem_.Solve(constraints_); common::MutexLocker locker(&mutex_); const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { int node_index = 0; const int num_nodes = trajectory_nodes_.num_indices(trajectory_id); for (; node_index != static_cast(node_data[trajectory_id].size()); ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = node_data[trajectory_id][node_index].point_cloud_pose; } // Extrapolate all point cloud poses that were added later. const auto new_submap_transforms = ExtrapolateSubmapTransforms( optimization_problem_.submap_data(), trajectory_id); const auto old_submap_transforms = ExtrapolateSubmapTransforms( optimized_submap_transforms_, trajectory_id); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); const transform::Rigid3d extrapolation_transform = new_submap_transforms.back() * old_submap_transforms.back().inverse(); for (; node_index < num_nodes; ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = extrapolation_transform * trajectory_nodes_.at(node_id).pose; } } optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { for (const int trajectory_id : connected_components_[i]) { reverse_connected_components_.emplace(trajectory_id, i); } } } std::vector> SparsePoseGraph::GetTrajectoryNodes() { common::MutexLocker locker(&mutex_); return trajectory_nodes_.data(); } std::vector SparsePoseGraph::constraints() { common::MutexLocker locker(&mutex_); return constraints_; } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); if (submap_states_.num_trajectories() <= trajectory_id || submap_states_.num_indices(trajectory_id) == 0) { return transform::Rigid3d::Identity(); } const auto extrapolated_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); return extrapolated_submap_transforms.back() * submap_states_ .at(mapping::SubmapId{ trajectory_id, static_cast(extrapolated_submap_transforms.size()) - 1}) .submap->local_pose.inverse(); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { common::MutexLocker locker(&mutex_); return connected_components_; } std::vector SparsePoseGraph::GetSubmapTransforms( const int trajectory_id) { common::MutexLocker locker(&mutex_); return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory_id); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, const int trajectory_id) const { if (trajectory_id >= submap_states_.num_trajectories()) { return {transform::Rigid3d::Identity()}; } // Submaps for which we have optimized poses. std::vector result; for (int submap_index = 0; submap_index != submap_states_.num_indices(trajectory_id); ++submap_index) { const mapping::SubmapId submap_id{trajectory_id, submap_index}; const auto& state = submap_states_.at(submap_id); if (static_cast(trajectory_id) < submap_transforms.size() && result.size() < submap_transforms.at(trajectory_id).size()) { // Submaps for which we have optimized poses. result.push_back( submap_transforms.at(trajectory_id).at(result.size()).pose); } else if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } else { // Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps // is okay, since the member is const. const mapping::SubmapId previous_submap_id{ trajectory_id, static_cast(result.size()) - 1}; result.push_back( result.back() * submap_states_.at(previous_submap_id).submap->local_pose.inverse() * state.submap->local_pose); } } if (result.empty()) { result.push_back(transform::Rigid3d::Identity()); } return result; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/sparse_pose_graph.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #include #include #include #include #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/submaps.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_3d { // Implements the loop closure method called Sparse Pose Adjustment (SPA) from // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." // Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference // on (pp. 22--29). IEEE, 2010. // // It is extended for submapping in 3D: // Each scan has been matched against one or more submaps (adding a constraint // for each match), both poses of scans and of submaps are to be optimized. // All constraints are between a submap i and a scan j. class SparsePoseGraph : public mapping::SparsePoseGraph { public: SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, common::ThreadPool* thread_pool); ~SparsePoseGraph() override; SparsePoseGraph(const SparsePoseGraph&) = delete; SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; // Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose' // that will later be optimized. The 'pose' was determined by scan matching // against the 'matching_submap' and the scan was inserted into the // 'insertion_submaps'. void AddScan(common::Time time, const sensor::RangeData& range_data_in_tracking, const transform::Rigid3d& pose, int trajectory_id, const Submap* matching_submap, const std::vector& insertion_submaps) EXCLUDES(mutex_); // Adds new IMU data to be used in the optimization. void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms(int trajectory_id) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); private: struct SubmapState { const Submap* submap = nullptr; // IDs of the scans that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. std::set node_ids; // Whether in the current state of the background thread this submap is // finished. When this transitions to true, all scans are tried to match // against this submap. Likewise, all new scans are matched against submaps // which are finished. bool finished = false; }; // Handles a new work item. void AddWorkItem(std::function work_item) REQUIRES(mutex_); mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const REQUIRES(mutex_) { const auto iterator = submap_ids_.find(submap); CHECK(iterator != submap_ids_.end()); return iterator->second; } // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. void GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) REQUIRES(mutex_); // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan(const Submap* matching_submap, std::vector insertion_submaps, const Submap* finished_submap, const transform::Rigid3d& pose) REQUIRES(mutex_); // Computes constraints for a scan and submap pair. void ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) REQUIRES(mutex_); // Adds constraints for older scans whenever a new submap is finished. void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_); // Registers the callback to run the optimization once all constraints have // been computed, that will also do all work that queue up in 'scan_queue_'. void HandleScanQueue() REQUIRES(mutex_); // Waits until we caught up (i.e. nothing is waiting to be scheduled), and // all computations have finished. void WaitForAllComputations() EXCLUDES(mutex_); // Runs the optimization. Callers have to make sure, that there is only one // optimization being run at a time. void RunOptimization() EXCLUDES(mutex_); // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, int trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; // If it exists, further scans must be added to this queue, and will be // considered later. std::unique_ptr>> scan_queue_ GUARDED_BY(mutex_); // How our various trajectories are related. mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); // We globally localize a fraction of the scans from each trajectory. std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); // Number of scans added since last loop closure. int num_scans_since_last_loop_closure_ GUARDED_BY(mutex_) = 0; // Whether the optimization has to be run before more data is added. bool run_loop_closure_ GUARDED_BY(mutex_) = false; // Current optimization problem. sparse_pose_graph::OptimizationProblem optimization_problem_; sparse_pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); // Submaps get assigned an ID and state as soon as they are seen, even // before they take part in the background computations. std::map submap_ids_ GUARDED_BY(mutex_); mapping::NestedVectorsById submap_states_ GUARDED_BY(mutex_); // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID. std::map reverse_connected_components_; // Data that are currently being shown. // // Deque to keep references valid for the background computation when adding // new data. std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ ================================================ FILE: mapping_3d/submaps.cc ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "cartographer/mapping_3d/submaps.h" #include #include #include "cartographer/common/math.h" #include "cartographer/sensor/range_data.h" #include "glog/logging.h" namespace cartographer { namespace mapping_3d { namespace { constexpr float kSliceHalfHeight = 0.1f; struct RaySegment { Eigen::Vector2f from; Eigen::Vector2f to; bool hit; // Whether there is a hit at 'to'. }; // We compute a slice around the xy-plane. 'transform' is applied to the rays in // global map frame to allow choosing an arbitrary slice. void GenerateSegmentForSlice(const sensor::RangeData& range_data, const transform::Rigid3f& pose, const transform::Rigid3f& transform, std::vector* segments) { const sensor::RangeData transformed_range_data = sensor::TransformRangeData(range_data, transform * pose); segments->reserve(transformed_range_data.returns.size()); for (const Eigen::Vector3f& hit : transformed_range_data.returns) { const Eigen::Vector2f origin_xy = transformed_range_data.origin.head<2>(); const float origin_z = transformed_range_data.origin.z(); const float delta_z = hit.z() - origin_z; const Eigen::Vector2f delta_xy = hit.head<2>() - origin_xy; if (origin_z < -kSliceHalfHeight) { // Ray originates below the slice. if (hit.z() > kSliceHalfHeight) { // Ray is cutting through the slice. segments->push_back(RaySegment{ origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() > -kSliceHalfHeight) { // Hit is inside the slice. segments->push_back(RaySegment{ origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, hit.head<2>(), true}); } } else if (origin_z < kSliceHalfHeight) { // Ray originates inside the slice. if (hit.z() < -kSliceHalfHeight) { // Hit is below. segments->push_back(RaySegment{ origin_xy, origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() < kSliceHalfHeight) { // Full ray is inside the slice. segments->push_back(RaySegment{origin_xy, hit.head<2>(), true}); } else { // Hit is above. segments->push_back(RaySegment{ origin_xy, origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } } else { // Ray originates above the slice. if (hit.z() < -kSliceHalfHeight) { // Ray is cutting through the slice. segments->push_back(RaySegment{ origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, origin_xy + (-kSliceHalfHeight - origin_z) / delta_z * delta_xy, false}); } else if (hit.z() < kSliceHalfHeight) { // Hit is inside the slice. segments->push_back(RaySegment{ origin_xy + (kSliceHalfHeight - origin_z) / delta_z * delta_xy, hit.head<2>(), true}); } } } } void UpdateFreeSpaceFromSegment(const RaySegment& segment, const std::vector& miss_table, mapping_2d::ProbabilityGrid* result) { Eigen::Array2i from = result->limits().GetXYIndexOfCellContainingPoint( segment.from.x(), segment.from.y()); Eigen::Array2i to = result->limits().GetXYIndexOfCellContainingPoint( segment.to.x(), segment.to.y()); bool large_delta_y = std::abs(to.y() - from.y()) > std::abs(to.x() - from.x()); if (large_delta_y) { std::swap(from.x(), from.y()); std::swap(to.x(), to.y()); } if (from.x() > to.x()) { std::swap(from, to); } const int dx = to.x() - from.x(); const int dy = std::abs(to.y() - from.y()); int error = dx / 2; const int direction = (from.y() < to.y()) ? 1 : -1; for (; from.x() < to.x(); ++from.x()) { if (large_delta_y) { result->ApplyLookupTable(Eigen::Array2i(from.y(), from.x()), miss_table); } else { result->ApplyLookupTable(from, miss_table); } error -= dy; if (error < 0) { from.y() += direction; error += dx; } } } void InsertSegmentsIntoProbabilityGrid(const std::vector& segments, const std::vector& hit_table, const std::vector& miss_table, mapping_2d::ProbabilityGrid* result) { result->StartUpdate(); if (segments.empty()) { return; } Eigen::Vector2f min = segments.front().from; Eigen::Vector2f max = min; for (const RaySegment& segment : segments) { min = min.cwiseMin(segment.from); min = min.cwiseMin(segment.to); max = max.cwiseMax(segment.from); max = max.cwiseMax(segment.to); } const float padding = 10. * result->limits().resolution(); max += Eigen::Vector2f(padding, padding); min -= Eigen::Vector2f(padding, padding); result->GrowLimits(min.x(), min.y()); result->GrowLimits(max.x(), max.y()); for (const RaySegment& segment : segments) { if (segment.hit) { result->ApplyLookupTable(result->limits().GetXYIndexOfCellContainingPoint( segment.to.x(), segment.to.y()), hit_table); } } for (const RaySegment& segment : segments) { UpdateFreeSpaceFromSegment(segment, miss_table, result); } } // Filters 'range_data', retaining only the returns that have no more than // 'max_range' distance from the origin. Removes misses and reflectivity // information. sensor::RangeData FilterRangeDataByMaxRange(const sensor::RangeData& range_data, const float max_range) { sensor::RangeData result{range_data.origin, {}, {}}; for (const Eigen::Vector3f& hit : range_data.returns) { if ((hit - range_data.origin).norm() <= max_range) { result.returns.push_back(hit); } } return result; } } // namespace void InsertIntoProbabilityGrid( const sensor::RangeData& range_data, const transform::Rigid3f& pose, const float slice_z, const mapping_2d::RangeDataInserter& range_data_inserter, mapping_2d::ProbabilityGrid* result) { std::vector segments; GenerateSegmentForSlice( range_data, pose, transform::Rigid3f::Translation(-slice_z * Eigen::Vector3f::UnitZ()), &segments); InsertSegmentsIntoProbabilityGrid(segments, range_data_inserter.hit_table(), range_data_inserter.miss_table(), result); } proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary) { proto::SubmapsOptions options; options.set_high_resolution( parameter_dictionary->GetDouble("high_resolution")); options.set_high_resolution_max_range( parameter_dictionary->GetDouble("high_resolution_max_range")); options.set_low_resolution(parameter_dictionary->GetDouble("low_resolution")); options.set_num_range_data( parameter_dictionary->GetNonNegativeInt("num_range_data")); *options.mutable_range_data_inserter_options() = CreateRangeDataInserterOptions( parameter_dictionary->GetDictionary("range_data_inserter").get()); CHECK_GT(options.num_range_data(), 0); return options; } Submap::Submap(const float high_resolution, const float low_resolution, const transform::Rigid3d& local_pose) : mapping::Submap(local_pose), high_resolution_hybrid_grid(high_resolution), low_resolution_hybrid_grid(low_resolution) {} Submaps::Submaps(const proto::SubmapsOptions& options) : options_(options), range_data_inserter_(options.range_data_inserter_options()) { // We always want to have at least one submap which we can return and will // create it at the origin in absence of a better choice. // // TODO(whess): Start with no submaps, so that all of them can be // approximately gravity aligned. AddSubmap(transform::Rigid3d::Identity()); } const Submap* Submaps::Get(int index) const { CHECK_GE(index, 0); CHECK_LT(index, size()); return submaps_[index].get(); } int Submaps::size() const { return submaps_.size(); } void Submaps::SubmapToProto( int index, const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* const response) const { // Generate an X-ray view through the 'hybrid_grid', aligned to the xy-plane // in the global map frame. const HybridGrid& hybrid_grid = Get(index)->high_resolution_hybrid_grid; response->set_resolution(hybrid_grid.resolution()); // Compute a bounding box for the texture. Eigen::Array2i min_index(INT_MAX, INT_MAX); Eigen::Array2i max_index(INT_MIN, INT_MIN); const std::vector voxel_indices_and_probabilities = ExtractVoxelData(hybrid_grid, global_submap_pose.cast(), &min_index, &max_index); const int width = max_index.y() - min_index.y() + 1; const int height = max_index.x() - min_index.x() + 1; response->set_width(width); response->set_height(height); const std::vector accumulated_pixel_data = AccumulatePixelData( width, height, min_index, max_index, voxel_indices_and_probabilities); const string cell_data = ComputePixelValues(accumulated_pixel_data); common::FastGzipString(cell_data, response->mutable_cells()); *response->mutable_slice_pose() = transform::ToProto(global_submap_pose.inverse() * transform::Rigid3d::Translation(Eigen::Vector3d( max_index.x() * hybrid_grid.resolution(), max_index.y() * hybrid_grid.resolution(), global_submap_pose.translation().z()))); } void Submaps::InsertRangeData(const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment) { for (const int index : insertion_indices()) { Submap* submap = submaps_[index].get(); const sensor::RangeData transformed_range_data = sensor::TransformRangeData( range_data, submap->local_pose.inverse().cast()); range_data_inserter_.Insert( FilterRangeDataByMaxRange(transformed_range_data, options_.high_resolution_max_range()), &submap->high_resolution_hybrid_grid); range_data_inserter_.Insert(transformed_range_data, &submap->low_resolution_hybrid_grid); ++submap->num_range_data; } const Submap* const last_submap = Get(size() - 1); if (last_submap->num_range_data == options_.num_range_data()) { AddSubmap(transform::Rigid3d(range_data.origin.cast(), gravity_alignment)); } } void Submaps::AddSubmap(const transform::Rigid3d& local_pose) { if (size() > 1) { Submap* submap = submaps_[size() - 2].get(); CHECK(!submap->finished); submap->finished = true; } submaps_.emplace_back(new Submap(options_.high_resolution(), options_.low_resolution(), local_pose)); LOG(INFO) << "Added submap " << size(); } std::vector Submaps::AccumulatePixelData( const int width, const int height, const Eigen::Array2i& min_index, const Eigen::Array2i& max_index, const std::vector& voxel_indices_and_probabilities) const { std::vector accumulated_pixel_data(width * height); for (const Eigen::Array4i& voxel_index_and_probability : voxel_indices_and_probabilities) { const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); if ((pixel_index < min_index).any() || (pixel_index > max_index).any()) { // Out of bounds. This could happen because of floating point inaccuracy. continue; } const int x = max_index.x() - pixel_index[0]; const int y = max_index.y() - pixel_index[1]; PixelData& pixel = accumulated_pixel_data[x * width + y]; ++pixel.count; pixel.min_z = std::min(pixel.min_z, voxel_index_and_probability[2]); pixel.max_z = std::max(pixel.max_z, voxel_index_and_probability[2]); const float probability = mapping::ValueToProbability(voxel_index_and_probability[3]); pixel.probability_sum += probability; pixel.max_probability = std::max(pixel.max_probability, probability); } return accumulated_pixel_data; } std::vector Submaps::ExtractVoxelData( const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, Eigen::Array2i* min_index, Eigen::Array2i* max_index) const { std::vector voxel_indices_and_probabilities; const float resolution_inverse = 1. / hybrid_grid.resolution(); constexpr double kXrayObstructedCellProbabilityLimit = 0.501; for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) { const uint16 probability_value = it.GetValue(); const float probability = mapping::ValueToProbability(probability_value); if (probability < kXrayObstructedCellProbabilityLimit) { // We ignore non-obstructed cells. continue; } const Eigen::Vector3f cell_center_submap = hybrid_grid.GetCenterOfCell(it.GetCellIndex()); const Eigen::Vector3f cell_center_global = transform * cell_center_submap; const Eigen::Array4i voxel_index_and_probability( common::RoundToInt(cell_center_global.x() * resolution_inverse), common::RoundToInt(cell_center_global.y() * resolution_inverse), common::RoundToInt(cell_center_global.z() * resolution_inverse), probability_value); voxel_indices_and_probabilities.push_back(voxel_index_and_probability); const Eigen::Array2i pixel_index = voxel_index_and_probability.head<2>(); *min_index = min_index->cwiseMin(pixel_index); *max_index = max_index->cwiseMax(pixel_index); } return voxel_indices_and_probabilities; } string Submaps::ComputePixelValues( const std::vector& accumulated_pixel_data) const { string cell_data; cell_data.reserve(2 * accumulated_pixel_data.size()); constexpr float kMinZDifference = 3.f; constexpr float kFreeSpaceWeight = 0.15f; for (const PixelData& pixel : accumulated_pixel_data) { // TODO(whess): Take into account submap rotation. // TODO(whess): Document the approach and make it more independent from the // chosen resolution. const float z_difference = pixel.count > 0 ? pixel.max_z - pixel.min_z : 0; if (z_difference < kMinZDifference) { cell_data.push_back(0); // value cell_data.push_back(0); // alpha continue; } const float free_space = std::max(z_difference - pixel.count, 0.f); const float free_space_weight = kFreeSpaceWeight * free_space; const float total_weight = pixel.count + free_space_weight; const float free_space_probability = 1.f - pixel.max_probability; const float average_probability = mapping::ClampProbability( (pixel.probability_sum + free_space_probability * free_space_weight) / total_weight); const int delta = 128 - mapping::ProbabilityToLogOddsInteger(average_probability); const uint8 alpha = delta > 0 ? 0 : -delta; const uint8 value = delta > 0 ? delta : 0; cell_data.push_back(value); // value cell_data.push_back((value || alpha) ? alpha : 1); // alpha } return cell_data; } } // namespace mapping_3d } // namespace cartographer ================================================ FILE: mapping_3d/submaps.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ #define CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ #include #include #include #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/mapping/id.h" #include "cartographer/mapping/proto/submap_visualization.pb.h" #include "cartographer/mapping/submaps.h" #include "cartographer/mapping_2d/probability_grid.h" #include "cartographer/mapping_2d/range_data_inserter.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/mapping_3d/proto/submaps_options.pb.h" #include "cartographer/mapping_3d/range_data_inserter.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace mapping_3d { void InsertIntoProbabilityGrid( const sensor::RangeData& range_data, const transform::Rigid3f& pose, const float slice_z, const mapping_2d::RangeDataInserter& range_data_inserter, mapping_2d::ProbabilityGrid* result); proto::SubmapsOptions CreateSubmapsOptions( common::LuaParameterDictionary* parameter_dictionary); struct Submap : public mapping::Submap { Submap(float high_resolution, float low_resolution, const transform::Rigid3d& local_pose); HybridGrid high_resolution_hybrid_grid; HybridGrid low_resolution_hybrid_grid; bool finished = false; }; // A container of Submaps. class Submaps : public mapping::Submaps { public: explicit Submaps(const proto::SubmapsOptions& options); Submaps(const Submaps&) = delete; Submaps& operator=(const Submaps&) = delete; const Submap* Get(int index) const override; int size() const override; void SubmapToProto( int index, const transform::Rigid3d& global_submap_pose, mapping::proto::SubmapQuery::Response* response) const override; // Inserts 'range_data' into the Submap collection. 'gravity_alignment' is // used for the orientation of new submaps so that the z axis approximately // aligns with gravity. void InsertRangeData(const sensor::RangeData& range_data, const Eigen::Quaterniond& gravity_alignment); private: struct PixelData { int min_z = INT_MAX; int max_z = INT_MIN; int count = 0; float probability_sum = 0.f; float max_probability = 0.5f; }; void AddSubmap(const transform::Rigid3d& local_pose); std::vector AccumulatePixelData( const int width, const int height, const Eigen::Array2i& min_index, const Eigen::Array2i& max_index, const std::vector& voxel_indices_and_probabilities) const; // The first three entries of each returned value are a cell_index and the // last is the corresponding probability value. We batch them together like // this to only have one vector and have better cache locality. std::vector ExtractVoxelData( const HybridGrid& hybrid_grid, const transform::Rigid3f& transform, Eigen::Array2i* min_index, Eigen::Array2i* max_index) const; // Builds texture data containing interleaved value and alpha for the // visualization from 'accumulated_pixel_data'. string ComputePixelValues( const std::vector& accumulated_pixel_data) const; const proto::SubmapsOptions options_; // 'submaps_' contains pointers, so that resizing the vector does not // invalidate handed out Submap* pointers. std::vector> submaps_; RangeDataInserter range_data_inserter_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_SUBMAPS_H_ ================================================ FILE: mapping_3d/translation_cost_function.h ================================================ /* * Copyright 2016 The Cartographer Authors * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #ifndef CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ #include "Eigen/Core" #include "Eigen/Geometry" namespace cartographer { namespace mapping_3d { // Penalizes differences between velocity and change in position. class TranslationCostFunction { public: TranslationCostFunction(const double scaling_factor, const double delta_time_seconds) : scaling_factor_(scaling_factor), delta_time_seconds_(delta_time_seconds) {} TranslationCostFunction(const TranslationCostFunction&) = delete; TranslationCostFunction& operator=(const TranslationCostFunction&) = delete; template bool operator()(const T* const start_translation, const T* const end_translation, const T* const velocity, T* residual) const { const T delta_x = end_translation[0] - start_translation[0]; const T delta_y = end_translation[1] - start_translation[1]; const T delta_z = end_translation[2] - start_translation[2]; residual[0] = scaling_factor_ * (delta_x - velocity[0] * delta_time_seconds_); residual[1] = scaling_factor_ * (delta_y - velocity[1] * delta_time_seconds_); residual[2] = scaling_factor_ * (delta_z - velocity[2] * delta_time_seconds_); return true; } private: const double scaling_factor_; const double delta_time_seconds_; }; } // namespace mapping_3d } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_ ================================================ FILE: sensor/collator.cc ================================================ #include "cartographer/sensor/collator.h" namespace cartographer { namespace sensor { /* 一个轨迹线,多个传感器 */ /* struct QueueKey { int trajectory_id;// 轨线id; string sensor_id; //传感器id } */ void Collator::AddTrajectory( const int trajectory_id, const std::unordered_set& expected_sensor_ids, const Callback callback) { for (const auto& sensor_id : expected_sensor_ids) { //对于每一个轨迹线+传感器,设置一个key const auto queue_key = QueueKey{trajectory_id, sensor_id}; //添加一个名为key的队列,并设置回调函数处理data queue_.AddQueue(queue_key, [callback, sensor_id](std::unique_ptr data) { callback(sensor_id, std::move(data)); }); //map>:添加轨迹线对应的key queue_keys_[trajectory_id].push_back(queue_key); } } /*队列不再接收数据*/ void Collator::FinishTrajectory(const int trajectory_id) { for (const auto& queue_key : queue_keys_[trajectory_id]) { queue_.MarkQueueAsFinished(queue_key); } } /* 主要的操作,添加传感器数据,数据形式是:key+data */ void Collator::AddSensorData(const int trajectory_id, const string& sensor_id, std::unique_ptr data) { //找到key，再move(data) queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data)); } void Collator::Flush() { queue_.Flush(); } int Collator::GetBlockingTrajectoryId() const { return queue_.GetBlocker().trajectory_id; } } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/collator.h ================================================ #ifndef CARTOGRAPHER_SENSOR_COLLATOR_H_ #define CARTOGRAPHER_SENSOR_COLLATOR_H_ #include #include #include #include #include #include "cartographer/sensor/data.h" #include "cartographer/sensor/ordered_multi_queue.h" namespace cartographer { namespace sensor { /* Collator,采集者，校对者，整理者 将多传感器采集的数据归并到轨迹上。 Collator类:不可拷贝,不可赋值. 只有一个默认构造函数. 有2个数据成员 1,OrderedMultiQueue queue_; key是pair<轨迹线id,传感器id>. 一般情况下，对于已有的bag文件，轨迹id等于0. 2,std::unordered_map> queue_keys_ 轨迹线和队列key组成的hash表,1：N模式 Collator类主要提供三个操作: 1,AddTrajectory() 添加一个轨迹线, 2,FinishTrajectory() 标记轨迹线已经采集完成 3,AddSensorData()接收传感器数据 4,Flush()刷新 */ class Collator { public: using Callback = std::function)>; Collator() {} Collator(const Collator&) = delete; Collator& operator=(const Collator&) = delete; /*添加一个轨迹线,接受有序的传感器数据,并使用callback回调处理data。 一个轨迹线对应多个传感器数据:id ->unordered_set */ // Adds a trajectory to produce sorted sensor output for. Calls 'callback' // for each collated sensor data. void AddTrajectory(int trajectory_id, const std::unordered_set& expected_sensor_ids, Callback callback); // Marks 'trajectory_id' as finished.标记轨迹线已经完成采样. void FinishTrajectory(int trajectory_id); /*添加一个传感器id对应的数据data,数据必须按时间排序，*/ // Adds 'data' for 'trajectory_id' to be collated. 'data' must contain valid // sensor data. Sensor packets with matching 'sensor_id' must be added in time // order. void AddSensorData(int trajectory_id, const string& sensor_id, std::unique_ptr data); //刷新 // Dispatches all queued sensor packets. May only be called once. // AddSensorData may not be called after Flush. void Flush(); /*返回阻塞的轨迹id, 此种情况多见于某一传感器持久未采集data， 造成ordered_multi_queue阻塞。 */ // Must only be called if at least one unfinished trajectory exists. Returns // the ID of the trajectory that needs more data before the Collator is // unblocked. int GetBlockingTrajectoryId() const; private: //多个key构成的多队列 // Queue keys are a pair of trajectory ID and sensor identifier. OrderedMultiQueue queue_; //int为传感器id,vector是id+sensor组成的QueueKey // Map of trajectory ID to all associated QueueKeys. std::unordered_map> queue_keys_; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_COLLATOR_H_ ================================================ FILE: sensor/collator_test.cc ================================================ #include "cartographer/sensor/collator.h" #include #include #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/make_unique.h" #include "cartographer/common/time.h" #include "cartographer/sensor/proto/sensor.pb.h" #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { TEST(Collator, Ordering) { const std::array kSensorId = { {"horizontal_rangefinder", "vertical_rangefinder", "imu", "odometry"}}; Data zero(common::FromUniversal(0), Data::Rangefinder{}); Data first(common::FromUniversal(100), Data::Rangefinder{}); Data second(common::FromUniversal(200), Data::Rangefinder{}); Data third(common::FromUniversal(300), Data::Imu{}); Data fourth(common::FromUniversal(400), Data::Rangefinder{}); Data fifth(common::FromUniversal(500), Data::Rangefinder{}); Data sixth(common::FromUniversal(600), transform::Rigid3d::Identity()); //构造6个传感器数据 std::vector> received; Collator collator; collator.AddTrajectory( //添加一个轨迹线0,多个传感器,并指定回调函数:所有的接收数据都存储在received中 0, std::unordered_set(kSensorId.begin(), kSensorId.end()), [&received](const string& sensor_id, std::unique_ptr data) { received.push_back(std::make_pair(sensor_id, *data)); //由传感器id和data组成一个pair }); constexpr int kTrajectoryId = 0; // Establish a common start time. collator.AddSensorData(kTrajectoryId, kSensorId[0], common::make_unique(zero)); //轨迹0,传感器0产生一个数据,产生时间是0us collator.AddSensorData(kTrajectoryId, kSensorId[1], common::make_unique(zero)); collator.AddSensorData(kTrajectoryId, kSensorId[2], common::make_unique(zero)); collator.AddSensorData(kTrajectoryId, kSensorId[3], common::make_unique(zero)); //同上 collator.AddSensorData(kTrajectoryId, kSensorId[0], common::make_unique(first));//产生数据 collator.AddSensorData(kTrajectoryId, kSensorId[3], common::make_unique(sixth)); collator.AddSensorData(kTrajectoryId, kSensorId[0], common::make_unique(fourth)); collator.AddSensorData(kTrajectoryId, kSensorId[1], common::make_unique(second)); collator.AddSensorData(kTrajectoryId, kSensorId[1], common::make_unique(fifth)); collator.AddSensorData(kTrajectoryId, kSensorId[2], common::make_unique(third)); /* h:{0,100,400} v:{0,200,500} i:{0,300} o:{0,600} */ ASSERT_EQ(7, received.size()); //{0,0,0,0,100,200,300} EXPECT_EQ(100, common::ToUniversal(received[4].second.time)); EXPECT_EQ(kSensorId[0], received[4].first); EXPECT_EQ(200, common::ToUniversal(received[5].second.time)); EXPECT_EQ(kSensorId[1], received[5].first); EXPECT_EQ(300, common::ToUniversal(received[6].second.time)); EXPECT_EQ(kSensorId[2], received[6].first); collator.Flush(); //刷新 ASSERT_EQ(10, received.size()); //10个数据,所有sensor采集的数据 EXPECT_EQ(kSensorId[0], received[7].first); EXPECT_EQ(500, common::ToUniversal(received[8].second.time)); EXPECT_EQ(kSensorId[1], received[8].first); EXPECT_EQ(600, common::ToUniversal(received[9].second.time)); EXPECT_EQ(kSensorId[3], received[9].first); } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/compressed_point_cloud.cc ================================================ #include "cartographer/sensor/compressed_point_cloud.h" #include #include "cartographer/common/math.h" #include "cartographer/mapping_3d/hybrid_grid.h" namespace cartographer { namespace sensor { namespace { // Points are encoded on a fixed grid with a grid spacing of 'kPrecision' with // integers. Points are organized in blocks, where each point is encoded // relative to the block's origin in an int32 with 'kBitsPerCoordinate' bits per // coordinate. constexpr float kPrecision = 0.001f; // in meters. constexpr int kBitsPerCoordinate = 10; constexpr int kCoordinateMask = (1 << kBitsPerCoordinate) - 1; constexpr int kMaxBitsPerDirection = 23; } // namespace CompressedPointCloud::ConstIterator::ConstIterator( const CompressedPointCloud* compressed_point_cloud) : compressed_point_cloud_(compressed_point_cloud), remaining_points_(compressed_point_cloud->num_points_), remaining_points_in_current_block_(0), input_(compressed_point_cloud->point_data_.begin()) { if (remaining_points_ > 0) { ReadNextPoint(); } } CompressedPointCloud::ConstIterator CompressedPointCloud::ConstIterator::EndIterator( const CompressedPointCloud* compressed_point_cloud) { ConstIterator end_iterator(compressed_point_cloud); end_iterator.remaining_points_ = 0; return end_iterator; } Eigen::Vector3f CompressedPointCloud::ConstIterator::operator*() const { CHECK_GT(remaining_points_, 0); return current_point_; } CompressedPointCloud::ConstIterator& CompressedPointCloud::ConstIterator:: operator++() { --remaining_points_; if (remaining_points_ > 0) { ReadNextPoint(); } return *this; } bool CompressedPointCloud::ConstIterator::operator!=( const ConstIterator& it) const { CHECK(compressed_point_cloud_ == it.compressed_point_cloud_); return remaining_points_ != it.remaining_points_; } void CompressedPointCloud::ConstIterator::ReadNextPoint() { if (remaining_points_in_current_block_ == 0) { remaining_points_in_current_block_ = *input_++; for (int i = 0; i < 3; ++i) { current_block_coordinates_[i] = *input_++ << kBitsPerCoordinate; } } --remaining_points_in_current_block_; const int point = *input_++; constexpr int kMask = (1 << kBitsPerCoordinate) - 1; current_point_[0] = (current_block_coordinates_[0] + (point & kMask)) * kPrecision; current_point_[1] = (current_block_coordinates_[1] + ((point >> kBitsPerCoordinate) & kMask)) * kPrecision; current_point_[2] = (current_block_coordinates_[2] + (point >> (2 * kBitsPerCoordinate))) * kPrecision; } /* 最重要的构造函数 压缩点云 */ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud) //point_cloud是一个3f的vector,压缩到point_data_中储存 : num_points_(point_cloud.size()) { // Distribute points into blocks. struct RasterPoint { Eigen::Array3i point; // Array3i (int d1, int d2, int d3) int index; }; using Blocks = mapping_3d::HybridGridBase>; Blocks blocks(kPrecision); int num_blocks = 0; CHECK_LE(point_cloud.size(), std::numeric_limits::max()); for (int point_index = 0; point_index < static_cast(point_cloud.size()); ++point_index) { const Eigen::Vector3f& point = point_cloud[point_index];//获取某个point{x,y,z} CHECK_LT(point.cwiseAbs().maxCoeff() / kPrecision, 1 << kMaxBitsPerDirection) << "Point out of bounds: " << point; Eigen::Array3i raster_point; Eigen::Array3i block_coordinate; for (int i = 0; i < 3; ++i) { raster_point[i] = common::RoundToInt(point[i] / kPrecision); block_coordinate[i] = raster_point[i] >> kBitsPerCoordinate; raster_point[i] &= kCoordinateMask; } auto* const block = blocks.mutable_value(block_coordinate); num_blocks += block->empty(); block->push_back({raster_point, point_index}); } //end for // Encode blocks. point_data_.reserve(4 * num_blocks + point_cloud.size()); for (Blocks::Iterator it(blocks); !it.Done(); it.Next(), --num_blocks) { const auto& raster_points = it.GetValue(); CHECK_LE(raster_points.size(), std::numeric_limits::max()); point_data_.push_back(raster_points.size()); const Eigen::Array3i block_coordinate = it.GetCellIndex(); point_data_.push_back(block_coordinate.x()); point_data_.push_back(block_coordinate.y()); point_data_.push_back(block_coordinate.z()); for (const RasterPoint& raster_point : raster_points) { point_data_.push_back((((raster_point.point.z() << kBitsPerCoordinate) + raster_point.point.y()) << kBitsPerCoordinate) + raster_point.point.x()); } } CHECK_EQ(num_blocks, 0); } /*私有的构造函数,外部不能调用*/ CompressedPointCloud::CompressedPointCloud(const std::vector& point_data, size_t num_points) : point_data_(point_data), num_points_(num_points) {} bool CompressedPointCloud::empty() const { return num_points_ == 0; } size_t CompressedPointCloud::size() const { return num_points_; } CompressedPointCloud::ConstIterator CompressedPointCloud::begin() const {//迭代器首, return ConstIterator(this); } CompressedPointCloud::ConstIterator CompressedPointCloud::end() const { //迭代器尾, return ConstIterator::EndIterator(this); } PointCloud CompressedPointCloud::Decompress() const { PointCloud decompressed; //Vector3f组成的vector for (const Eigen::Vector3f& point : *this) { //此处调用的是迭代器函数,begin(),end() decompressed.push_back(point); } return decompressed; } proto::CompressedPointCloud CompressedPointCloud::ToProto() const { proto::CompressedPointCloud result; result.set_num_points(num_points_); //序列化点云的个数 for (const int32 data : point_data_) { result.add_point_data(data); //依次序添加数据 } return result; } } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/compressed_point_cloud.h ================================================ #ifndef CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ #define CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ #include #include #include "Eigen/Core" #include "cartographer/common/port.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/sensor.pb.h" namespace cartographer { namespace sensor { /* CompressedPointCloud是点云压缩类, 目的：压缩ponits以减少存储空间，压缩后有精度损失。 方法：按照block分组。 只有一个私有的 */ // A compressed representation of a point cloud consisting of a collection of // points (Vector3f). // Internally, points are grouped by blocks. Each block encodes a bit of meta // data (number of points in block, coordinates of the block) and encodes each // point with a fixed bit rate in relation to the block. class CompressedPointCloud { public: class ConstIterator; //前置声明 CompressedPointCloud() : num_points_(0) {} explicit CompressedPointCloud(const PointCloud& point_cloud); // Returns decompressed point cloud. PointCloud Decompress() const; bool empty() const; // num_points_==0 size_t size() const; // num_points_ ConstIterator begin() const; ConstIterator end() const; proto::CompressedPointCloud ToProto() const; private: CompressedPointCloud(const std::vector& point_data, size_t num_points); std::vector point_data_; size_t num_points_; }; /*前行迭代器*/ // Forward iterator for compressed point clouds. class CompressedPointCloud::ConstIterator : public std::iterator { public: // Creates begin iterator. explicit ConstIterator(const CompressedPointCloud* compressed_point_cloud); // Creates end iterator. static ConstIterator EndIterator( const CompressedPointCloud* compressed_point_cloud); Eigen::Vector3f operator*() const; ConstIterator& operator++(); bool operator!=(const ConstIterator& it) const; private: // Reads next point from buffer. Also handles reading the meta data of the // next block, if the current block is depleted. void ReadNextPoint(); const CompressedPointCloud* compressed_point_cloud_; size_t remaining_points_; int32 remaining_points_in_current_block_; Eigen::Vector3f current_point_; Eigen::Vector3i current_block_coordinates_; std::vector::const_iterator input_; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_COMPRESSED_POINT_CLOUD_H_ ================================================ FILE: sensor/compressed_point_cloud_test.cc ================================================ #include "cartographer/sensor/compressed_point_cloud.h" #include "gmock/gmock.h" namespace Eigen { // Prints Vector3f in a readable format in matcher ApproximatelyEquals when // failing a test. Without this function, the output is formated as hexadecimal // 8 bit numbers. void PrintTo(const Vector3f& x, std::ostream* os) { *os << "(" << x[0] << ", " << x[1] << ", " << x[2] << ")"; } } // namespace Eigen namespace cartographer { namespace sensor { namespace { using ::testing::Contains; using ::testing::FloatNear; using ::testing::PrintToString; constexpr float kPrecision = 0.001f; // Matcher for 3-d vectors w.r.t. to the target precision. MATCHER_P(ApproximatelyEquals, expected, string("is equal to ") + PrintToString(expected)) { return (arg - expected).isZero(kPrecision);//压缩后有精度丢失,精确度为0.001 } // Helper function to test the mapping of a single point. Includes test for // recompressing the same point again. void TestPoint(const Eigen::Vector3f& p) { CompressedPointCloud compressed({p}); EXPECT_EQ(1, compressed.size()); EXPECT_THAT(*compressed.begin(), ApproximatelyEquals(p)); CompressedPointCloud recompressed({*compressed.begin()}); EXPECT_THAT(*recompressed.begin(), ApproximatelyEquals(p)); } TEST(CompressPointCloudTest, CompressesPointsCorrectly) { TestPoint(Eigen::Vector3f(8000.f, 7500.f, 5000.f)); TestPoint(Eigen::Vector3f(1000.f, 2000.f, 3000.f)); TestPoint(Eigen::Vector3f(100.f, 200.f, 300.f)); TestPoint(Eigen::Vector3f(10.f, 20.f, 30.f)); TestPoint(Eigen::Vector3f(-0.00049f, -0.0005f, -0.0015f)); TestPoint(Eigen::Vector3f(0.05119f, 0.0512f, 0.05121)); TestPoint(Eigen::Vector3f(-0.05119f, -0.0512f, -0.05121)); TestPoint(Eigen::Vector3f(0.8405f, 0.84f, 0.8396f)); TestPoint(Eigen::Vector3f(0.8395f, 0.8394f, 0.8393f)); TestPoint(Eigen::Vector3f(0.839f, 0.8391f, 0.8392f)); TestPoint(Eigen::Vector3f(0.8389f, 0.8388f, 0.83985f)); } TEST(CompressPointCloudTest, Compresses) { const CompressedPointCloud compressed({Eigen::Vector3f(0.838f, 0, 0), Eigen::Vector3f(0.839f, 0, 0), Eigen::Vector3f(0.840f, 0, 0)}); EXPECT_FALSE(compressed.empty()); EXPECT_EQ(3, compressed.size()); const PointCloud decompressed = compressed.Decompress(); EXPECT_EQ(3, decompressed.size()); EXPECT_THAT(decompressed, Contains(ApproximatelyEquals(Eigen::Vector3f(0.838f, 0, 0))));//压缩解压缩后,前3位小数不变 EXPECT_THAT(decompressed, Contains(ApproximatelyEquals(Eigen::Vector3f(0.839f, 0, 0)))); EXPECT_THAT(decompressed, Contains(ApproximatelyEquals(Eigen::Vector3f(0.840f, 0, 0)))); } TEST(CompressPointCloudTest, CompressesEmptyPointCloud) { CompressedPointCloud compressed; EXPECT_TRUE(compressed.empty()); EXPECT_EQ(0, compressed.size()); } // Test for gaps. // Produces a series of points densly packed along the x axis, compresses these // points (twice), and tests, whether there are gaps between two consecutive // points. TEST(CompressPointCloudTest, CompressesNoGaps) { PointCloud point_cloud; for (int i = 0; i < 3000; ++i) { point_cloud.push_back(Eigen::Vector3f(kPrecision * i - 1.5f, 0, 0)); } const CompressedPointCloud compressed(point_cloud);//压缩 const PointCloud decompressed = compressed.Decompress();//解压缩 const CompressedPointCloud recompressed(decompressed);//再压缩 EXPECT_EQ(decompressed.size(), recompressed.size()); std::vector x_coord; for (const auto& p : compressed) { x_coord.push_back(p[0]); } std::sort(x_coord.begin(), x_coord.end()); for (size_t i = 1; i < x_coord.size(); ++i) { EXPECT_THAT(std::abs(x_coord[i] - x_coord[i - 1]), FloatNear(kPrecision, 1e-7f)); //前后相差不大 } } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/configuration.cc ================================================ #include "cartographer/sensor/configuration.h" #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/sensor/proto/configuration.pb.h" #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace sensor { /* assets_writer_backpack_2d.lua Sensor定义： message Sensor { string frame_id = 2; //'frame_id' of the sensor. Rigid3d transform = 3; } */ proto::Configuration::Sensor CreateSensorConfiguration( common::LuaParameterDictionary* const parameter_dictionary) { proto::Configuration::Sensor sensor; sensor.set_frame_id(parameter_dictionary->GetString("frame_id")); *sensor.mutable_transform() = transform::ToProto(transform::FromDictionary( parameter_dictionary->GetDictionary("transform").get())); return sensor; } /* Configuration定义： message Configuration { repeated Sensor sensor = 15; } */ proto::Configuration CreateConfiguration( common::LuaParameterDictionary* const parameter_dictionary) { proto::Configuration configuration; for (auto& sensor_parameter_dictionary : parameter_dictionary->GetArrayValuesAsDictionaries()) { *configuration.add_sensor() = CreateSensorConfiguration(sensor_parameter_dictionary.get()); } return configuration; } /* 判断系统是否支持某一传感器id。 */ bool IsEnabled(const string& frame_id, const sensor::proto::Configuration& sensor_configuration) { for (const auto& sensor : sensor_configuration.sensor()) { if (sensor.frame_id() == frame_id) { return true; } } return false; } /* 将sensor坐标转换为机器坐标。以便于跟踪计算 */ transform::Rigid3d GetTransformToTracking( const string& frame_id, const sensor::proto::Configuration& sensor_configuration) { for (const auto& sensor : sensor_configuration.sensor()) { if (sensor.frame_id() == frame_id) { return transform::ToRigid3(sensor.transform()); } } LOG(FATAL) << "No configuration found for sensor with frame ID '" << frame_id << "'."; } } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/configuration.h ================================================ #ifndef CARTOGRAPHER_SENSOR_CONFIGURATION_H_ #define CARTOGRAPHER_SENSOR_CONFIGURATION_H_ #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/sensor/proto/configuration.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace sensor { //从sensor配置文件解析sensor的数据参数。主要是sensor到机器坐标的转换 // Creates the configuration for a singular sensor from 'parameter_dictionary'. proto::Configuration::Sensor CreateSensorConfiguration( common::LuaParameterDictionary* parameter_dictionary); //求得多个sensor的配置的集合。 // Creates the mapping from frame_id to Sensors defined in // 'parameter_dictionary'. proto::Configuration CreateConfiguration( common::LuaParameterDictionary* parameter_dictionary); //系统是否支持某一传感器。 // Returns true if 'frame_id' is mentioned in 'sensor_configuration'. bool IsEnabled(const string& frame_id, const sensor::proto::Configuration& sensor_configuration); //将sensor采集的data经过3d坐标变换为机器坐标。 // Returns the transform which takes data from the sensor frame to the // tracking frame. transform::Rigid3d GetTransformToTracking( const string& frame_id, const sensor::proto::Configuration& sensor_configuration); } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_CONFIGURATION_H_ ================================================ FILE: sensor/data.h ================================================ #ifndef CARTOGRAPHER_MAPPING_DATA_H_ #define CARTOGRAPHER_MAPPING_DATA_H_ #include "cartographer/common/time.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/range_data.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace sensor { /* Data是针对某一类的传感器的数据的封装, 数据成员含: 1,传感器类型,包括imu，雷达，里程计 2,测量时间,time 3,Imu测量值,(imu:惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。) 4,rangefinder,测距仪, 5,odometer_pose,里程计,指(如装在汽车上的)测量行程的装置。 构造函数有3个,每一类传感器对应一个构造函数. */ // This type is a logical union, i.e. only one type of sensor data is actually // filled in. It is only used for time ordering sensor data before passing it // on. struct Data { enum class Type { kImu, kRangefinder, kOdometer }; struct Imu { Eigen::Vector3d linear_acceleration; //线性加速度,m/s2 Eigen::Vector3d angular_velocity; //角速度, rad/s }; //测距仪.基本原理是向待测距的物体发射激光脉冲并开始计时，接收到反射光时停止计时。这段时间即可以转换为激光器与目标之间的距离。 struct Rangefinder { Eigen::Vector3f origin; //sensor的位姿 PointCloud ranges; //测距点云 }; Data(const common::Time time, const Imu& imu) //传感器类型是imu : type(Type::kImu), time(time), imu(imu) {} Data(const common::Time time, const Rangefinder& rangefinder)//传感器类型是雷达 : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} Data(const common::Time time, const transform::Rigid3d& odometer_pose) //传感器类型是里程计 : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} Type type; common::Time time; Imu imu; Rangefinder rangefinder; transform::Rigid3d odometer_pose; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_MAPPING_DATA_H_ /* rigidbody:刚体 */ ================================================ FILE: sensor/ordered_multi_queue.cc ================================================ #include "cartographer/sensor/ordered_multi_queue.h" #include #include #include #include "cartographer/common/make_unique.h" #include "glog/logging.h" namespace cartographer { namespace sensor { namespace { // Number of items that can be queued up before we log which queues are waiting // for data. const int kMaxQueueSize = 500; } // namespace //重载QueueKey的<<输出运算符,友元函数 inline std::ostream& operator<<(std::ostream& out, const QueueKey& key) { return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')'; } OrderedMultiQueue::OrderedMultiQueue() {} OrderedMultiQueue::~OrderedMultiQueue() { for (auto& entry : queues_) { CHECK(entry.second.finished); } } //添加一个关键词是key的队列,并用比较函数Callback排序 void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) { CHECK_EQ(queues_.count(queue_key), 0); //map.count()相对于[],调用时，是不添加key的。而[]是带有副作用的。c++pr.386 queues_[queue_key].callback = std::move(callback); } void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey& queue_key) { auto it = queues_.find(queue_key); CHECK(it != queues_.end()) << "Did not find '" << queue_key << "'."; auto& queue = it->second; CHECK(!queue.finished);//检查状态 queue.finished = true;//标记本队列已完成,别的数据不能再入队. Dispatch(); //调用一次MarkQueueAsFinished()就要调用一次Dispatch() } //根据key找到队列,并添加data元素 void OrderedMultiQueue::Add(const QueueKey& queue_key, std::unique_ptr data) { auto it = queues_.find(queue_key); if (it == queues_.end()) {//没有key时，警告 LOG_EVERY_N(WARNING, 1000) << "Ignored data for queue: '" << queue_key << "'"; return; } it->second.queue.Push(std::move(data));//Queue的queue.push() Dispatch();//调用一次Add()就要调用一次Dispatch() } //先找到没有finished的队列,然后再对这些队列标记finished.已完成的则不作任何处理 void OrderedMultiQueue::Flush() { std::vector unfinished_queues; for (auto& entry : queues_) { if (!entry.second.finished) { unfinished_queues.push_back(entry.first); } } for (auto& unfinished_queue : unfinished_queues) { MarkQueueAsFinished(unfinished_queue);//一个一个的处理。 } } QueueKey OrderedMultiQueue::GetBlocker() const { CHECK(!queues_.empty()); return blocker_; } /* Dispatch()函数，不断的处理来自sensor的数据。按照data采集的时间顺序处理。 kFirst: {1,2,3} finised kSecond:{} finised kThird: {} finised */ void OrderedMultiQueue::Dispatch() { while (true) { //首先处理的数据，也即最早采集的数据 const Data* next_data = nullptr; Queue* next_queue = nullptr; QueueKey next_queue_key; //遍历队列中的每一个key：填充上面3个变量值。如果某一key对应的data为空，则直接return for (auto it = queues_.begin(); it != queues_.end();) { const auto* data = it->second.queue.Peek();//获取某一队的队首data if (data == nullptr) { if (it->second.finished) {//it对应的队列为空且为finished,故删除it对应的key queues_.erase(it++); //map迭代器没有失效? continue; } CannotMakeProgress(it->first);//此时什么也不做。 return; } if (next_data == nullptr || data->time < next_data->time) { //找到next_data数据:即采集时间最早的数据，理论上应该最先处理它。 next_data = data; next_queue = &it->second; next_queue_key = it->first; } CHECK_LE(last_dispatched_time_, next_data->time) << "Non-sorted data added to queue: '" << it->first << "'"; ++it; } if (next_data == nullptr) { CHECK(queues_.empty());//只有多队列为空，才可能next_data==nullptr return; } // If we haven't dispatched any data for this trajectory yet, fast forward // all queues of this trajectory until a common start time has been reached. const common::Time common_start_time = GetCommonStartTime(next_queue_key.trajectory_id); //common_start_time即所有的sensor都开始有data的时间点。 if (next_data->time >= common_start_time) { //大多数情况，happy case // Happy case, we are beyond the 'common_start_time' already. last_dispatched_time_ = next_data->time; next_queue->callback(next_queue->queue.Pop()); //调用回调函数处理data。 } else if (next_queue->queue.Size() < 2) {// 罕见 if (!next_queue->finished) { // We cannot decide whether to drop or dispatch this yet. CannotMakeProgress(next_queue_key); return; } last_dispatched_time_ = next_data->time; next_queue->callback(next_queue->queue.Pop()); } else { // We take a peek at the time after next data. If it also is not beyond // 'common_start_time' we drop 'next_data', otherwise we just found the // first packet to dispatch from this queue. std::unique_ptr next_data_owner = next_queue->queue.Pop(); if (next_queue->queue.Peek()->time > common_start_time) { last_dispatched_time_ = next_data->time; next_queue->callback(std::move(next_data_owner)); } } } } void OrderedMultiQueue::CannotMakeProgress(const QueueKey& queue_key) { blocker_ = queue_key; //标识该队列Queue已经阻塞 for (auto& entry : queues_) {//std::map if (entry.second.queue.Size() > kMaxQueueSize) {//？ LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key; return; } } } /*对同一轨迹id，求得所有sensor的首次采集data的最晚时间maxtime 不同轨迹按不同轨迹算： kFirst: {0,1,2,3} finised kSecond:{2} kThird: {4} {2,2,} */ common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id) { //map.emplace():Construct and insert element，根据trajectory_id构造一个map。 auto emplace_result = common_start_time_per_trajectory_.emplace( trajectory_id, common::Time::min()); common::Time& common_start_time = emplace_result.first->second; if (emplace_result.second) { for (auto& entry : queues_) { //entry是map的pair<,>.本循环求得所有传感器中的maxtime if (entry.first.trajectory_id == trajectory_id) { common_start_time = std::max(common_start_time, entry.second.queue.Peek()->time); } } LOG(INFO) << "All sensor data for trajectory " << trajectory_id << " is available starting at '" << common_start_time << "'."; } return common_start_time; } } // namespace sensor } // namespace cartographer /* ordered_multi_queue.cc:172] All sensor data for trajectory 0 is available starting at '635727208200176089' */ /* LOG_EVERY_N(INFO, 60) << "Queue...";在程序中周期性的记录日志信息， 在该语句第1、61、121……次被执行的时候，记录日志信息。 */ ================================================ FILE: sensor/ordered_multi_queue.h ================================================ #ifndef CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_ #define CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_ #include #include #include #include #include #include "cartographer/common/blocking_queue.h" #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/sensor/data.h" namespace cartographer { namespace sensor { struct QueueKey { int trajectory_id;// 轨线id; string sensor_id; //传感器id //重载小于运算符,forward_as_tuple:完美转发. (以tuple规则比较2者),tuple定义了<运算符 bool operator<(const QueueKey& other) const { return std::forward_as_tuple(trajectory_id, sensor_id) < std::forward_as_tuple(other.trajectory_id, other.sensor_id); } }; /* OrderedMultiQueue，用于管理多个有序的传感器数据， 是有序的多队列类,每个队列有一个key,并且有一个自定义排序函数 queues_的形式为： key1:Queue key2：Queue key3：Queue Queue的形式为 struct Queue { common::BlockingQueue> queue; Callback callback; bool finished = false; }; OrderedMultiQueue的数据成员有 1,common_start_time_per_trajectory_:轨迹id及对应创建轨迹时间 2,last_dispatched_time_ 3,std::map queues_;按照key排序的map 4,QueueKey blocker_; */ // Maintains multiple queues of sorted sensor data and dispatches(迅速办理) it in merge // sorted order. It will wait to see at least one value for each unfinished // queue before dispatching the next time ordered value across all queues. // // This class is thread-compatible. class OrderedMultiQueue { public: using Callback = std::function)>;//回调函数 OrderedMultiQueue(); ~OrderedMultiQueue(); //添加一个【队列】Queue,名称是key,以后入队的data，调用回调函数callback处理 // Adds a new queue with key 'queue_key' which must not already exist. // 'callback' will be called whenever data from this queue can be dispatched. void AddQueue(const QueueKey& queue_key, Callback callback); /* 某一key标识的【队列】Queue已经完成入队,因此不能再入队列,并在map中移除key. */ // Marks a queue as finished, i.e. no further data can be added. The queue // will be removed once the last piece of data from it has been dispatched. void MarkQueueAsFinished(const QueueKey& queue_key); //对某一key标识的队列Queue,压入data,data按照回调函数处理 // Adds 'data' to a queue with the given 'queue_key'. Data must be added // sorted per queue. void Add(const QueueKey& queue_key, std::unique_ptr data); /*标记全部队列都已经finished. kFirst: {0,4,5,6} kSecond:{0,1,3,7} kThird: {0,2,8} 之前只处理到6，调用Flush()则处理剩余的7,8 如果不调用Flush()，则析构时会出错 */ // Dispatches all remaining values in sorted order and removes the underlying // queues. void Flush(); /*返回阻塞的队列(意为该队列对应的sensor的data未到达)*/ // Must only be called if at least one unfinished queue exists. Returns the // key of a queue that needs more data before the OrderedMultiQueue can // dispatch data. QueueKey GetBlocker() const; private: struct Queue { common::BlockingQueue> queue; Callback callback; bool finished = false; }; void Dispatch(); void CannotMakeProgress(const QueueKey& queue_key); common::Time GetCommonStartTime(int trajectory_id); // Used to verify that values are dispatched in sorted order. common::Time last_dispatched_time_ = common::Time::min(); std::map common_start_time_per_trajectory_; std::map queues_;//多队列主体,本类最大的内存占用量 QueueKey blocker_; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_ORDERED_MULTI_QUEUE_H_ /* queues_: first 是QueueKey second 是 Queue。 Queue的形式为 struct Queue { common::BlockingQueue> queue; Callback callback; bool finished = false; }; */ ================================================ FILE: sensor/ordered_multi_queue_le_test.cc ================================================ #define sout(Xit) {std::cout<<__LINE__<<" "<< Xit <<""< #include "cartographer/common/make_unique.h" #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { /*测试夹具（Test Fixtures） TEST_F提供了一个初始化函数（SetUp）和一个清理函数(TearDown)， 在TEST_F中使用的变量可以在初始化函数SetUp中初始化， 在TearDown中销毁，并且所有的TEST_F是互相独立的，都是在初始化以后的状态开始运行， 一个TEST_F不会影响另一个TEST_F所使用的数据. 用TEST_F定义测试，写法与TEST相同，但测试用例名必须为上面定义的类名。 */ class OrderedMultiQueueTest : public ::testing::Test { protected: // These are keys are chosen so that they sort first, second, third. const QueueKey kFirst{1, "a"};//trajectory_id，sensor_id const QueueKey kSecond{1, "b"}; const QueueKey kThird{2, "b"}; //void AddQueue(const QueueKey& queue_key, Callback callback); void SetUp() { //初始化函数,定义处理data的函数： //(以后用于test比较):检查values_的最大值是否小于欲添加的值. for (const auto& queue_key : {kFirst, kSecond, kThird}) { queue_.AddQueue(queue_key, [this](std::unique_ptr data) { if (!values_.empty()) { EXPECT_GE(data->time, values_.back().time); } values_.push_back(*data); }); } } //Data的定义在data.h std::unique_ptr MakeImu(const int ordinal) {//时间,序列 return common::make_unique( common::FromUniversal(ordinal), Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); } std::vector values_; OrderedMultiQueue queue_; }; TEST_F(OrderedMultiQueueTest, Ordering) { // void Add(const QueueKey& queue_key, std::unique_ptr data); queue_.Add(kFirst, MakeImu(0));// queue_.Add(kFirst, MakeImu(4)); queue_.Add(kFirst, MakeImu(5)); queue_.Add(kFirst, MakeImu(6)); EXPECT_TRUE(values_.empty()); //why为空?：queue_对应的其他sensor还没有产生data， //故调用Add()时，只在本队列插入data，而Dispatch()直接返回，没有调用Callback函数。 queue_.Add(kSecond, MakeImu(0)); queue_.Add(kSecond, MakeImu(1)); EXPECT_TRUE(values_.empty()); //同上 queue_.Add(kThird, MakeImu(0)); queue_.Add(kThird, MakeImu(2)); EXPECT_EQ(values_.size(), 4); /* kFirst: {0,4,5,6} kSecond:{0,1} kThird: {0,2} */ for(auto i:values_) { sout(i.time);//{0,0,0,1} } queue_.Add(kSecond, MakeImu(3)); EXPECT_EQ(values_.size(), 5); /* kFirst: {0,4,5,6} kSecond:{0,1,3} kThird: {0,2} */ for(auto i:values_) { sout(i.time);//{0,0,0,1,2} } queue_.Add(kSecond, MakeImu(7)); queue_.Add(kThird, MakeImu(8)); /* kFirst: {0,4,5,6} kSecond:{0,1,3,7} kThird: {0,2,8} */ for(auto i:values_) { sout(i.time);//{0,0,0,1,2,3,4,5,6} } //先找到没有finished的队列,然后再对这些队列标记finished queue_.Flush();//应该调用,否则析构时出错 EXPECT_EQ(11, values_.size());// 4+4+3 /* kFirst: {0,4,5,6} kSecond:{0,1,3,7} kThird: {0,2,8} */ for(auto i:values_) { sout(i.time);//{0,0,0,1,2,3,4,5,6,7,8} } for (size_t i = 0; i < values_.size() - 1; ++i) { //检查是否按序 EXPECT_LE(values_[i].time, values_[i + 1].time); } } TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { queue_.Add(kFirst, MakeImu(1)); queue_.Add(kFirst, MakeImu(2)); queue_.Add(kFirst, MakeImu(3)); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kFirst); //调用该函数一次，就调用一次Dispatch(),若key对应的队列为空，则从队列中删除key EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kSecond); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kThird); /* kFirst: {1,2,3} finised kSecond:{} finised kThird: {} finised */ for(auto i:values_) { sout(i.time);//{1,2,3} } EXPECT_EQ(3, values_.size()); for (size_t i = 0; i < values_.size(); ++i) { EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); } } TEST_F(OrderedMultiQueueTest, CommonStartTimePerTrajectory) { queue_.Add(kFirst, MakeImu(0)); queue_.Add(kFirst, MakeImu(1)); queue_.Add(kFirst, MakeImu(2)); queue_.Add(kFirst, MakeImu(3)); queue_.Add(kSecond, MakeImu(2)); EXPECT_TRUE(values_.empty()); queue_.Add(kThird, MakeImu(4)); EXPECT_EQ(values_.size(), 2); /* kFirst: {0,1,2,3} kSecond:{2} kThird: {4} */ for(auto i:values_) { sout(i.time);//{2,2} } queue_.MarkQueueAsFinished(kFirst); EXPECT_EQ(values_.size(), 2);//没有0,1，因为起点时间不是0,1，而是2 /* kFirst: {0,1,2,3} finised kSecond:{2} kThird: {4} */ for(auto i:values_) { sout(i.time);//{2,2} } sout(".id2.");//为何与test1不同？起点time不同。test1：起点时间全为0，test3起点时间是2 queue_.MarkQueueAsFinished(kSecond); EXPECT_EQ(values_.size(), 4); /* kFirst: {0,1,2,3} finised kSecond:{2} finised kThird: {4} */ for(auto i:values_) { sout(i.time);//{2,2,3,4} } //第二条轨迹id开始采集数据。 queue_.MarkQueueAsFinished(kThird); EXPECT_EQ(values_.size(), 4); /* kFirst: {0,1,2,3} finised kSecond:{2} finised kThird: {4} finised */ for(auto i:values_) { sout(i.time);//{2,2,3,4} } } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/ordered_multi_queue_test.cc ================================================ #include "cartographer/sensor/ordered_multi_queue.h" #include #include "cartographer/common/make_unique.h" #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { /*测试夹具（Test Fixtures） TEST_F提供了一个初始化函数（SetUp）和一个清理函数(TearDown)， 在TEST_F中使用的变量可以在初始化函数SetUp中初始化， 在TearDown中销毁，并且所有的TEST_F是互相独立的，都是在初始化以后的状态开始运行， 一个TEST_F不会影响另一个TEST_F所使用的数据. 用TEST_F定义测试，写法与TEST相同，但测试用例名必须为上面定义的类名。 */ class OrderedMultiQueueTest : public ::testing::Test { protected: // These are keys are chosen so that they sort first, second, third. const QueueKey kFirst{1, "a"};//trajectory_id，sensor_id const QueueKey kSecond{1, "b"}; const QueueKey kThird{2, "b"}; //void AddQueue(const QueueKey& queue_key, Callback callback); void SetUp() { //初始化函数,定义处理data的函数： //(以后用于test比较):检查values_的最大值是否小于欲添加的值. for (const auto& queue_key : {kFirst, kSecond, kThird}) { queue_.AddQueue(queue_key, [this](std::unique_ptr data) { if (!values_.empty()) { EXPECT_GE(data->time, values_.back().time); } values_.push_back(*data); }); } } //Data的定义在data.h std::unique_ptr MakeImu(const int ordinal) {//时间,序列 return common::make_unique( common::FromUniversal(ordinal), Data::Imu{Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()}); } std::vector values_; OrderedMultiQueue queue_; }; TEST_F(OrderedMultiQueueTest, Ordering) { // void Add(const QueueKey& queue_key, std::unique_ptr data); queue_.Add(kFirst, MakeImu(0));// queue_.Add(kFirst, MakeImu(4)); queue_.Add(kFirst, MakeImu(5)); queue_.Add(kFirst, MakeImu(6)); EXPECT_TRUE(values_.empty()); //why为空?：queue_对应的其他sensor还没有产生data， //故调用Add()时，只在本队列插入data，而Dispatch()直接返回，没有调用Callback函数。 queue_.Add(kSecond, MakeImu(0)); queue_.Add(kSecond, MakeImu(1)); EXPECT_TRUE(values_.empty()); //同上 queue_.Add(kThird, MakeImu(0)); queue_.Add(kThird, MakeImu(2)); EXPECT_EQ(values_.size(), 4); /* kFirst: {0,4,5,6} kSecond:{0,1} kThird: {0,2} */ queue_.Add(kSecond, MakeImu(3)); EXPECT_EQ(values_.size(), 5); queue_.Add(kSecond, MakeImu(7)); queue_.Add(kThird, MakeImu(8)); queue_.Flush();//应该调用,否则出错 EXPECT_EQ(11, values_.size());// f:4,s:4,t:3 for (size_t i = 0; i < values_.size() - 1; ++i) { //检查是否按序 EXPECT_LE(values_[i].time, values_[i + 1].time); } } TEST_F(OrderedMultiQueueTest, MarkQueueAsFinished) { queue_.Add(kFirst, MakeImu(1)); queue_.Add(kFirst, MakeImu(2)); queue_.Add(kFirst, MakeImu(3)); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kFirst); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kSecond); EXPECT_TRUE(values_.empty()); queue_.MarkQueueAsFinished(kThird); EXPECT_EQ(3, values_.size()); for (size_t i = 0; i < values_.size(); ++i) { EXPECT_EQ(i + 1, common::ToUniversal(values_[i].time)); } } TEST_F(OrderedMultiQueueTest, CommonStartTimePerTrajectory) { queue_.Add(kFirst, MakeImu(0)); queue_.Add(kFirst, MakeImu(1)); queue_.Add(kFirst, MakeImu(2)); queue_.Add(kFirst, MakeImu(3)); queue_.Add(kSecond, MakeImu(2)); EXPECT_TRUE(values_.empty()); queue_.Add(kThird, MakeImu(4)); EXPECT_EQ(values_.size(), 2); queue_.MarkQueueAsFinished(kFirst); EXPECT_EQ(values_.size(), 2); queue_.MarkQueueAsFinished(kSecond); EXPECT_EQ(values_.size(), 4); queue_.MarkQueueAsFinished(kThird); EXPECT_EQ(values_.size(), 4); } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/point_cloud.cc ================================================ #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace sensor { /*. 功能：根据3D变换，转换点云, 返回：转换后的新点云结果，原点云未变。 */ PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform) { PointCloud result; //vector,元素是3*1f result.reserve(point_cloud.size()); //分配内存 for (const Eigen::Vector3f& point : point_cloud) { result.emplace_back(transform * point); //C=A*B //A是Rigid3f,B是3*1f } return result; } /* 功能：按范围丢弃点云，去掉z轴区域外的点云 返回：新的符合要求的点云。 */ PointCloud Crop(const PointCloud& point_cloud, const float min_z, const float max_z) { PointCloud cropped_point_cloud; for (const auto& point : point_cloud) { if (min_z <= point.z() && point.z() <= max_z) {//只保留在minz~maxz之间的点云 cropped_point_cloud.push_back(point); } } return cropped_point_cloud; } //序列化 proto::PointCloud ToProto(const PointCloud& point_cloud) { proto::PointCloud proto; for (const auto& point : point_cloud) { proto.add_x(point.x()); proto.add_y(point.y()); proto.add_z(point.z()); } return proto; } //反序列化 PointCloud ToPointCloud(const proto::PointCloud& proto) { PointCloud point_cloud; const int size = std::min({proto.x_size(), proto.y_size(), proto.z_size()}); point_cloud.reserve(size);//最小，否则以下语句有bug for (int i = 0; i != size; ++i) { point_cloud.emplace_back(proto.x(i), proto.y(i), proto.z(i)); } return point_cloud; } } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/point_cloud.h ================================================ #ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ #define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ #include #include "Eigen/Core" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/rigid_transform.h" #include "glog/logging.h" /* 点云数据是指在一个三维坐标系统中的一组向量的集合。{p1,p2,p3,...} 这些向量通常以X,Y,Z三维坐标的形式表示p:{x,y,z}， 而且一般主要用来代表一个物体的外表面形状。 除{x,y,z}可以代表的几何位置信息之外， 点云数据还可以表示一个点的RGB颜色，灰度值，深度，分割结果等。 Eg..Pi={Xi, Yi, Zi,…….}表示空间中的一个点， 则Point Cloud={P1, P2, P3,…..Pn}表示一组点云数据。 cartographer的PointCloud是由Vector3f组成的vector。 PointCloudWithIntensities则是由点云和光线强度组成的struct类。 */ namespace cartographer { namespace sensor { typedef std::vector PointCloud;//vector，元素是3*1f struct PointCloudWithIntensities { //点云+光线强度,{x,y,z}+intensity PointCloud points; //3*1的vector std::vector intensities; }; // Transforms 'point_cloud' according to 'transform'. 根据三维网格参数转换点云 PointCloud TransformPointCloud(const PointCloud& point_cloud, const transform::Rigid3f& transform); /*去掉z轴区域外的点云,返回一个新的点云*/ // Returns a new point cloud without points that fall outside the region defined // by 'min_z' and 'max_z'. PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z); //序列化 // Converts 'point_cloud' to a proto::PointCloud. proto::PointCloud ToProto(const PointCloud& point_cloud); //反序列化 // Converts 'proto' to a PointCloud. PointCloud ToPointCloud(const proto::PointCloud& proto); } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ ================================================ FILE: sensor/point_cloud_test.cc ================================================ #include "cartographer/sensor/point_cloud.h" #include "cartographer/transform/transform.h" #include #include "gtest/gtest.h" namespace cartographer { namespace sensor { namespace { TEST(PointCloudTest, TransformPointCloud) { PointCloud point_cloud; point_cloud.emplace_back(0.5f, 0.5f, 1.f); //构造一个点云{0.5,0.5,1} point_cloud.emplace_back(3.5f, 0.5f, 42.f); //{3.5,0.5,42} //调用static Rigid2 Rotation(const double rotation) const PointCloud transformed_point_cloud = TransformPointCloud( point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); /*绕z轴逆时针旋转 pi/2: [x',y',x']=[]*[x,y,z] 化简后： x=-y，y=x，z=z */ EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/proto/adaptive_voxel_filter_options.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.sensor.proto; message AdaptiveVoxelFilterOptions { // 'max_length' of a voxel edge. optional float max_length = 1; //不少于min_num // If there are more points and not at least 'min_num_points' remain, the // voxel length is reduced trying to get this minimum number of points. optional float min_num_points = 2; // Points further away from the origin are removed. optional float max_range = 3; } ================================================ FILE: sensor/proto/configuration.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.sensor.proto; import "cartographer/transform/proto/transform.proto"; // NEXT_ID: 16 message Configuration { // NEXT_ID: 4 message Sensor { // 'frame_id' of the sensor. optional string frame_id = 2; // Transformation from 'frame_id' to the tracking frame. optional transform.proto.Rigid3d transform = 3; } repeated Sensor sensor = 15; } ================================================ FILE: sensor/proto/sensor.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.sensor.proto; option java_outer_classname = "Sensor"; import "cartographer/transform/proto/transform.proto"; // Collection of 3D 'points'. message PointCloud { //点云,zyx // Points as repeated floats for efficiency. All fields have the same size. float占4位,double占8位 repeated float x = 3 [packed = true]; repeated float y = 4 [packed = true]; repeated float z = 5 [packed = true]; } // Compressed variant of PointCloud. message CompressedPointCloud { //压缩:点云数量+点云值 optional int32 num_points = 1; repeated int32 point_data = 3 [packed = true]; } // Proto representation of ::cartographer::sensor::RangeData message RangeData { optional transform.proto.Vector3f origin = 1;//sensor坐标 optional PointCloud point_cloud = 2; optional PointCloud missing_echo_point_cloud = 3; } ================================================ FILE: sensor/range_data.cc ================================================ #include "cartographer/sensor/range_data.h" #include "cartographer/sensor/proto/sensor.pb.h" #include "cartographer/transform/transform.h" namespace cartographer { namespace sensor { /* message PointCloud { repeated float x = 3 [packed = true]; repeated float y = 4 [packed = true]; repeated float z = 5 [packed = true]; } message CompressedPointCloud { optional int32 num_points = 1; repeated int32 point_data = 3 [packed = true]; } message RangeData { optional transform.proto.Vector3f origin = 1; optional PointCloud point_cloud = 2; optional PointCloud missing_echo_point_cloud = 3; } PointCloud;//vector，元素是3*1f */ proto::RangeData ToProto(const RangeData& range_data) { proto::RangeData proto; *proto.mutable_origin() = transform::ToProto(range_data.origin); *proto.mutable_point_cloud() = ToProto(range_data.returns); *proto.mutable_missing_echo_point_cloud() = ToProto(range_data.misses); return proto; } RangeData FromProto(const proto::RangeData& proto) { auto range_data = RangeData{ transform::ToEigen(proto.origin()), ToPointCloud(proto.point_cloud()), ToPointCloud(proto.missing_echo_point_cloud()), }; return range_data; } /* 将sensor坐标变换为机器坐标。 */ RangeData TransformRangeData(const RangeData& range_data, const transform::Rigid3f& transform) { return RangeData{ transform * range_data.origin, TransformPointCloud(range_data.returns, transform), TransformPointCloud(range_data.misses, transform), }; } /* 不在给定的z轴范围内的点云删除。 */ RangeData CropRangeData(const RangeData& range_data, const float min_z, const float max_z) { return RangeData{range_data.origin, Crop(range_data.returns, min_z, max_z), Crop(range_data.misses, min_z, max_z)}; } /* 压缩，有精度丢失。 */ CompressedRangeData Compress(const RangeData& range_data) { return CompressedRangeData{ range_data.origin, CompressedPointCloud(range_data.returns), CompressedPointCloud(range_data.misses), }; } /* 解压缩，有精度丢失 */ RangeData Decompress(const CompressedRangeData& compressed_range_data) { return RangeData{compressed_range_data.origin, compressed_range_data.returns.Decompress(), compressed_range_data.misses.Decompress()}; } } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/range_data.h ================================================ #ifndef CARTOGRAPHER_SENSOR_RANGE_DATA_H_ #define CARTOGRAPHER_SENSOR_RANGE_DATA_H_ #include "cartographer/common/port.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/sensor.pb.h" namespace cartographer { namespace sensor { /* RangeData: 数据成员包括 1),原始位置,{x0,y0,z0} 2),返回点云,{x,y,z} 3),缺失点云,标识free space. */ // Rays begin at 'origin'. 'returns' are the points where obstructions were // detected. 'misses' are points in the direction of rays for which no return // was detected, and were inserted at a configured distance. It is assumed that // between the 'origin' and 'misses' is free space. struct RangeData { Eigen::Vector3f origin;//{x0,y0,z0},sensor坐标。 PointCloud returns; //反射位置{x,y,z}，表征有物体反射。 PointCloud misses; //无反射,自由空间 }; // Converts 'range_data' to a proto::RangeData. 序列化 proto::RangeData ToProto(const RangeData& range_data); // Converts 'proto' to a RangeData. 反序列化 RangeData FromProto(const proto::RangeData& proto); //对数据进行3d变换，转换为机器坐标 RangeData TransformRangeData(const RangeData& range_data, const transform::Rigid3f& transform); //根据min_z和max_z把不在z轴范围内的点云丢弃，剪裁到给定范围 // Crops 'range_data' according to the region defined by 'min_z' and 'max_z'. RangeData CropRangeData(const RangeData& range_data, float min_z, float max_z); //压缩后的点云数据 // Like RangeData but with compressed point clouds. The point order changes // when converting from RangeData. struct CompressedRangeData { Eigen::Vector3f origin; CompressedPointCloud returns; CompressedPointCloud misses; }; CompressedRangeData Compress(const RangeData& range_data); RangeData Decompress(const CompressedRangeData& compressed_range_Data); } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_RANGE_DATA_H_ ================================================ FILE: sensor/range_data_test.cc ================================================ #include "cartographer/sensor/range_data.h" #include #include #include "gmock/gmock.h" namespace cartographer { namespace sensor { namespace { using ::testing::Contains; using ::testing::PrintToString; // Custom matcher for Eigen::Vector3f entries. MATCHER_P(ApproximatelyEquals, expected, string("is equal to ") + PrintToString(expected)) { return (arg - expected).isZero(0.001f); } TEST(RangeDataTest, Compression) { const std::vector returns = {Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6), Eigen::Vector3f(0, 1, 2)}; //构造一个测量数据 const RangeData range_data = { Eigen::Vector3f(1, 1, 1), returns, {Eigen::Vector3f(7, 8, 9)}}; //压缩再解压，减少空间占用。 const RangeData actual = Decompress(Compress(range_data)); //isApprox():true if *this is approximately equal to other, within the precision determined by prec. //压缩前后，精度比较 EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6)); EXPECT_EQ(3, actual.returns.size()); EXPECT_EQ(1, actual.misses.size()); EXPECT_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f)); // Returns will be reordered, so we compare in an unordered manner. EXPECT_EQ(3, actual.returns.size()); //是否包含给定值，returns是乱序的。所以使用Contains()函数。 EXPECT_THAT(actual.returns, Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2)))); EXPECT_THAT(actual.returns, Contains(ApproximatelyEquals(Eigen::Vector3f(4, 5, 6)))); } } // namespace } // namespace sensor } // namespace cartographer ================================================ FILE: sensor/voxel_filter.cc ================================================ #include "cartographer/sensor/voxel_filter.h" #include #include "cartographer/common/math.h" namespace cartographer { namespace sensor { namespace { /* 对点云数据，根据指定范围max_range进行l2距离过滤， 形参： points， 返回值： points */ PointCloud FilterByMaxRange(const PointCloud& point_cloud, const float max_range) { PointCloud result; for (const Eigen::Vector3f& point : point_cloud) { if (point.norm() <= max_range) { // l2范数,向量各个元素平方和的1/2次方 result.push_back(point); } } return result; } /* 自适应滤波函数。 形参1：滤波器option， 形参2：待滤波的points 返回值：points voxels_：表征一系列的Cell，每个Cell有一个size，piont以size为间距分布，实现稀疏表示。 Cell的数量和即为稀疏表达的point的数量和。 滤波实现： 1，调整edge的大小，以使每个Cell占据的points个数在min_num_points左右,以实现稀疏表达。 2，按照l2范数测度。 3， */ PointCloud AdaptivelyVoxelFiltered( const proto::AdaptiveVoxelFilterOptions& options, const PointCloud& point_cloud) { if (point_cloud.size() <= options.min_num_points()) { // 'point_cloud' is already sparse enough. return point_cloud; } //max_length即为体素edge的max_size。下面以max_size为间距，稀疏提取point。 PointCloud result = VoxelFiltered(point_cloud, options.max_length()); if (result.size() >= options.min_num_points()) { //即使以max_size为间距也超过min_num.则直接返回。 // Filtering with 'max_length' resulted in a sufficiently dense point cloud. return result; } // Search for a 'low_length' that is known to result in a sufficiently // dense point cloud. We give up and use the full 'point_cloud' if reducing // the edge length by a factor of 1e-2 is not enough. /* 以max_size为间距没有超过min_num.则继续减小edge大小，使得以low_length为间距。 low_length是每个Cell的长度。 每个Cell“占据”的points不能超过min_num. 二分查找法： for()循环每次将edge减半 对每一个low_length: while()循环控制edge不能小于10% 满足要求则记录，否则将high折半。 结果： 每个Cell最少“占据”min_num个points。 */ for (float high_length = options.max_length(); high_length > 1e-2f * options.max_length(); high_length /= 2.f) { float low_length = high_length / 2.f; result = VoxelFiltered(point_cloud, low_length);//edge大小为low_length if (result.size() >= options.min_num_points()) { // Binary search to find the right amount of filtering. 'low_length' gave // a sufficiently dense 'result', 'high_length' did not. We stop when the // edge length is at most 10% off. while ((high_length - low_length) / low_length > 1e-1f) { const float mid_length = (low_length + high_length) / 2.f; const PointCloud candidate = VoxelFiltered(point_cloud, mid_length); if (candidate.size() >= options.min_num_points()) { low_length = mid_length; result = candidate; } else { high_length = mid_length; } } return result; } } return result; } } // namespace /* 形参1：pionts 形参2：size，体素大小 返回值：稀疏点云，points */ PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) { VoxelFilter voxel_filter(size);//创建滤波器 voxel_filter.InsertPointCloud(point_cloud); return voxel_filter.point_cloud(); } VoxelFilter::VoxelFilter(const float size) : voxels_(size) {} //插入点云 void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) { for (const Eigen::Vector3f& point : point_cloud) { /*对每一个点云point,如果落在Cell中，则插入piont，并且后续不在接收落在此Cell的point。 [point1] ,[point2],[point3],[point4].... [point11] ,[point12],[point13],[point14].... */ auto* const value = voxels_.mutable_value(voxels_.GetCellIndex(point)); if (*value == 0) { point_cloud_.push_back(point); *value = 1; } } } //表征Cell边界的point const PointCloud& VoxelFilter::point_cloud() const { return point_cloud_; } /* sparse_pose_graph.lua/trajectory_builder_2d.lua adaptive_voxel_filter = { max_length = 0.9, min_num_points = 100, max_range = 50., }, 2d:adaptive_voxel_filter = { max_length = 0.5, min_num_points = 200, max_range = 50., }, 根据.lua文件选项，配置options滤波器参数。 */ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary) { proto::AdaptiveVoxelFilterOptions options; options.set_max_length(parameter_dictionary->GetDouble("max_length")); options.set_min_num_points( parameter_dictionary->GetNonNegativeInt("min_num_points")); options.set_max_range(parameter_dictionary->GetDouble("max_range")); return options; } //构造函数 AdaptiveVoxelFilter::AdaptiveVoxelFilter( const proto::AdaptiveVoxelFilterOptions& options) : options_(options) {} //将point进行自适应滤波，返回point。 PointCloud AdaptiveVoxelFilter::Filter(const PointCloud& point_cloud) const { return AdaptivelyVoxelFiltered( options_, FilterByMaxRange(point_cloud, options_.max_range())); } } // namespace sensor } // namespace cartographer /* 3d： high_resolution_adaptive_voxel_filter = { max_length = 2., min_num_points = 150, max_range = 15., }, low_resolution_adaptive_voxel_filter = { max_length = 4., min_num_points = 200, max_range = MAX_3D_RANGE, }, */ ================================================ FILE: sensor/voxel_filter.h ================================================ #ifndef CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ #define CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/mapping_3d/hybrid_grid.h" #include "cartographer/sensor/point_cloud.h" #include "cartographer/sensor/proto/adaptive_voxel_filter_options.pb.h" /* message AdaptiveVoxelFilterOptions { optional float max_length = 1; optional float min_num_points = 2; optional float max_range = 3; } 2d: adaptive_voxel_filter = { max_length = 0.9, //voxel_的大小edge的最大值 min_num_points = 100, //voxel_最多“占据”的points数量 max_range = 50., }, Voxel, 三维像素,体素 3D模型体素化：http://blog.csdn.net/bugrunner/article/details/5527332 */ namespace cartographer { namespace sensor { //返回过滤后的点云,size是体素的长度。 // Returns a voxel filtered copy of 'point_cloud' where 'size' is the length // a voxel edge. PointCloud VoxelFiltered(const PointCloud& point_cloud, float size); /* 体素滤波器,对每一个体素voxel,采用第一个point代替所有的points VoxelFilter:不可拷贝/不可赋值 默认构造函数指定体素边界大小 */ // Voxel filter for point clouds. For each voxel, the assembled point cloud // contains the first point that fell into it from any of the inserted point // clouds. class VoxelFilter { public: // 'size' is the length of a voxel edge. explicit VoxelFilter(float size); VoxelFilter(const VoxelFilter&) = delete; VoxelFilter& operator=(const VoxelFilter&) = delete; //将点云插入体素网格中 // Inserts a point cloud into the voxel filter. void InsertPointCloud(const PointCloud& point_cloud); //返回表达occupied 体素的点云 // Returns the filtered point cloud representing the occupied voxels. const PointCloud& point_cloud() const; private: mapping_3d::HybridGridBase voxels_;//以体素表示的网格，Grid/Pixel PointCloud point_cloud_; //网格内的点云 }; /* 自适应体素滤波,不可拷贝/赋值 */ proto::AdaptiveVoxelFilterOptions CreateAdaptiveVoxelFilterOptions( common::LuaParameterDictionary* const parameter_dictionary); class AdaptiveVoxelFilter { public: //根据配置文件设置自适应体素滤波的options explicit AdaptiveVoxelFilter( const proto::AdaptiveVoxelFilterOptions& options); AdaptiveVoxelFilter(const AdaptiveVoxelFilter&) = delete; AdaptiveVoxelFilter& operator=(const AdaptiveVoxelFilter&) = delete; //对点云进行体素滤波,返回过滤后的点云 PointCloud Filter(const PointCloud& point_cloud) const; private: const proto::AdaptiveVoxelFilterOptions options_; }; } // namespace sensor } // namespace cartographer #endif // CARTOGRAPHER_SENSOR_VOXEL_FILTER_H_ ================================================ FILE: sensor/voxel_filter_test.cc ================================================ #include "cartographer/sensor/voxel_filter.h" #include #include "gmock/gmock.h" namespace cartographer { namespace sensor { namespace { using ::testing::ContainerEq; TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { PointCloud point_cloud = {{0.f, 0.f, 0.f}, {0.1f, -0.1f, 0.1f}, {0.3f, -0.1f, 0.f}, {0.f, 0.f, 0.1f}}; EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),//按照max_range滤波, ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); } } // namespace } // namespace sensor } // namespace cartographer /* ContainerEq: The same as Eq(container) except that the failure message also includes which elements are in one container but not the other. */ ================================================ FILE: transform/proto/transform.proto ================================================ // Copyright 2016 The Cartographer Authors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. // You may obtain a copy of the License at // // http://www.apache.org/licenses/LICENSE-2.0 // // Unless required by applicable law or agreed to in writing, software // distributed under the License is distributed on an "AS IS" BASIS, // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. syntax = "proto2"; package cartographer.transform.proto; // All coordinates are expressed in the right-handed Cartesian coordinate system // used by Cartographer (x forward, y left, z up). Message names are chosen to // mirror those used in the Eigen library. //所有的坐标都是右手旋转笛卡儿坐标; message Vector2d { optional double x = 1; optional double y = 2; } message Vector2f { optional float x = 1; optional float y = 2; } message Vector3d { optional double x = 1; optional double y = 2; optional double z = 3; } message Vector3f { optional float x = 1; optional float y = 2; optional float z = 3; } message Quaterniond { optional double x = 1; optional double y = 2; optional double z = 3; optional double w = 4; } message Quaternionf { optional float x = 1; optional float y = 2; optional float z = 3; optional float w = 4; } message Rigid2d { optional Vector2d translation = 1; optional double rotation = 2; } message Rigid2f { optional Vector2f translation = 1; optional float rotation = 2; } message Rigid3d { optional Vector3d translation = 1; optional Quaterniond rotation = 2; } message Rigid3f { optional Vector3f translation = 1; optional Quaternionf rotation = 2; } ================================================ FILE: transform/rigid_transform.cc ================================================ #include "cartographer/transform/rigid_transform.h" #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "glog/logging.h" namespace cartographer { namespace transform { namespace { //typedef Matrix< double , 3 , 1> Eigen::Vector3d //根据lua字典获取vector,再转换成Vector3d Eigen::Vector3d TranslationFromDictionary( common::LuaParameterDictionary* dictionary) { const std::vector translation = dictionary->GetArrayValuesAsDoubles(); CHECK_EQ(3, translation.size()) << "Need (x, y, z) for translation."; return Eigen::Vector3d(translation[0], translation[1], translation[2]); } } // namespace /* 利用AngleAxisd可以根据3个坐标轴旋转.构成四元数 Represents a 3D rotation as a rotation angle around an arbitrary 3D axis. https://stackoverflow.com/questions/21412169/creating-a-rotation-matrix-with-pitch-yaw-roll-using-eigen http://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html 返回根据roll,pathch和yaw构成的4元数 */ Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, const double yaw) { const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX()); //构造一个AngleAxisd对象 const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());//同上 const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());//同上 return yaw_angle * pitch_angle * roll_angle; //返回4元数 } /* 从assets_writer_backpack_2d.lua取得如： XY_TRANSFORM = { translation = { 0., 0., 0. }, rotation = { 0., -math.pi / 2., 0., }, } translation:平移矩阵 rotation:旋转矩阵,w,x,y,z */ transform::Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary) { const Eigen::Vector3d translation = TranslationFromDictionary(dictionary->GetDictionary("translation").get()); auto rotation_dictionary = dictionary->GetDictionary("rotation"); if (rotation_dictionary->HasKey("w")) { const Eigen::Quaterniond rotation(rotation_dictionary->GetDouble("w"), rotation_dictionary->GetDouble("x"), rotation_dictionary->GetDouble("y"), rotation_dictionary->GetDouble("z")); CHECK_NEAR(rotation.norm(), 1., 1e-9); return transform::Rigid3d(translation, rotation); } else { const std::vector rotation = rotation_dictionary->GetArrayValuesAsDoubles(); CHECK_EQ(3, rotation.size()) << "Need (roll, pitch, yaw) for rotation."; return transform::Rigid3d( translation, RollPitchYaw(rotation[0], rotation[1], rotation[2])); } } } // namespace transform } // namespace cartographer ================================================ FILE: transform/rigid_transform.h ================================================ #ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ #include #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/math.h" #include "cartographer/common/port.h" namespace cartographer { namespace transform { /* 几个名词细微的区别 : transformation:变换 translation:平移 rotation:旋转 scaling:缩放 counter clock wise rotation:逆时针旋转，counter：反向 Identity():单位矩阵,不同于线性代数的“单位”。在不同的语义下，含义会不同。 Rigid2是对二维刚性变换【旋转与平移】的封装,SE2.对应论文公式(1): Rigid2表现为先将vector[x0,y0]旋转θ角度得到[x',y']，然后再叠加[dx,dy]得到[x1,y1] 即: x1=x'+dx, y1=y'+dy, 模板参数必须是浮点型。(int类型为错误用法) 提供2个操作： 1，按照指定角度θ逆时针旋转。此时dx,dy=0 2，按照指定平移向量dx,dy平移. 此时θ=0 静态成员函数包括： 1,Rotation() 2,Translation() 3,Identity() Translation()指定对象的2D translation（2D平移）。 第一个参数对应X轴，第二个参数对应Y轴。默认是单位变换。　　　 [x' [cosθ , -sinθ [ x y'] = sinθ , cosθ ] * y ] */ template //FloatType不能是int类型，不然Rotation2D的cos0==0,丢失角度信息，无法得到正确结果 class Rigid2 { public: using Vector = Eigen::Matrix;//2行1列,p142 using Rotation2D = Eigen::Rotation2D; // 无参构造函数,平移向量为单位向量[1,0]^t,旋转角度为0,debugstring()输出:[1,0,0] Rigid2() : translation_(Vector::Identity()), rotation_(Rotation2D::Identity()) {} //Rotation2D(double ): Construct a 2D counter clock wise rotation from the angle a in radian. //双参构造函数，给定平移向量[dx,dy]和旋转角度0，进行旋转变换: Rigid2(const Vector& translation, const Rotation2D& rotation) : translation_(translation), rotation_(rotation) {} //同上，给定旋转角度θ,double是弧度值。 Rigid2(const Vector& translation, const double rotation) : translation_(translation), rotation_(rotation) {} //类的静态成员函数，返回Rigid2,debugstring()是[0,0,θ ] static Rigid2 Rotation(const double rotation) { //给定旋转角度θ return Rigid2(Vector::Zero(), rotation); } //同上 static Rigid2 Rotation(const Rotation2D& rotation) {//给定旋转矩阵,角度为θ return Rigid2(Vector::Zero(), rotation); } //旋转角度是单位矩阵,即θ为0 ,[dx,dy,0] static Rigid2 Translation(const Vector& vector) { return Rigid2(vector, Rotation2D::Identity());//θ为0 } //静态成员函数，全为0： static Rigid2 Identity() { //返回Rigid2,[0,0,0] return Rigid2(Vector::Zero(), Rotation2D::Identity()); } /*为什么要加translation_.template： 参见成员模板函数的写法： https://stackoverflow.com/questions/29754251/issue-casting-c-eigenmatrix-types-via-templates https://stackoverflow.com/questions/12676190/how-to-call-a-template-member-function https://stackoverflow.com/questions/4942703/why-do-i-get-an-error-trying-to-call-a-template-member-function-with-an-explicit?answertab=votes cast()按照指定的参数类型将数据成员进行类型转换： Rigid2数据成员原本是double,对象rigid2转换成float可调用： rigid2.cast() */ template Rigid2 cast() const { return Rigid2(translation_.template cast(), rotation_.template cast()); } //返回平移向量,[dx,dy] const Vector& translation() const { return translation_; } /*返回旋转矩阵: [cosθ , -sinθ sinθ , cosθ ] */ Rotation2D rotation() const { return rotation_; } //归一化角度 ,弧度[-pi;pi] double normalized_angle() const { return common::NormalizeAngleDifference(rotation().angle()); //方位角θ.以弧度计。 } /* 平移变换矩阵的逆矩阵与原来的平移量相同，但是方向相反。 旋转变换矩阵的逆矩阵与原来的旋转轴相同,但是角度相反。 Rotation2D重载了operator*(maxtrix).故可以使用乘法。 乘以rotation：[dx,dy]是旋转了θ以后的[dx,dy] 而求逆是反方向旋转θ，故需要求[dx,dy]在旋转θ之前的方向的投影。 translation加负号的原因：x1=x'+dx，所以x'=x1-dx ----公式推导---------- P'=R*P+t; P=R^(-1)*(P'-t)=R^(-1)* P'-R^(-1)*t. -------------------- */ Rigid2 inverse() const { const Rotation2D rotation = rotation_.inverse(); //取负,得到-θ。即上面公式中的R^(-1) const Vector translation = -(rotation * translation_);//得到[-dx',-dy']。即上面公式中的-R^(-1)*t. return Rigid2(translation, rotation); //返回一个新2维刚性变换矩阵.[-dx’,-dy’,-θ]。即上面公式中的R^(-1)和-R^(-1)*t。 } string DebugString() const { string out; out.append("{ t: ["); out.append(std::to_string(translation().x())); out.append(", "); out.append(std::to_string(translation().y())); out.append("], r: ["); out.append(std::to_string(rotation().angle())); out.append("] }"); return out; } private: //2行1列 的矩阵.平移向量[dx,dy] Vector translation_; //旋转角度。 Rotation2D rotation_;//Eigen::Rotation2D,方向角,旋转变换 [θ] /* Rotation2D介绍： Rotation2D 是二维旋转里根据逆时针旋转θ角而生成的旋转矩阵: [cosθ , -sinθ sinθ , cosθ ] */ }; /* 定义 * 乘法操作符:2个Rigid2相乘,得到第三个Rigid2, 等效于连续变换2次 实现细节： 1，最终的[dx'',dy'']等于lhs在rhs方向上的投影[dx',dy']加上rhs自身的[dx,dy] 2，角度相加(等效于旋转矩阵相乘) ------公式推导-------- P'=R1*P+t1; P''=R2*P'+t2,即 P''=R2(R1*P+t1)+t2=R2*R1*P+R2*t1+t2. -------------------- */ template Rigid2 operator*(const Rigid2& lhs, const Rigid2& rhs) { return Rigid2( lhs.rotation() * rhs.translation() + lhs.translation(),//对应R2*t1+t2. lhs.rotation() * rhs.rotation()); //对应R2*R1 } /* 该函数即为公式(1): 参数1：【旋转+平移】矩阵 参数2：要进行上述变换的point。 返回值：刚性变换后的point。 等效于对某个2维point旋转0角，再叠加平移矩阵[dx,dy]。 实现： 1，先对vector[x0,y0]自身旋转0，得到[x',y']. 2，[x',y']再加上dx,dy.得到vector[x1,y1] --------公式推导--------- P'=R*P+t; ------------------------ */ template typename Rigid2::Vector operator*( const Rigid2& rigid, const typename Rigid2::Vector& point) { return rigid.rotation() * point + rigid.translation(); /* Rotation2D的乘法原型： Vector2 Eigen::Rotation2D< Scalar >::operator* ( const Vector2 & vec ) const : Applies the rotation to a 2D vector */ } //定义 << 输出运算符 template std::ostream& operator<<(std::ostream& os, const cartographer::transform::Rigid2& rigid) { os << rigid.DebugString(); return os; } //模板特化 using Rigid2d = Rigid2; using Rigid2f = Rigid2; /* 预备概念： 四元数,通常用quaternion来计算3D物体的旋转角度，与Matrix相比，quaternion更加高效。 在数学上，quaternion表示复数w+xi+yj+zk，其中i,j,k都是虚数单位。 四元数也可以表示为[w,iv].其中虚数v=[x,y,z]，用四元数描述旋转需要的存储空间很小。 更为关键的是可以使用被称为球面线性插值（Slerp Algorithm）的方法对四元数进行插值运算， 从而解决了平滑旋转的插值问题。 3D旋转可以分解为绕x,y,z三个方向的旋转的叠加。 而Eigen的AngleAxis表示3D旋转绕任意轴axis的旋转。 并且与MatrixBase::Unit{X,Y,Z}组合,AngleAxis 能非常容易的得到欧垃角。 */ /* Rigid3是三维刚性变换.SE3 该类含义2个数据成员： Vector translation_; Quaternion rotation_; 1),3个构造函数分别初始化平移矩阵和旋转矩阵。 2),3个静态成员函数分别按照给定方式旋转. Rotation(),Translation(),Identity() 构造函数应为单位四元数 */ template class Rigid3 { public: using Vector = Eigen::Matrix; //3行1列的矩阵 using Quaternion = Eigen::Quaternion;//四元数,见游戏引擎架构p156 using AngleAxis = Eigen::AngleAxis; //轴角 //构造函数，默认[1,0,0]和[1,0,0,0]。 Rigid3() : translation_(Vector::Identity()), rotation_(Quaternion::Identity()) {} //构造函数，提供平移向量[dx,dy,dz]和四元数 Rigid3(const Vector& translation, const Quaternion& rotation) : translation_(translation), rotation_(rotation) {} //构造函数，提供平移向量[dx,dy,dz]和轴角 Rigid3(const Vector& translation, const AngleAxis& rotation) : translation_(translation), rotation_(rotation) {} //静态成员函数.[dx,dy,dz]为0,只绕轴角旋转。 static Rigid3 Rotation(const AngleAxis& angle_axis) { return Rigid3(Vector::Zero(), Quaternion(angle_axis)); } //静态成员函数, 只旋转，不平移。 static Rigid3 Rotation(const Quaternion& rotation) { return Rigid3(Vector::Zero(), rotation); } //不旋转,只平移[dx,dy,dz] static Rigid3 Translation(const Vector& vector) { return Rigid3(vector, Quaternion::Identity());//[x,y,z,w]为[0,0,0,1] } //[0,0,0]和[1,0,0,0]。 static Rigid3 Identity() { return Rigid3(Vector::Zero(), Quaternion::Identity()); } //类型转换 template Rigid3 cast() const { return Rigid3(translation_.template cast(), rotation_.template cast()); } //获取数据成员 const Vector& translation() const { return translation_; } const Quaternion& rotation() const { return rotation_; } /*求逆,即逆方向旋转和平移。 细节： 1),四元数的逆是共轭。 2),求[-dx',-dy',-dz'] ----公式推导---------- P'=R*P+t; P=R^(-1)*(P'-t)=R^(-1)* P'-R^(-1)*t. -------------------- */ Rigid3 inverse() const { const Quaternion rotation = rotation_.conjugate(); //R^(-1) const Vector translation = -(rotation * translation_); //-R^(-1)*t. return Rigid3(translation, rotation); } string DebugString() const { string out; out.append("{ t: ["); out.append(std::to_string(translation().x())); out.append(", "); out.append(std::to_string(translation().y())); out.append(", "); out.append(std::to_string(translation().z())); out.append("], q: ["); out.append(std::to_string(rotation().w())); out.append(", "); out.append(std::to_string(rotation().x())); out.append(", "); out.append(std::to_string(rotation().y())); out.append(", "); out.append(std::to_string(rotation().z())); out.append("] }"); return out; } private: Vector translation_;//x,y,z方向上的平移向量[dx,dy,dz] Quaternion rotation_;//四元数。旋转表达。 }; //乘法操作：Rigid3*Rigid3,即连续2次刚性变换，得到Rigid3 /* ------公式推导-------- P'=R1*P+t1; P''=R2*P'+t2,即 P''=R2(R1*P+t1)+t2=R2*R1*P+R2*t1+t2. -------------------- */ template Rigid3 operator*(const Rigid3& lhs, const Rigid3& rhs) { return Rigid3( lhs.rotation() * rhs.translation() + lhs.translation(), //R2*t1+t2 (lhs.rotation() * rhs.rotation()).normalized()); //R2*R1 } /* 对点p(x,y,z)进行刚性变换。得到p' --------公式推导--------- P'=R*P+t; ------------------------ */ template typename Rigid3::Vector operator*( const Rigid3& rigid, const typename Rigid3::Vector& point) { return rigid.rotation() * point + rigid.translation(); } //重载<< 输出运算符 // This is needed for gmock. template std::ostream& operator<<(std::ostream& os, const cartographer::transform::Rigid3& rigid) { os << rigid.DebugString(); return os; } //特化 using Rigid3d = Rigid3; using Rigid3f = Rigid3; /* -----------左手--------------- 游戏引擎架构,p126,姿态角（Euler角）: pitch是围绕X轴旋转，也叫做俯仰角 yaw是围绕Y轴旋转，也叫偏航角 roll是围绕Z轴旋转，也叫翻滚角 -----------右手------------ 右手笛卡尔坐标系绕x,y和z轴的旋转分别叫做roll, pitch 和 yaw 旋转。 google采用的就是右手:roll, pitch, yaw. 绕x轴:θx 是 roll 角，和右手螺旋的方向相反（在yz平面顺时针） 绕y轴:θy 是 pitch 角，和右手螺旋的方向相反（在zx平面顺时针）。 绕z轴:θz 是yaw 角，和右手螺旋的方向相反（在xy平面顺时针）。 ref:https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5 */ //返回根据roll,pathch和yaw构成的4元数 // Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF // specification http://wiki.ros.org/urdf/XML/joint. Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw); /* 从assets_writer_backpack_2d.lua取得如： XY_TRANSFORM = { translation = { 0., 0., 0. }, rotation = { 0., -math.pi / 2., 0., }, } */ // Returns an transform::Rigid3d given a 'dictionary' containing 'translation' // (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw) // or a dictionary with (w, x, y, z) values as a quaternion. Rigid3d FromDictionary(common::LuaParameterDictionary* dictionary); } // namespace transform } // namespace cartographer #endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_ /* coefficient-wise operations系数操作 the Array class provides an easy way to perform coefficient-wise operations, which might not have a linear algebraic meaning, such as adding a constant to every coefficient in the array or multiplying two arrays coefficient-wise. */ /* 3D旋转的简单推导： 可以把一个3D旋转转换为绕基本坐标轴的旋转，即绕三个坐标值x、y、z的旋转。 一）绕X轴的旋转：x不变。围绕x坐标画圆。yoz平面，方向是顺时针(与右手方向相反) 点P(x,y,z)绕x轴旋转θ角得到点P’(x’,y’,z’)。由于是绕x轴进行的旋转， 因此x坐标保持不变，y和z组成的yoz（o是坐标原点）平面上进行的是一个二维的旋转 p=[1,0,0],q=[0,1,0],r=[0,0,1] 三个坐标轴单位向量p,q,r旋转后： p'=[1, 0, 0] q'=[0,cosθ,-sinθ] r'=[0,sinθ,cosθ] 所以: x′=x y′=ycosθ−zsinθ z′=ysinθ+zcosθ 二）绕Y轴旋转：Y坐标保持不变，ZOX平面 p'=[cosθ, 0, sinθ] q'=[ 0, 1, 0] r'=[-sinθ,0,cosθ] 所以： x′=xcosθ+zsinθ y′=y z′=−xsinθ+zcosθ 三）绕Z轴旋转：Z坐标保持不变，xoy平面 p'=[sinθ,-cosθ,0] q'=[sinθ, cosθ,0] r'=[0,0,1] 所以: x′=xcosθ-ysinθ y′=xsinθ+ycosθ z′=z ref: https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%89%E7%BE%A4 https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5 */ ================================================ FILE: transform/rigid_transform_le2_grid2_test.cc ================================================ #include "cartographer/transform/rigid_transform.h" #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/lua_parameter_dictionary.h" #include "glog/logging.h" #include "gtest/gtest.h" #include namespace cartographer { namespace transform { #define sout(Xit) {std::cout<<__LINE__<<" "<< Xit <<""< r1; sout(r1); //[1,0,0] sout(r1.Identity()); //[0,0,0] sout(r1.Rotation(3.14/2));//[0,0,1.57] sout(r1.Translation({1,2}));//[1,2,0],静态成员函数，与对象无关 sout(Rigid2::Translation({1,2}))//同上 sout(r1.inverse()); //[-1,-0,-0] sout(r1.inverse().inverse());//[1,0,0] sout(r1*r1); //[2,0,0] Eigen::Matrix mat{1,2}; sout(r1*mat); //[2,2] ：[1,0]+[1,2] //sout((r1*mat)(1,0));//Eigen矩阵索引是()而不是[] } TEST(Rigid2s, rigid_2) { Rigid2 r1({1,2},M_PI_2); sout(M_PI_2) //1.57 sout(r1); //[1,2,1.57] ，pi/2对应的弧度是1.57 sout(r1.Identity()); //[0,0,0] sout(r1.Rotation(3.14/2));//[0,0,1.57]静态成员函数，与对象无关 //sout(Rigid2::Rotation(3.14/2));//同上 sout(r1.Translation({1,2}));//[1,2,0] sout(r1); //[1,2,1.57] sout(r1.inverse()); //[-2,1,-1.57] sout(r1.inverse().inverse()); Rigid2 r2({2,2},M_PI_2); sout(r2.cast()); //[2.0,2.0,1.0],成员模板函数cast() Rigid2 r3({3,2},3.2*2); sout(r3); //[3,2,6.4] sout(r3.normalized_angle()); //0.117 } TEST(Rigid2s, rigid_3) { Rigid2 r1({1,2},M_PI_2); Rigid2 r2({2,2},M_PI_2); Rigid2 r4({2,2},M_PI_2); sout(r1); //[1,2,1.57] sout(r1*r1); //[-1,3,3.14] sout(r1*r2); //[-1,4,3.14] Eigen::Matrix mat{1,1}; Eigen::Matrix matd{1,1}; sout(r1.rotation().matrix ()); //[0,-1;1,0] sout(r1.rotation() * mat ); //[-1,1] sout(r1.translation()); //[1,2] sout(mat); //[1,1] sout(r1*mat); //[0,3]. sout(r4.rotation().matrix ( ) );//[0,-1;1,0] sout(r4.rotation() * matd ); //[-1,1] sout(r4.translation()); //[2,2] sout(matd); //[1,1] sout(r4*matd); //[1,3]. } } } // namespace transform } // namespace cartographer ================================================ FILE: transform/rigid_transform_test_helpers.h ================================================ #ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/port.h" #include "cartographer/transform/rigid_transform.h" #include "gmock/gmock.h" #include "gtest/gtest.h" namespace cartographer { namespace transform { /* 本文件用于辅助gtest Eigen::Translation:Represents an homogeneous transformation in a N dimensional space. */ // template Eigen::Transform ToEigen(const Rigid2& rigid2) { return Eigen::Translation(rigid2.translation()) * rigid2.rotation(); } template Eigen::Transform ToEigen(const Rigid3& rigid3) { return Eigen::Translation(rigid3.translation()) * rigid3.rotation(); } /*gmock模拟对象行为*/ MATCHER_P2(IsNearly, rigid, epsilon, string(string(negation ? "isn't" : "is", " nearly ") + rigid.DebugString())) { return ToEigen(arg).isApprox(ToEigen(rigid), epsilon); } } // namespace transform } // namespace cartographer #endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ ================================================ FILE: transform/transform.cc ================================================ #include "cartographer/transform/transform.h" namespace cartographer { namespace transform { //proto的translation对应被旋转矩阵,rotation对应旋转角度数值 Rigid2d ToRigid2(const proto::Rigid2d& pose) { return Rigid2d({pose.translation().x(), pose.translation().y()}, pose.rotation()); } Eigen::Vector2d ToEigen(const proto::Vector2d& vector) { return Eigen::Vector2d(vector.x(), vector.y()); //proto的x对应eigen的x,y对应y } Eigen::Vector3f ToEigen(const proto::Vector3f& vector) { //同上 return Eigen::Vector3f(vector.x(), vector.y(), vector.z()); } Eigen::Vector3d ToEigen(const proto::Vector3d& vector) { //同上 return Eigen::Vector3d(vector.x(), vector.y(), vector.z()); } //proto的Quaterniond四元数x,y,x,w对应Eigen的w,x,y,z Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion) { return Eigen::Quaterniond(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); } proto::Rigid2d ToProto(const transform::Rigid2d& transform) { //序列化,将二维数转化成proto proto::Rigid2d proto; proto.mutable_translation()->set_x(transform.translation().x()); proto.mutable_translation()->set_y(transform.translation().y()); proto.set_rotation(transform.rotation().angle()); //angle()返回方位角,弧度[-pi;pi] return proto; } proto::Rigid2f ToProto(const transform::Rigid2f& transform) { proto::Rigid2f proto; proto.mutable_translation()->set_x(transform.translation().x());//指针运算 proto.mutable_translation()->set_y(transform.translation().y()); proto.set_rotation(transform.rotation().angle()); return proto; } proto::Rigid3d ToProto(const transform::Rigid3d& rigid) { proto::Rigid3d proto; *proto.mutable_translation() = ToProto(rigid.translation());//加*,赋值运算 *proto.mutable_rotation() = ToProto(rigid.rotation()); return proto; } transform::Rigid3d ToRigid3(const proto::Rigid3d& rigid) { //调用的是transform::Rigid3d()的构造函数, //构造函数里又调用的ToEigen(const proto::Vector3d& vector).. return transform::Rigid3d(ToEigen(rigid.translation()), ToEigen(rigid.rotation())); } proto::Rigid3f ToProto(const transform::Rigid3f& rigid) { proto::Rigid3f proto; *proto.mutable_translation() = ToProto(rigid.translation()); *proto.mutable_rotation() = ToProto(rigid.rotation()); return proto; } proto::Vector2d ToProto(const Eigen::Vector2d& vector) { proto::Vector2d proto; proto.set_x(vector.x()); proto.set_y(vector.y()); return proto; } proto::Vector3f ToProto(const Eigen::Vector3f& vector) { proto::Vector3f proto; proto.set_x(vector.x()); proto.set_y(vector.y()); proto.set_z(vector.z()); return proto; } proto::Vector3d ToProto(const Eigen::Vector3d& vector) { proto::Vector3d proto; proto.set_x(vector.x()); proto.set_y(vector.y()); proto.set_z(vector.z()); return proto; } proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion) { proto::Quaternionf proto; proto.set_w(quaternion.w()); proto.set_x(quaternion.x()); proto.set_y(quaternion.y()); proto.set_z(quaternion.z()); return proto; } proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion) { proto::Quaterniond proto; proto.set_w(quaternion.w()); proto.set_x(quaternion.x()); proto.set_y(quaternion.y()); proto.set_z(quaternion.z()); return proto; } } // namespace transform } // namespace cartographer ================================================ FILE: transform/transform.h ================================================ #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ #define CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/transform/proto/transform.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace transform { //google采用三维 xyz:roll, pitch, yaw /* atan2:返回一个非负的弧度值, double atan2(double y,double x) :计算复数 x+yi 的辐角 vec()：a vector expression of the imaginary part (x,y,z) norm()： for vectors, the l2 norm of *this, for matrices the Frobenius norm。 w() : the w coefficient 为何乘以2: 四元数q=[cos(θ/2),sin(θ/2)x,sin(θ/2)y,sin(θ/2)z] */ // Returns the non-negative rotation angle in radians of the 3D transformation // 'transform'. template FloatType GetAngle(const Rigid3& transform) { return FloatType(2) * std::atan2(transform.rotation().vec().norm(), std::abs(transform.rotation().w())); } /* 返回3D变换yaw方向的弧度值,也就是z轴方向的弧度。 */ // Returns the yaw component in radians of the given 3D 'rotation'. Assuming // 'rotation' is composed of three rotations around X, then Y, then Z, returns // the angle of the Z rotation. template T GetYaw(const Eigen::Quaternion& rotation) { const Eigen::Matrix direction = rotation * Eigen::Matrix::UnitX(); return atan2(direction.y(), direction.x()); } /* 返回3D变换yaw方向(z轴)的弧度值 */ // Returns the yaw component in radians of the given 3D transformation // 'transform'. template T GetYaw(const Rigid3& transform) { return GetYaw(transform.rotation()); } /* 根据四元数返回绕任意轴旋转的矩阵。angle-axis */ // Returns an angle-axis vector (a vector with the length of the rotation angle // pointing to the direction of the rotation axis) representing the same // rotation as the given 'quaternion'. template Eigen::Matrix RotationQuaternionToAngleAxisVector( const Eigen::Quaternion& quaternion) { Eigen::Quaternion normalized_quaternion = quaternion.normalized(); // We choose the quaternion with positive 'w', i.e., the one with a smaller // angle that represents this orientation. if (normalized_quaternion.w() < 0.) { //w为负时，全部乘以-1 // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 normalized_quaternion.w() *= T(-1.); normalized_quaternion.x() *= T(-1.); normalized_quaternion.y() *= T(-1.); normalized_quaternion.z() *= T(-1.); } // We convert the normalized_quaternion into a vector along the rotation axis // with length of the rotation angle. //得到角度angle const T angle = T(2.) * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w()); constexpr double kCutoffAngle = 1e-7; // We linearize below this angle. //scale 放大倍数。 const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.)); return Eigen::Matrix(scale * normalized_quaternion.x(), scale * normalized_quaternion.y(), scale * normalized_quaternion.z()); } /* 与上面函数相反的操作。 给定绕angle-axis旋转，返回四元数。 */ // Returns a quaternion representing the same rotation as the given 'angle_axis' // vector. template Eigen::Quaternion AngleAxisVectorToRotationQuaternion( const Eigen::Matrix& angle_axis) { T scale = T(0.5); T w = T(1.); constexpr double kCutoffAngle = 1e-8; // We linearize below this angle. if (angle_axis.squaredNorm() > kCutoffAngle) { const T norm = angle_axis.norm(); scale = sin(norm / 2.) / norm; w = cos(norm / 2.); } const Eigen::Matrix quaternion_xyz = scale * angle_axis; return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(), quaternion_xyz.z()); } /*投影到2维平面xy。 具体： 1,取平移中的[dx,dy], 2,xy平面的平移角度是yaw. */ // Projects 'transform' onto the XY plane. template Rigid2 Project2D(const Rigid3& transform) { return Rigid2(transform.translation().template head<2>(), GetYaw(transform)); } /* 将2维变换转换为3维变换。 具体: 1,平移矩阵为[dx,dy,0] 2,z方向单位旋转。 */ // Embeds 'transform' into 3D space in the XY plane.Embeds：嵌入。z轴不变。 template Rigid3 Embed3D(const Rigid2& transform) { return Rigid3( {transform.translation().x(), transform.translation().y(), T(0)}, Eigen::AngleAxis(transform.rotation().angle(), Eigen::Matrix::UnitZ())); } /* 序列化和反序列化的函数,实现在.cc文件 */ // Conversions between Eigen and proto. Rigid2d ToRigid2(const proto::Rigid2d& transform); Eigen::Vector2d ToEigen(const proto::Vector2d& vector); Eigen::Vector3f ToEigen(const proto::Vector3f& vector); Eigen::Vector3d ToEigen(const proto::Vector3d& vector); Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion); proto::Rigid2d ToProto(const Rigid2d& transform); proto::Rigid2f ToProto(const Rigid2f& transform); proto::Rigid3d ToProto(const Rigid3d& rigid); Rigid3d ToRigid3(const proto::Rigid3d& rigid); proto::Rigid3f ToProto(const Rigid3f& rigid); proto::Vector2d ToProto(const Eigen::Vector2d& vector); proto::Vector3f ToProto(const Eigen::Vector3f& vector); proto::Vector3d ToProto(const Eigen::Vector3d& vector); proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion); proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion); } // namespace transform } // namespace cartographer #endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_H_ ================================================ FILE: transform/transform_interpolation_buffer.cc ================================================ #include "cartographer/transform/transform_interpolation_buffer.h" #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/make_unique.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { namespace transform { void TransformInterpolationBuffer::Push(const common::Time time, const transform::Rigid3d& transform) { if (deque_.size() > 0) { CHECK_GE(time, latest_time()) << "New transform is older than latest."; } deque_.push_back(TimestampedTransform{time, transform});//构造临时对象 } bool TransformInterpolationBuffer::Has(const common::Time time) const { if (deque_.empty()) { return false; } return earliest_time() <= time && time <= latest_time();//检查时间合理性 } transform::Rigid3d TransformInterpolationBuffer::Lookup( //按照time查找 const common::Time time) const { CHECK(Has(time)) << "Missing transform for: " << time; //lower_bound,二分查找的函数,查找序列中的第一个出现的值大于等于time的位置。（非小于） auto start = std::lower_bound(deque_.begin(), deque_.end(), time, [](const TimestampedTransform& timestamped_transform, //自定义lambda表达式,按照时间戳比较 const common::Time time) { return timestamped_transform.time < time; }); /*由于支持插值，所有当time=30,deque=[10,20,20,30,30,50]时： start=30 ，直接返回30 */ auto end = start;//迭代器 if (end->time == time) { return end->transform; } /*由于支持插值，所有当time=20,deque=[10,20,20,30,30,50]时： start=30 ，返回20 */ --start; if (start->time == time) { return start->transform; } /*由于支持插值，所有当time=25,deque=[10,20,20,30,30,50]时： start=30 ，end=20，返回通过插值得到的25 */ const double duration = common::ToSeconds(end->time - start->time); const double factor = common::ToSeconds(time - start->time) / duration; //[dx,dy,dz]按比例缩放。 const Eigen::Vector3d origin = start->transform.translation() + (end->transform.translation() - start->transform.translation()) * factor; //roation则使用slerp插值。 const Eigen::Quaterniond rotation = Eigen::Quaterniond(start->transform.rotation()) .slerp(factor, Eigen::Quaterniond(end->transform.rotation())); //返回插值得到的变换矩阵 return transform::Rigid3d(origin, rotation); } common::Time TransformInterpolationBuffer::earliest_time() const { CHECK(!empty()) << "Empty buffer."; return deque_.front().time; } common::Time TransformInterpolationBuffer::latest_time() const { CHECK(!empty()) << "Empty buffer."; return deque_.back().time; } bool TransformInterpolationBuffer::empty() const { return deque_.empty(); } std::unique_ptr TransformInterpolationBuffer::FromTrajectory( const mapping::proto::Trajectory& trajectory) { auto interpolation_buffer = common::make_unique();//返回智能指针 for (const mapping::proto::Trajectory::Node& node : trajectory.node()) { interpolation_buffer->Push(common::FromUniversal(node.timestamp()), transform::ToRigid3(node.pose())); } return interpolation_buffer; } } // namespace transform } // namespace cartographer /* ForwardIterator lower_bound (ForwardIterator first, ForwardIterator last, const T& val, Compare comp); 返回一个非递减序列[first, last)中的第一个大于等于值val的位置。 upper_bound返回的是最后一个大于val的位置，也是有一个新元素val进来时的插入位置。 10 10 10 20 20 20 30 30 lower_bound: ^ upper_bound: ^ */ ================================================ FILE: transform/transform_interpolation_buffer.h ================================================ #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ #define CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ #include #include #include "cartographer/common/time.h" #include "cartographer/mapping/proto/trajectory.pb.h" #include "cartographer/transform/rigid_transform.h" namespace cartographer { namespace transform { /* 基于时间有序的变换,支持在队列中按时间顺序查找,即使变换不存在于队列中，任然支持相邻时间内的插值变换进行近似。 作用与ROS的tf2函数族类似。 数据成员： 1,deque_; 成员函数： 1,Push() 2,Has() 3,Lookup() 4,earliest_time() 5,latest_time() 6,empty() */ // A time-ordered buffer of transforms that supports interpolated lookups. class TransformInterpolationBuffer { public: //函数,返回智能指针 static std::unique_ptr FromTrajectory( const mapping::proto::Trajectory& trajectory); /* 添加变换到队列尾部,当缓冲区已满时,删除队首元素 */ // Adds a new transform to the buffer and removes the oldest transform if the // buffer size limit is exceeded. void Push(common::Time time, const transform::Rigid3d& transform); //返回能否在给定时间内计算的插值变换。time应在early-old之间，可以插值。 // Returns true if an interpolated transfrom can be computed at 'time' bool Has(common::Time time) const; //返回time处的变换,可插值 // Returns an interpolated transform at 'time'. CHECK()s that a transform at // 'time' is available. transform::Rigid3d Lookup(common::Time time) const; /* 返回队列缓冲区内变换的最早时间，也就是队首元素。 */ // Returns the timestamp of the earliest transform in the buffer or 0 if the // buffer is empty. common::Time earliest_time() const; /* 最晚时间，也就是队尾元素 */ // Returns the timestamp of the earliest transform in the buffer or 0 if the // buffer is empty. common::Time latest_time() const; // Returns true if the buffer is empty. bool empty() const; private: struct TimestampedTransform { common::Time time; //发生时间 transform::Rigid3d transform;//变换矩阵 }; std::deque deque_; //队列，元素是带时间戳的变换,存储了一段时间内的变换矩阵信息 }; } // namespace transform } // namespace cartographer #endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_INTERPOLATION_BUFFER_H_ ================================================ FILE: transform/transform_interpolation_buffer_test.cc ================================================ #include "cartographer/transform/transform_interpolation_buffer.h" #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { namespace transform { namespace { TEST(TransformInterpolationBufferTest, testHas) { TransformInterpolationBuffer buffer; EXPECT_FALSE(buffer.Has(common::FromUniversal(50)));//false 50us buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity());// EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));//false，只有1个元素。 EXPECT_TRUE(buffer.Has(common::FromUniversal(50)));//true，1个元素 EXPECT_FALSE(buffer.Has(common::FromUniversal(75))); //false buffer.Push(common::FromUniversal(100), transform::Rigid3d::Identity()); EXPECT_FALSE(buffer.Has(common::FromUniversal(25)));//false，50-100才是true EXPECT_TRUE(buffer.Has(common::FromUniversal(50))); EXPECT_TRUE(buffer.Has(common::FromUniversal(75)));//在50-100之间 EXPECT_TRUE(buffer.Has(common::FromUniversal(100))); EXPECT_FALSE(buffer.Has(common::FromUniversal(125)));//不在 EXPECT_EQ(common::FromUniversal(50), buffer.earliest_time()); EXPECT_EQ(common::FromUniversal(100), buffer.latest_time()); } TEST(TransformInterpolationBufferTest, testLookup) { TransformInterpolationBuffer buffer; buffer.Push(common::FromUniversal(50), transform::Rigid3d::Identity()); // The rotation needs to be relatively small in order for the // interpolation to remain a z-axis rotation. buffer.Push(common::FromUniversal(100), transform::Rigid3d::Translation(Eigen::Vector3d(10., 10., 10.)) * transform::Rigid3d::Rotation( Eigen::AngleAxisd(2., Eigen::Vector3d::UnitZ()))); const common::Time time = common::FromUniversal(75); //75在50-100之间，可以使用插值计算。 const transform::Rigid3d interpolated = buffer.Lookup(time); EXPECT_THAT( interpolated, IsNearly(transform::Rigid3d::Translation(Eigen::Vector3d(5., 5., 5.)) * transform::Rigid3d::Rotation( Eigen::AngleAxisd(1., Eigen::Vector3d::UnitZ())), 1e-6)); } } // namespace } // namespace transform } // namespace cartographer ================================================ FILE: transform/transform_test.cc ================================================ #include "cartographer/transform/transform.h" #include #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform_test_helpers.h" #include "gtest/gtest.h" namespace cartographer { namespace transform { namespace { TEST(TransformTest, GetAngle) { std::mt19937 rng(42);//随机数种子 std::uniform_real_distribution angle_distribution(0.f, M_PI); //[0- pi],概率均匀分布 std::uniform_real_distribution position_distribution(-1.f, 1.f); //[-1 ,1] for (int i = 0; i != 100; ++i) { const float angle = angle_distribution(rng); const float x = position_distribution(rng); const float y = position_distribution(rng); const float z = position_distribution(rng); const Eigen::Vector3f axis = Eigen::Vector3f(x, y, z).normalized(); //, for vectors, the l2 norm of *this, and for matrices the Frobenius norm. //给定角度绕axis旋转，得到Rigid3f，再求Rigid3d的angle。2者理论上应该相同 EXPECT_NEAR(angle, GetAngle(Rigid3f::Rotation(AngleAxisVectorToRotationQuaternion( Eigen::Vector3f(angle * axis)))), 1e-6f); } } TEST(TransformTest, GetYaw) { const auto rotation = Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ())); EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); EXPECT_NEAR(-1.2345, GetYaw(rotation.inverse()), 1e-9);//四元数的逆就是共轭。 } TEST(TransformTest, GetYawAxisOrdering) { //乘法顺序不能颠倒。zyx const auto rotation = Rigid3d::Rotation(Eigen::AngleAxisd(1.2345, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.4321, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.6789, Eigen::Vector3d::UnitX())); EXPECT_NEAR(1.2345, GetYaw(rotation), 1e-9); } TEST(TransformTest, Embed3D) { const Rigid2d rigid2d({1., 2.}, 0.); const Rigid3d rigid3d( Rigid3d::Translation(Eigen::Vector3d(1., 2., 3.)) * Rigid3d::Rotation(Eigen::Quaterniond(1., 2., 3., 4.).normalized())); const Rigid3d expected = rigid3d * Rigid3d::Translation(Eigen::Vector3d(1., 2., 0.)); EXPECT_THAT(expected, IsNearly(rigid3d * Embed3D(rigid2d), 1e-9)); } } // namespace } // namespace transform } // namespace cartographer