├── README.md ├── calib_odom ├── CMakeLists.txt ├── README.assets │ ├── 1554776817122.png │ ├── 1554777572171.png │ └── 1554777724377.png ├── README.md ├── include │ └── calib_odom │ │ └── Odom_Calib.hpp ├── launch │ └── odomCalib.launch ├── package.xml ├── src │ ├── Odom_Calib.cpp │ └── main.cpp ├── 第一课（公开课）PPT.pdf └── 第二课-里程计.pdf ├── cartographer_annotations ├── .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.cc │ ├── sparse_pose_graph.h │ ├── 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 │ ├── 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.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ ├── optimization_problem.h │ │ └── spa_cost_function.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.cc │ ├── sparse_pose_graph.h │ ├── sparse_pose_graph │ │ ├── constraint_builder.cc │ │ ├── constraint_builder.h │ │ ├── optimization_problem.cc │ │ ├── optimization_problem.h │ │ ├── optimization_problem_test.cc │ │ └── spa_cost_function.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 ├── champion_nav_msgs ├── CMakeLists.txt ├── msg │ └── ChampionNavLaserScan.msg └── package.xml ├── gmapping ├── README.assets │ ├── 1555126014034.png │ └── gmapping.png ├── README.md ├── bag │ └── gmapping.bag ├── openslam_gmapping │ ├── .gitignore │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README │ ├── build_tools │ │ ├── Makefile.app │ │ ├── Makefile.generic-shared-object │ │ ├── Makefile.subdirs │ │ ├── generate_shared_object │ │ ├── message │ │ ├── pretty_compiler │ │ └── testlib │ ├── carmenwrapper │ │ ├── carmenwrapper.cpp │ │ └── carmenwrapper.h │ ├── docs │ │ ├── Instructions.txt │ │ ├── scanmatcher.tex │ │ └── userver.txt │ ├── gfs-carmen │ │ ├── Makefile │ │ └── gfs-carmen.cpp │ ├── grid │ │ ├── Makefile │ │ ├── graphmap.cpp │ │ └── map_test.cpp │ ├── gridfastslam │ │ ├── CMakeLists.txt │ │ ├── gfs2log.cpp │ │ ├── gfs2neff.cpp │ │ ├── gfs2rec.cpp │ │ ├── gfs2stat.cpp │ │ ├── gfs2stream.cpp │ │ ├── gfsreader.cpp │ │ ├── gfsreader.h │ │ ├── gridslamprocessor.cpp │ │ ├── gridslamprocessor_tree.cpp │ │ └── motionmodel.cpp │ ├── gui │ │ ├── CMakeLists.txt │ │ ├── gfs2img.cpp │ │ ├── gfs_logplayer.cpp │ │ ├── gfs_nogui.cpp │ │ ├── gfs_simplegui.cpp │ │ ├── gsp_thread.cpp │ │ ├── gsp_thread.h │ │ ├── qgraphpainter.cpp │ │ ├── qgraphpainter.h │ │ ├── qmappainter.cpp │ │ ├── qmappainter.h │ │ ├── qnavigatorwidget.cpp │ │ ├── qnavigatorwidget.h │ │ ├── qparticleviewer.cpp │ │ ├── qparticleviewer.h │ │ ├── qpixmapdumper.cpp │ │ ├── qpixmapdumper.h │ │ ├── qslamandnavwidget.cpp │ │ └── qslamandnavwidget.h │ ├── include │ │ └── gmapping │ │ │ ├── grid │ │ │ ├── accessstate.h │ │ │ ├── array2d.h │ │ │ ├── harray2d.h │ │ │ └── map.h │ │ │ ├── gridfastslam │ │ │ ├── gridslamprocessor.h │ │ │ ├── gridslamprocessor.hxx │ │ │ └── motionmodel.h │ │ │ ├── log │ │ │ ├── configuration.h │ │ │ └── sensorlog.h │ │ │ ├── particlefilter │ │ │ └── particlefilter.h │ │ │ ├── scanmatcher │ │ │ ├── icp.h │ │ │ ├── scanmatcher.h │ │ │ └── smmap.h │ │ │ ├── sensor │ │ │ ├── sensor_base │ │ │ │ ├── sensor.h │ │ │ │ └── sensorreading.h │ │ │ ├── sensor_odometry │ │ │ │ ├── odometryreading.h │ │ │ │ └── odometrysensor.h │ │ │ └── sensor_range │ │ │ │ ├── rangereading.h │ │ │ │ └── rangesensor.h │ │ │ └── utils │ │ │ ├── autoptr.h │ │ │ ├── commandline.h │ │ │ ├── gvalues.h │ │ │ ├── macro_params.h │ │ │ ├── point.h │ │ │ └── stat.h │ ├── ini │ │ ├── gfs-LMS-10cm.ini │ │ ├── gfs-LMS-20cm.ini │ │ ├── gfs-LMS-5cm.ini │ │ ├── gfs-PLS-10cm.ini │ │ └── gfs-PLS-5cm.ini │ ├── log │ │ ├── Makefile │ │ ├── carmenconfiguration.cpp │ │ ├── carmenconfiguration.h │ │ ├── configuration.cpp │ │ ├── log_plot.cpp │ │ ├── log_test.cpp │ │ ├── rdk2carmen.cpp │ │ ├── scanstudio2carmen.cpp │ │ ├── sensorlog.cpp │ │ ├── sensorstream.cpp │ │ └── sensorstream.h │ ├── package.xml │ ├── particlefilter │ │ ├── particlefilter.cpp │ │ ├── particlefilter_test.cpp │ │ ├── pf.h │ │ └── range_bearing.cpp │ ├── scanmatcher │ │ ├── CMakeLists.txt │ │ ├── correlationGrid.cpp │ │ ├── correlationGrid.h │ │ ├── eig3.cpp │ │ ├── eig3.h │ │ ├── gridlinetraversal.h │ │ ├── icptest.cpp │ │ ├── lumiles.h │ │ ├── scanmatch_test.cpp │ │ ├── scanmatcher.cpp │ │ ├── scanmatcher.new.cpp │ │ ├── scanmatcherprocessor.cpp │ │ ├── scanmatcherprocessor.h │ │ └── smmap.cpp │ ├── sensor │ │ ├── CMakeLists.txt │ │ ├── Makefile │ │ ├── sensor_base │ │ │ ├── CMakeLists.txt │ │ │ ├── sensor.cpp │ │ │ ├── sensoreading.h │ │ │ └── sensorreading.cpp │ │ ├── sensor_odometry │ │ │ ├── CMakeLists.txt │ │ │ ├── odometryreading.cpp │ │ │ └── odometrysensor.cpp │ │ └── sensor_range │ │ │ ├── CMakeLists.txt │ │ │ ├── rangereading.cpp │ │ │ └── rangesensor.cpp │ └── utils │ │ ├── CMakeLists.txt │ │ ├── autoptr_test.cpp │ │ ├── datasmoother.h │ │ ├── dmatrix.h │ │ ├── movement.cpp │ │ ├── movement.h │ │ ├── optimizer.h │ │ ├── orientedboundingbox.h │ │ ├── orientedboundingbox.hxx │ │ ├── printmemusage.cpp │ │ ├── printmemusage.h │ │ ├── printpgm.h │ │ ├── stat.cpp │ │ └── stat_test.cpp ├── slam_gmapping │ ├── .gitignore │ ├── .travis.yml │ ├── gmapping │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── gmapping_sim.launch │ │ ├── nodelet_plugins.xml │ │ ├── package.xml │ │ ├── src │ │ │ ├── main.cpp │ │ │ ├── nodelet.cpp │ │ │ ├── replay.cpp │ │ │ ├── slam_gmapping.cpp │ │ │ ├── slam_gmapping.h │ │ │ └── tftest.cpp │ │ └── test │ │ │ ├── basic_localization_laser_different_beamcount.test │ │ │ ├── basic_localization_stage.launch │ │ │ ├── basic_localization_stage_replay.launch │ │ │ ├── basic_localization_stage_replay2.launch │ │ │ ├── basic_localization_symmetry.launch │ │ │ ├── basic_localization_upside_down.launch │ │ │ ├── rtest.cpp │ │ │ └── test_map.py │ └── slam_gmapping │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml ├── 第五课-基于滤波的SLAM算法.pdf └── 第四课-帧间匹配算法.pdf ├── laser_undistortion ├── CMakeLists.txt ├── README.assets │ ├── laser_undistortion.jpg │ └── laser_undistortion.png ├── README.md ├── bag │ └── laser.bag ├── launch │ └── LaserUndistortion.launch ├── package.xml ├── src │ └── LidarMotionUndistortion.cpp └── 第三课-激光雷达及畸变去除.pdf ├── ls_slam ├── CMakeLists.txt ├── README.assets │ ├── GN.png │ ├── GN1.png │ ├── GN2.png │ ├── error.png │ ├── xianxinghua.png │ └── ykb.png ├── README.md ├── data │ ├── intel-e.dat │ ├── intel-v.dat │ ├── killian-e.dat │ ├── killian-v.dat │ ├── test_quadrat-e.dat │ └── test_quadrat-v.dat ├── include │ └── ls_slam │ │ ├── gaussian_newton.h │ │ └── readfile.h ├── package.xml ├── src │ ├── gaussian_newton.cpp │ ├── main.cpp │ └── readfile.cpp └── 第六课-图优化SLAM算法.pdf ├── occupany_mapping ├── CMakeLists.txt ├── README.assets │ ├── occupany.jpg │ └── occupany.png ├── README.md ├── data │ ├── pose.txt │ ├── ranges.txt │ └── scanAngles.txt ├── include │ └── occupany_mapping │ │ ├── occupany_mapping.h │ │ └── readfile.h ├── package.xml ├── src │ ├── occupany_mapping.cpp │ └── readfile.cpp ├── 第七课-已知定位的构图算法.pdf └── 第八课-3D激光SLAM介绍.pdf ├── 激光SLAM方案对比.png ├── 激光SLAM知识框架.png └── 激光SLAM算法知识.png /README.md: -------------------------------------------------------------------------------- 1 | # laser-slam 2 | 深蓝学院 第一期激光slam 作业完整答案解析 3 | 4 | 1. 在ros工作空间的src文件夹下执行 git clone 本项目地址 5 | 2. 共有5个工程，具体分析看每个工程的pdf课件和README文件。 6 | 7 | - [x] calib_odom : 实现了一个基于csm库的icp函数的激光里程计，并用线性最小二乘对里程计做了矫正。 8 | - [x] laser_undistortion : 实现了利用里程计线性插值的数据，去除激光雷达在运动过程中产生的畸变。依赖于champion_nav_msgs包，一种自定义的激光雷达数据消息类型。如果编译报错，可以先编译champion_nav_msgs包，再编译本身。 9 | - [x] gmapping : 对gmapping的流程进行了分析。 10 | - [x] ls_slam : 实现了一个二维位姿的图优化，手写了一个BA。 11 | - [x] occupany_mapping : 实现了基于已知定位的覆盖栅格建图算法。 12 | - [x] cartographer_annotation : cartographer算法注解。 13 | 14 | ![激光SLAM算法知识](./激光SLAM算法知识.png) 15 | ![激光SLAM方案对比](./激光SLAM方案对比.png) -------------------------------------------------------------------------------- /calib_odom/README.assets/1554776817122.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/calib_odom/README.assets/1554776817122.png -------------------------------------------------------------------------------- /calib_odom/README.assets/1554777572171.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/calib_odom/README.assets/1554777572171.png -------------------------------------------------------------------------------- /calib_odom/README.assets/1554777724377.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/calib_odom/README.assets/1554777724377.png -------------------------------------------------------------------------------- /calib_odom/include/calib_odom/Odom_Calib.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ODOM_CALIB_H 2 | #define ODOM_CALIB_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | class OdomCalib 9 | { 10 | 11 | public: 12 | OdomCalib(){ 13 | data_len = 0; 14 | now_len = 0; 15 | } 16 | 17 | // virtual ~OdomCalib(); 18 | void Set_data_len(int len); 19 | bool Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan); 20 | Eigen::Matrix3d Solve(); 21 | bool is_full(); 22 | void set_data_zero(); 23 | 24 | private: 25 | Eigen::MatrixXd A; 26 | Eigen::VectorXd b; 27 | int data_len,now_len; 28 | 29 | }; 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /calib_odom/launch/odomCalib.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /calib_odom/第一课（公开课）PPT.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/calib_odom/第一课（公开课）PPT.pdf -------------------------------------------------------------------------------- /calib_odom/第二课-里程计.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/calib_odom/第二课-里程计.pdf -------------------------------------------------------------------------------- /cartographer_annotations/.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | 35 | # 忽略*.o和*.a文件 36 | *.[oa] 37 | *.[out] 38 | *build 39 | build #过滤整个build文件夹 40 | devel 41 | *.class #过滤所有.class文件 42 | .idea 43 | cartographer 44 | cartographer_ros 45 | ceres-solver 46 | Eigen 47 | src 48 | .zip 49 | orig 50 | backup 51 | pictures 52 | html 53 | note -------------------------------------------------------------------------------- /cartographer_annotations/README.md: -------------------------------------------------------------------------------- 1 | # cartographer 2 | 关于谷歌slam地图库 [cartographer](https://github.com/googlecartographer/cartographer)的源码注释。(只含源码) 3 | 4 | 5 | 6 | 博客文章： 7 | 8 | 9 | 10 | * [知乎](https://zhuanlan.zhihu.com/learnmoreonce) (简略) 11 | 12 | * [CSDN](http://blog.csdn.net/learnmoreonce) (详细) 13 | 14 | 15 | 源码分析的markdown文件可在 [这里]( https://github.com/slam4code/SLAM/tree/master/cartographer-%E6%BA%90%E7%A0%81%E5%88%86%E6%9E%90 ) 16 | 查找。 17 | -------------------------------------------------------------------------------- /cartographer_annotations/common/ceres_solver_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/common/ceres_solver_options.h" 18 | 19 | namespace cartographer { 20 | namespace common { 21 | 22 | /* 23 | 根据lua文件设置 24 | */ 25 | proto::CeresSolverOptions CreateCeresSolverOptionsProto( 26 | common::LuaParameterDictionary* parameter_dictionary) 27 | 28 | { 29 | proto::CeresSolverOptions proto; 30 | proto.set_use_nonmonotonic_steps( 31 | parameter_dictionary->GetBool("use_nonmonotonic_steps"));//是否设置使用nonmonotonic 32 | proto.set_max_num_iterations( 33 | parameter_dictionary->GetNonNegativeInt("max_num_iterations"));//迭代次数 34 | proto.set_num_threads(parameter_dictionary->GetNonNegativeInt("num_threads"));//线程数 35 | CHECK_GT(proto.max_num_iterations(), 0); 36 | CHECK_GT(proto.num_threads(), 0); 37 | return proto; 38 | } 39 | /* 40 | 根据proto文件设置 41 | */ 42 | ceres::Solver::Options CreateCeresSolverOptions( 43 | const proto::CeresSolverOptions& proto) 44 | 45 | { 46 | ceres::Solver::Options options;//建立临时对象 47 | options.use_nonmonotonic_steps = proto.use_nonmonotonic_steps();//为临时对象赋值 48 | options.max_num_iterations = proto.max_num_iterations(); //赋值 49 | options.num_threads = proto.num_threads(); //赋值 50 | return options; 51 | } 52 | 53 | } // namespace common 54 | } // namespace cartographer 55 | -------------------------------------------------------------------------------- /cartographer_annotations/common/ceres_solver_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 18 | #define CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/common/proto/ceres_solver_options.pb.h" 22 | #include "ceres/ceres.h" 23 | 24 | namespace cartographer { 25 | namespace common { 26 | 27 | /* 28 | 配置加载类, 根据lua设置 CeresSolver的选项 29 | 30 | */ 31 | proto::CeresSolverOptions CreateCeresSolverOptionsProto( 32 | common::LuaParameterDictionary* parameter_dictionary); 33 | 34 | //根据proto设置 CeresSolver的选项 35 | ceres::Solver::Options CreateCeresSolverOptions( 36 | const proto::CeresSolverOptions& proto); 37 | 38 | } // namespace common 39 | } // namespace cartographer 40 | 41 | #endif // CARTOGRAPHER_COMMON_CERES_SOLVER_OPTIONS_H_ 42 | -------------------------------------------------------------------------------- /cartographer_annotations/common/config.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CONFIG_H_ 18 | #define CARTOGRAPHER_COMMON_CONFIG_H_ 19 | 20 | namespace cartographer { 21 | namespace common { 22 | 23 | constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径 24 | ""; 25 | constexpr char kSourceDirectory[] = "./"; 26 | 27 | } // namespace common 28 | } // namespace cartographer 29 | 30 | #endif // CARTOGRAPHER_COMMON_CONFIG_H_ 31 | -------------------------------------------------------------------------------- /cartographer_annotations/common/config.h.cmake: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_CONFIG_H_ 18 | #define CARTOGRAPHER_COMMON_CONFIG_H_ 19 | 20 | namespace cartographer { 21 | namespace common { 22 | 23 | constexpr char kConfigurationFilesDirectory[] = //设置配置文件的路径 24 | "@CARTOGRAPHER_CONFIGURATION_FILES_DIRECTORY@"; 25 | constexpr char kSourceDirectory[] = "@PROJECT_SOURCE_DIR@"; 26 | 27 | } // namespace common 28 | } // namespace cartographer 29 | 30 | #endif // CARTOGRAPHER_COMMON_CONFIG_H_ 31 | -------------------------------------------------------------------------------- /cartographer_annotations/common/fixed_ratio_sampler.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/common/fixed_ratio_sampler.h" 4 | 5 | namespace cartographer { 6 | namespace common { 7 | 8 | FixedRatioSampler::FixedRatioSampler(const double ratio) : ratio_(ratio) {} 9 | 10 | FixedRatioSampler::~FixedRatioSampler() {} 11 | 12 | bool FixedRatioSampler::Pulse() { 13 | 14 | //如果当前采样率 samples/ pulses低于固定采样率,此次事件计入采样. 15 | ++num_pulses_; 16 | if (static_cast(num_samples_) / num_pulses_ < ratio_) { 17 | ++num_samples_; 18 | return true; 19 | } 20 | return false; 21 | } 22 | 23 | string FixedRatioSampler::DebugString() { 24 | return std::to_string(num_samples_) + " (" + 25 | std::to_string(100. * num_samples_ / num_pulses_) + "%)"; 26 | } 27 | 28 | } // namespace common 29 | } // namespace cartographer 30 | -------------------------------------------------------------------------------- /cartographer_annotations/common/fixed_ratio_sampler.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 3 | #define CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 4 | 5 | #include 6 | 7 | #include "cartographer/common/port.h" 8 | 9 | namespace cartographer { 10 | namespace common { 11 | 12 | /* 13 | FixedRatioSampler是频率固定的采样器类，目的是从数据流中均匀的按照固定频率采样数据 14 | FixedRatioSampler不可拷贝,不可赋值. 15 | 成员函数提供2种操作: 16 | Pulse()产生一个事件pulses,并且:如果计入采样samples，返回true 17 | DebugString():以string形式输出采样率 18 | 19 | */ 20 | // Signals when a sample should be taken from a stream of data to select a 21 | // uniformly distributed fraction of the data. 22 | class FixedRatioSampler { 23 | public: 24 | explicit FixedRatioSampler(double ratio); 25 | ~FixedRatioSampler(); 26 | 27 | FixedRatioSampler(const FixedRatioSampler&) = delete; 28 | FixedRatioSampler& operator=(const FixedRatioSampler&) = delete; 29 | 30 | // Returns true if this pulse should result in an sample. 31 | bool Pulse(); 32 | 33 | // Returns a debug string describing the current ratio of samples to pulses. 34 | string DebugString(); 35 | 36 | private: 37 | // Sampling occurs if the proportion of samples to pulses drops below this 38 | // number. 39 | const double ratio_; 40 | 41 | int64 num_pulses_ = 0; //产生的脉冲次数 42 | int64 num_samples_ = 0;//记录的采样次数 43 | }; 44 | 45 | } // namespace common 46 | } // namespace cartographer 47 | 48 | #endif // CARTOGRAPHER_COMMON_FIXED_RATIO_SAMPLER_H_ 49 | -------------------------------------------------------------------------------- /cartographer_annotations/common/fixed_ratio_sampler_test.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/common/fixed_ratio_sampler.h" 4 | 5 | #include "gtest/gtest.h" 6 | 7 | namespace cartographer { 8 | namespace common { 9 | namespace { 10 | 11 | TEST(FixedRatioSamplerTest, AlwaysTrue) { 12 | FixedRatioSampler fixed_ratio_sampler(1.);//固定采样率是1hz，每次Pulse都采集samples 13 | for (int i = 0; i < 100; ++i) { 14 | EXPECT_TRUE(fixed_ratio_sampler.Pulse()); 15 | } 16 | } 17 | 18 | TEST(FixedRatioSamplerTest, AlwaysFalse) { 19 | FixedRatioSampler fixed_ratio_sampler(0.);//0 hz，不采集samples 20 | for (int i = 0; i < 100; ++i) { 21 | EXPECT_FALSE(fixed_ratio_sampler.Pulse()); 22 | } 23 | } 24 | 25 | TEST(FixedRatioSamplerTest, SometimesTrue) { 26 | FixedRatioSampler fixed_ratio_sampler(0.5); //0.5hz 27 | for (int i = 0; i < 100; ++i) { 28 | EXPECT_EQ(i % 2 == 0, fixed_ratio_sampler.Pulse());//每2次Pulse采集一次samples 29 | } 30 | } 31 | 32 | TEST(FixedRatioSamplerTest, FirstPulseIsTrue) { 33 | // Choose a very very small positive number for the ratio. 34 | FixedRatioSampler fixed_ratio_sampler(1e-20); //0.000000...001hz 35 | EXPECT_TRUE(fixed_ratio_sampler.Pulse()); 36 | for (int i = 0; i < 100; ++i) { 37 | EXPECT_FALSE(fixed_ratio_sampler.Pulse()); //每100000000次Pulse采集一次samples 38 | } 39 | } 40 | 41 | } // namespace 42 | } // namespace common 43 | } // namespace cartographer 44 | -------------------------------------------------------------------------------- /cartographer_annotations/common/histogram.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef CARTOGRAPHER_COMMON_HISTOGRAM_H_ 4 | #define CARTOGRAPHER_COMMON_HISTOGRAM_H_ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include "cartographer/common/port.h" 11 | 12 | namespace cartographer { 13 | namespace common { 14 | /* 15 | Histogram直方图类, 16 | 提供2个操作: 17 | 1，Add()添加元素 18 | 2，ToString(buckets )以字符串的形式输出buckets个直方图信息,bin大小是篮子个数. 19 | 20 | Histogram只有一个数据成员，用vector表示 21 | 22 | */ 23 | class Histogram { 24 | public: 25 | void Add(float value); //添加value,可乱序 26 | string ToString(int buckets) const;//分为几块 27 | 28 | private: 29 | std::vector values_; 30 | }; 31 | 32 | } // namespace common 33 | } // namespace cartographer 34 | 35 | #endif // CARTOGRAPHER_COMMON_HISTOGRAM_H_ 36 | 37 | 38 | 39 | /*测试： 40 | 若bin为2:[2,1,2,4,5,5]: 41 | Count: 6 Min: 1.000000 Max: 5.000000 Mean: 3.166667 42 | [1.000000, 3.000000) ########## Count: 3 (50.000000%) Total: 3 (50.000000%) 43 | [3.000000, 5.000000] ########## Count: 3 (50.000000%) Total: 6 (100.000000%) 44 | 45 | 46 | 若bin为3:[2,1,2,4,5,5,6,7,8]: 47 | Count: 10 Min: 1.000000 Max: 8.000000 Mean: 4.500000 48 | [1.000000, 3.333333) ###### Count: 3 (30.000000%) Total: 3 (30.000000%) 49 | [3.333333, 5.666667) ######## Count: 4 (40.000000%) Total: 7 (70.000000%) 50 | [5.666667, 8.000000] ###### Count: 3 (30.000000%) Total: 10 (100.000000%) 51 | 52 | */ -------------------------------------------------------------------------------- /cartographer_annotations/common/lua.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_COMMON_LUA_H_ 18 | #define CARTOGRAPHER_COMMON_LUA_H_ 19 | 20 | #include 21 | 22 | #endif // CARTOGRAPHER_COMMON_LUA_H_ 23 | -------------------------------------------------------------------------------- /cartographer_annotations/common/make_unique.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 4 | #define CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace cartographer { 12 | namespace common { 13 | 14 | /* 15 | make_unique.h在不支持c++14的环境下实现 std::make_unique的功能 16 | 实现细节:完美转发和移动语义 17 | 18 | */ 19 | // Implementation of c++14's std::make_unique, taken from 20 | // https://isocpp.org/files/papers/N3656.txt 21 | template 22 | struct _Unique_if { 23 | typedef std::unique_ptr _Single_object; 24 | }; 25 | 26 | template 27 | struct _Unique_if { 28 | typedef std::unique_ptr _Unknown_bound; //不支持数组(不定长) 29 | }; 30 | 31 | template 32 | struct _Unique_if { //不支持数组(定长) 33 | typedef void _Known_bound; 34 | }; 35 | 36 | template 37 | typename _Unique_if::_Single_object make_unique(Args&&... args) { 38 | return std::unique_ptr(new T(std::forward(args)...)); //完美转发参数 39 | } 40 | 41 | template 42 | typename _Unique_if::_Unknown_bound make_unique(size_t n) { 43 | typedef typename std::remove_extent::type U; 44 | return std::unique_ptr(new U[n]()); 45 | } 46 | 47 | template 48 | typename _Unique_if::_Known_bound make_unique(Args&&...) = delete;//不能使用定长数组 49 | 50 | } // namespace common 51 | } // namespace cartographer 52 | 53 | #endif // CARTOGRAPHER_COMMON_MAKE_UNIQUE_H_ 54 | 55 | /* 56 | 完美转发: 57 | 完美的传递函数参数而不修改参数类型，左值依然是左值，右值依然是右值。 58 | */ -------------------------------------------------------------------------------- /cartographer_annotations/common/make_unique_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include "cartographer/common/make_unique.h" 5 | #include "gtest/gtest.h" 6 | //using namespace std; 7 | using std::cout; 8 | using std::cin; 9 | using std::endl; 10 | using std::string; 11 | namespace cartographer { 12 | namespace common { 13 | TEST(MAKE_UNIQUE ,make_unique) { 14 | cout << *make_unique() << endl; 15 | cout << *make_unique(1729) << endl; 16 | cout << "\"" << *make_unique() << "\"" << endl; 17 | cout << "\"" << *make_unique("meow") << "\"" << endl; 18 | cout << "\"" << *make_unique(6, 'z') << "\"" << endl; 19 | 20 | auto up = make_unique(5); 21 | 22 | for (int i = 0; i < 5; ++i) { 23 | cout << up[i] << " "; 24 | } 25 | 26 | cout << endl; 27 | 28 | #if defined(ERROR1) 29 | auto up1 = make_unique("error"); 30 | #elif defined(ERROR2) 31 | auto up2 = make_unique(10, 20, 30, 40); 32 | #elif defined(ERROR3) 33 | auto up3 = make_unique(); 34 | #elif defined(ERROR4) 35 | auto up4 = make_unique(11, 22, 33, 44, 55); 36 | #endif 37 | } 38 | 39 | /* 40 | 输出: 41 | 42 | 0 43 | 1729 44 | "" 45 | "meow" 46 | "zzzzzz" 47 | 0 0 0 0 0 48 | 49 | 50 | */ 51 | } 52 | } -------------------------------------------------------------------------------- /cartographer_annotations/common/math_test.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/common/math.h" 4 | 5 | #include "gtest/gtest.h" 6 | 7 | namespace cartographer { 8 | namespace common { 9 | namespace { 10 | 11 | TEST(MathTest, testPower) { 12 | EXPECT_EQ(0., Power(0, 42));// 0的42次方==0 13 | EXPECT_EQ(1., Power(0, 0)); //0^0 ==0 14 | EXPECT_EQ(1., Power(1, 0)); // 1^0 ==0 15 | EXPECT_EQ(1., Power(1, 42));//1^42==1 16 | EXPECT_EQ(4., Power(2, 2)); //2^2==4 17 | } 18 | 19 | TEST(MathTest, testPow2) { 20 | EXPECT_EQ(0., Pow2(0)); //0^2==0 21 | EXPECT_EQ(1., Pow2(1)); //1^2==1 22 | EXPECT_EQ(4., Pow2(2)); //2^2==4 23 | EXPECT_EQ(49., Pow2(7));//7^2==49 24 | } 25 | 26 | TEST(MathTest, testDeg2rad) { 27 | EXPECT_NEAR(M_PI, DegToRad(180.), 1e-9); // 180° ==pi 28 | EXPECT_NEAR(2. * M_PI, DegToRad(360. - 1e-9), 1e-6);//360° ==2pi 29 | } 30 | 31 | TEST(MathTest, testRad2deg) { 32 | EXPECT_NEAR(180., RadToDeg(M_PI), 1e-9); //pi ==180° 33 | EXPECT_NEAR(360., RadToDeg(2. * M_PI - 1e-9), 1e-6);//2pi ==360° 34 | } 35 | 36 | TEST(MathTest, testNormalizeAngleDifference) { 37 | EXPECT_NEAR(0., NormalizeAngleDifference(0.), 1e-9); //0==0 38 | EXPECT_NEAR(M_PI, NormalizeAngleDifference(M_PI), 1e-9); //pi==oi 39 | EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-M_PI), 1e-9); //-pi==-pi 40 | EXPECT_NEAR(0., NormalizeAngleDifference(2. * M_PI), 1e-9); //2pi==0 41 | EXPECT_NEAR(M_PI, NormalizeAngleDifference(5. * M_PI), 1e-9); //5pi==pi 42 | EXPECT_NEAR(-M_PI, NormalizeAngleDifference(-5. * M_PI), 1e-9);//-5pi=-pi 43 | } 44 | 45 | } // namespace 46 | } // namespace common 47 | } // namespace cartographer 48 | -------------------------------------------------------------------------------- /cartographer_annotations/common/port_le_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | /* 5 | c++11 已经支持back_inserter。 6 | */ 7 | //#include 8 | 9 | using std::cout; 10 | using std::endl; 11 | int main(int argc, char *argv[]) 12 | { 13 | std::vector vec; 14 | for (int i = 0 ; i < 3; ++i) 15 | vec.push_back(i); 16 | 17 | std::vector ::iterator vIter; 18 | std::cout << "The initial vector vec is: ( "; 19 | for (vIter = vec.begin(); vIter != vec.end(); vIter++) 20 | std::cout << *vIter << " "; 21 | std::cout << ")." << std::endl; 22 | 23 | // Insertions can be done with template function 24 | std::back_insert_iterator > backiter(vec); 25 | *backiter = 30; 26 | backiter++; 27 | *backiter = 40; 28 | 29 | // Alternatively, insertions can be done with the 30 | // back_insert_iterator member function 31 | std::back_inserter(vec) = 500;//从vector后插入 32 | std::back_inserter(vec) = 600; 33 | 34 | std::cout << "After the insertions, the vector vec is: ( "; 35 | for (vIter = vec.begin(); vIter != vec.end(); vIter++) 36 | std::cout << *vIter << " "; 37 | std::cout << ")." << std::endl<<"----\n\n"; 38 | 39 | 40 | std::vector firstvector, secondvector; 41 | for (int i=1; i<=5; i++) 42 | { 43 | firstvector.push_back(i); 44 | secondvector.push_back(i*10); 45 | } 46 | 47 | // 在firstvector向量后面加入secondvector的值. 48 | std::copy(secondvector.begin(), secondvector.end(), std::back_inserter(firstvector)); 49 | 50 | std::vector::iterator it; 51 | for ( it = firstvector.begin(); it!= firstvector.end(); ++it ) 52 | std::cout << *it << " "; 53 | std::cout << std::endl; 54 | 55 | return 0; 56 | } -------------------------------------------------------------------------------- /cartographer_annotations/common/proto/ceres_solver_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.common.proto; 18 | 19 | message CeresSolverOptions { 20 | // Configure the Ceres solver. See the Ceres documentation for more 21 | // information: https://code.google.com/p/ceres-solver/ 22 | optional bool use_nonmonotonic_steps = 1; 23 | optional int32 max_num_iterations = 2; 24 | optional int32 num_threads = 3; 25 | } 26 | -------------------------------------------------------------------------------- /cartographer_annotations/common/rate_timer_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/common/rate_timer.h" 3 | 4 | #include "gtest/gtest.h" 5 | 6 | namespace cartographer { 7 | namespace common { 8 | namespace { 9 | 10 | TEST(RateTimerTest, ComputeRate) { 11 | RateTimer<> rate_timer(common::FromSeconds(1.));//默认模板参数是steady_clock,设置时间段是1s 12 | common::Time time = common::FromUniversal(42); // 时间点,42us 13 | for (int i = 0; i < 100; ++i) { //每隔0.1s产生一次事件,产生100次 14 | rate_timer.Pulse(time); 15 | time += common::FromSeconds(0.1); 16 | } 17 | EXPECT_NEAR(10., rate_timer.ComputeRate(), 1e-3);//频率应该等于10次每s 18 | } 19 | 20 | struct SimulatedClock { /*模拟的时间clock类*/ 21 | using rep = std::chrono::steady_clock::rep; 22 | using period = std::chrono::steady_clock::period; 23 | using duration = std::chrono::steady_clock::duration; 24 | using time_point = std::chrono::steady_clock::time_point; 25 | static constexpr bool is_steady = true; 26 | 27 | static time_point time; 28 | static time_point now() noexcept { return time; } 29 | }; 30 | 31 | SimulatedClock::time_point SimulatedClock::time; 32 | 33 | TEST(RateTimerTest, ComputeWallTimeRateRatio) { 34 | common::Time time = common::FromUniversal(42); //时间点 42us 35 | RateTimer rate_timer(common::FromSeconds(1.)); //时间段是1s 36 | for (int i = 0; i < 100; ++i) { //100次事件,0.1s一次. 37 | rate_timer.Pulse(time); 38 | time += common::FromSeconds(0.1); //加0.1s,代表事件发生的时间(调用了Pulse) 39 | SimulatedClock::time += //模仿的时间,加0.05s,代表流失了0.05秒. 40 | std::chrono::duration_cast( 41 | std::chrono::duration(0.05)); 42 | } 43 | EXPECT_NEAR(2., rate_timer.ComputeWallTimeRateRatio(), 1e-3); //0.1/0.05==2 44 | } 45 | 46 | } // namespace 47 | } // namespace common 48 | } // namespace cartographer 49 | -------------------------------------------------------------------------------- /cartographer_annotations/common/thread_pool.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_COMMON_THREAD_POOL_H_ 3 | #define CARTOGRAPHER_COMMON_THREAD_POOL_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "cartographer/common/mutex.h" 11 | 12 | namespace cartographer { 13 | namespace common { 14 | 15 | /* 16 | ThreadPool 是对c++11 thread的封装. 17 | ThreadPool是线程数量固定的线程池，不可拷贝 和复制. 18 | 19 | 1，构造函数ThreadPool(int num_threads) 初始化一个线程数量固定的线程池。 20 | 2，Schedule(std::function work_item)添加想要ThreadPool执行的函数, 21 | std::thread会在线程后台依次排队执行相关函数. 22 | 3，数据成员pool_是具体的线程，work_queue_是待执行的函数队列。 23 | 24 | 25 | */ 26 | // A fixed number of threads working on a work queue of work items. Adding a 27 | // new work item does not block, and will be executed by a background thread 28 | // eventually. The queue must be empty before calling the destructor. The thread 29 | // pool will then wait for the currently executing work items to finish and then 30 | // destroy the threads. 31 | class ThreadPool { 32 | public: 33 | explicit ThreadPool(int num_threads); 34 | ~ThreadPool(); 35 | 36 | ThreadPool(const ThreadPool&) = delete; 37 | ThreadPool& operator=(const ThreadPool&) = delete; 38 | 39 | void Schedule(std::function work_item); 40 | 41 | private: 42 | void DoWork(); 43 | 44 | Mutex mutex_; //互斥锁，保证线程安全 45 | 46 | //running_只是一个监视哨,只有线程池在running_状态时,才能往work_queue_加入函数. 47 | bool running_ GUARDED_BY(mutex_) = true; 48 | std::vector pool_ GUARDED_BY(mutex_); 49 | std::deque> work_queue_ GUARDED_BY(mutex_); 50 | }; 51 | 52 | } // namespace common 53 | } // namespace cartographer 54 | 55 | #endif // CARTOGRAPHER_COMMON_THREAD_POOL_H_ 56 | -------------------------------------------------------------------------------- /cartographer_annotations/common/time.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/common/time.h" 4 | 5 | #include 6 | 7 | namespace cartographer { 8 | namespace common { 9 | 10 | //duration_cast是c++ 11的时间显式转换函数. 11 | Duration FromSeconds(const double seconds) { 12 | return std::chrono::duration_cast( 13 | //将double类型的秒数转化为duration对象 14 | std::chrono::duration(seconds)); 15 | } 16 | 17 | double ToSeconds(const Duration duration) { 18 | //反转化,count()返回时钟周期数,ticks 19 | return std::chrono::duration_cast>(duration) 20 | .count(); 21 | } 22 | 23 | //先构造一个临时duration对象,再将其转化为time_point对象 24 | //Duration(ticks)调用的是UniversalTimeScaleClock的构造函数 25 | Time FromUniversal(const int64 ticks) { return Time(Duration(ticks)); } 26 | 27 | 28 | //count()返回time_point自epoch以来的时钟周期数 29 | int64 ToUniversal(const Time time) { return time.time_since_epoch().count(); } 30 | 31 | //先将Time转化为 int64 , 再转为字符串形式 32 | std::ostream& operator<<(std::ostream& os, const Time time) { 33 | os << std::to_string(ToUniversal(time)); 34 | return os; 35 | } 36 | 37 | //mill是ms,micro是us,先将毫秒转为以毫秒计时的duration对象,再转化为以微妙计. 38 | common::Duration FromMilliseconds(const int64 milliseconds) { 39 | return std::chrono::duration_cast( 40 | std::chrono::milliseconds(milliseconds)); 41 | } 42 | 43 | } // namespace common 44 | } // namespace cartographer 45 | -------------------------------------------------------------------------------- /cartographer_annotations/io/cairo_types.h: -------------------------------------------------------------------------------- 1 | #ifndef CARTOGRAPHER_IO_CAIRO_TYPES_H_ 2 | #define CARTOGRAPHER_IO_CAIRO_TYPES_H_ 3 | 4 | #include 5 | 6 | #include "cairo/cairo.h" 7 | /* 8 | Cairo是一个2D图形库，支持多种输出设备。 9 | cairo 是一个免费的矢量绘图软件库，它可以绘制多种输出格式。 10 | 11 | https://www.ibm.com/developerworks/cn/linux/l-cairo/ 12 | http://liyanrui.is-programmer.com/2009/3/18/cairo-tutorial-05.7742.html 13 | http://blog.csdn.net/turingo/article/details/8131057 14 | 15 | */ 16 | namespace cartographer { 17 | namespace io { 18 | namespace cairo { 19 | 20 | //智能指针管理对象生命期.在unique_ptr销毁时释放指向的对象资源. 21 | 22 | 23 | //对象类型是cairo_surface_t,自定义删除器deleter是函数指针. 24 | // std::unique_ptr for Cairo surfaces. The surface is destroyed when the 25 | // std::unique_ptr is reset or destroyed. 26 | using UniqueSurfacePtr = 27 | std::unique_ptr; 28 | 29 | //对象类型是cairo_t,自定义删除器deleter是函数指针. 30 | // std::unique_ptr for Cairo contexts. The context is destroyed when the 31 | // std::unique_ptr is reset or destroyed. 32 | using UniqueContextPtr = std::unique_ptr; 33 | 34 | //对象类型是cairo_path_t,自定义删除器deleter是函数指针. 35 | // std::unique_ptr for Cairo paths. The path is destroyed when the 36 | // std::unique_ptr is reset or destroyed. 37 | using UniquePathPtr = std::unique_ptr; 38 | 39 | } // namespace cairo 40 | } // namespace io 41 | } // namespace cartographer 42 | 43 | #endif // CARTOGRAPHER_IO_CAIRO_TYPES_H_ 44 | -------------------------------------------------------------------------------- /cartographer_annotations/io/coloring_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ 3 | #define CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ 4 | 5 | #include 6 | 7 | #include "cartographer/common/lua_parameter_dictionary.h" 8 | #include "cartographer/io/points_batch.h" 9 | #include "cartographer/io/points_processor.h" 10 | 11 | namespace cartographer { 12 | namespace io { 13 | /* 14 | { 15 | action = "color_points", 16 | frame_id = "horizontal_laser_link", 17 | color = { 255., 0., 0. }, 18 | }, 19 | { 20 | action = "color_points", 21 | frame_id = "vertical_laser_link", 22 | color = { 0., 255., 0. }, 23 | }, 24 | 25 | ColoringPointsProcessor是PointsProcessor的第六子类(6). 26 | 功能: 用固定的Color填充PointsBatch的Color分量。 27 | 28 | 数据成员: 29 | 1),color_：rgb值 30 | 2),frame_id_:只有相同的id才填充Color，color一般为[255,0,0],[0,255,0] 31 | 2),next_下一阶段的PointsProcessor. 32 | 33 | */ 34 | 35 | // Colors points with a fixed color by frame_id. 36 | class ColoringPointsProcessor : public PointsProcessor { 37 | public: 38 | constexpr static const char* kConfigurationFileActionName = "color_points"; 39 | 40 | ColoringPointsProcessor(const Color& color, const string& frame_id, 41 | PointsProcessor* next); 42 | 43 | static std::unique_ptr FromDictionary( 44 | common::LuaParameterDictionary* dictionary, PointsProcessor* next); 45 | 46 | ~ColoringPointsProcessor() override{}; 47 | 48 | ColoringPointsProcessor(const ColoringPointsProcessor&) = delete; 49 | ColoringPointsProcessor& operator=(const ColoringPointsProcessor&) = delete; 50 | 51 | void Process(std::unique_ptr batch) override; 52 | FlushResult Flush() override; 53 | 54 | private: 55 | const Color color_; 56 | const string frame_id_; 57 | PointsProcessor* const next_; 58 | }; 59 | 60 | } // namespace io 61 | } // namespace cartographer 62 | 63 | #endif // CARTOGRAPHER_IO_COLORING_POINTS_PROCESSOR_H_ 64 | -------------------------------------------------------------------------------- /cartographer_annotations/io/counting_points_processor.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/io/counting_points_processor.h" 3 | 4 | #include "cartographer/common/make_unique.h" 5 | #include "glog/logging.h" 6 | 7 | namespace cartographer { 8 | namespace io { 9 | 10 | //初始化是num是0 11 | CountingPointsProcessor::CountingPointsProcessor(PointsProcessor* next) 12 | : num_points_(0), next_(next) {} 13 | 14 | //与其他FromDictionary()函数统一写法 15 | std::unique_ptr 16 | CountingPointsProcessor::FromDictionary( 17 | common::LuaParameterDictionary* const dictionary, 18 | PointsProcessor* const next) { 19 | return common::make_unique(next); 20 | } 21 | 22 | //不处理points,而是将num_points_加上batch.size(),，即统计点云数据。然后直接流水线到下一PointsProcessor 23 | void CountingPointsProcessor::Process(std::unique_ptr batch) { 24 | num_points_ += batch->points.size();//相加 25 | next_->Process(std::move(batch)); 26 | } 27 | 28 | //依据下一阶段的状态重置本阶段的状态。 29 | PointsProcessor::FlushResult CountingPointsProcessor::Flush() { 30 | switch (next_->Flush()) { 31 | case FlushResult::kFinished: 32 | LOG(INFO) << "Processed " << num_points_ << " and finishing."; 33 | return FlushResult::kFinished; 34 | 35 | case FlushResult::kRestartStream: 36 | LOG(INFO) << "Processed " << num_points_ << " and restarting stream."; 37 | num_points_ = 0; //重启,则置为0 38 | return FlushResult::kRestartStream; 39 | } 40 | LOG(FATAL); 41 | } 42 | 43 | } // namespace io 44 | } // namespace cartographer 45 | -------------------------------------------------------------------------------- /cartographer_annotations/io/counting_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ 3 | #define CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ 4 | 5 | #include "cartographer/common/lua_parameter_dictionary.h" 6 | #include "cartographer/io/points_processor.h" 7 | 8 | namespace cartographer { 9 | namespace io { 10 | 11 | /* 12 | 13 | PointsProcessor的第二个子类(2). 14 | 功能:记录有多少 point被输出处理 15 | 16 | 数据成员: 17 | 1),num_points_记录points数量 18 | 2),next_下一阶段的PointsProcessor. 19 | 20 | */ 21 | // Passes through points, but keeps track of how many points it saw and output 22 | // that on Flush. 23 | class CountingPointsProcessor : public PointsProcessor { 24 | public: 25 | constexpr static const char* kConfigurationFileActionName = "dump_num_points"; 26 | explicit CountingPointsProcessor(PointsProcessor* next); 27 | 28 | static std::unique_ptr FromDictionary( 29 | common::LuaParameterDictionary* dictionary, PointsProcessor* next); 30 | 31 | ~CountingPointsProcessor() override {} 32 | 33 | CountingPointsProcessor(const CountingPointsProcessor&) = delete; 34 | CountingPointsProcessor& operator=(const CountingPointsProcessor&) = delete; 35 | 36 | void Process(std::unique_ptr points) override; 37 | FlushResult Flush() override; 38 | 39 | private: 40 | int64 num_points_; 41 | PointsProcessor* next_; 42 | }; 43 | 44 | } // namespace io 45 | } // namespace cartographer 46 | 47 | #endif // CARTOGRAPHER_IO_COUNTING_POINTS_PROCESSOR_H_ 48 | -------------------------------------------------------------------------------- /cartographer_annotations/io/file_writer.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/io/file_writer.h" 3 | 4 | namespace cartographer { 5 | namespace io { 6 | 7 | //out:File open for writing: the internal stream buffer supports output operations. 8 | //binary:Operations are performed in binary mode rather than text. 9 | 10 | StreamFileWriter::StreamFileWriter(const string& filename) 11 | : out_(filename, std::ios::out | std::ios::binary) {}//打开,并以2进制方式写入 12 | 13 | StreamFileWriter::~StreamFileWriter() {} 14 | 15 | //写入len个char 16 | bool StreamFileWriter::Write(const char* const data, const size_t len) { 17 | if (out_.bad()) { 18 | return false; 19 | } 20 | out_.write(data, len); 21 | return !out_.bad(); 22 | } 23 | 24 | bool StreamFileWriter::Close() { //关闭文件 25 | if (out_.bad()) { 26 | return false; 27 | } 28 | out_.close(); 29 | return !out_.bad(); 30 | } 31 | 32 | bool StreamFileWriter::WriteHeader(const char* const data, const size_t len) { 33 | if (out_.bad()) { 34 | return false; 35 | } 36 | out_.flush(); //清空buffer 37 | out_.seekp(0); //偏移量为0 38 | return Write(data, len); 39 | } 40 | 41 | } // namespace io 42 | } // namespace cartographer 43 | -------------------------------------------------------------------------------- /cartographer_annotations/io/file_writer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_IO_FILE_WRITER_H_ 3 | #define CARTOGRAPHER_IO_FILE_WRITER_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "cartographer/common/port.h" 10 | 11 | namespace cartographer { 12 | namespace io { 13 | /* 14 | FileWriter负责文件写入的虚基类,不可拷贝/赋值 15 | 没有数据成员.只提供一系列抽象接口. 16 | 1),WriteHeader(),写入文件head 17 | 2),Write(),写入数据 18 | 3),Close(),关闭文件 19 | */ 20 | // Simple abstraction for a file. 21 | class FileWriter { 22 | public: 23 | FileWriter() {} 24 | FileWriter(const FileWriter&) = delete; 25 | FileWriter& operator=(const FileWriter&) = delete; 26 | 27 | virtual ~FileWriter() {} 28 | 29 | // Write 'data' to the beginning of the file. This is required to overwrite 30 | // fixed size headers which contain the number of points once we actually know 31 | // how many points there are. 32 | virtual bool WriteHeader(const char* data, size_t len) = 0; 33 | 34 | virtual bool Write(const char* data, size_t len) = 0; 35 | virtual bool Close() = 0; 36 | }; 37 | 38 | /* 39 | StreamFileWriter 文件流写入类,继承自FileWriter 40 | 数据成员只有一个std::ofstream out_负责将文件写入磁盘 41 | 42 | */ 43 | // An Implementation of file using std::ofstream. 44 | class StreamFileWriter : public FileWriter { 45 | public: 46 | ~StreamFileWriter() override; 47 | 48 | StreamFileWriter(const string& filename); 49 | 50 | bool Write(const char* data, size_t len) override; 51 | bool WriteHeader(const char* data, size_t len) override; //从文件开始处写入,覆盖旧数据 52 | bool Close() override; 53 | 54 | private: 55 | std::ofstream out_; 56 | }; 57 | 58 | /*工厂模式, 59 | 创建一个FileWriter对象,由智能指针管理生命期, 60 | 返回值是std::unique_ptr, 61 | 函数参数是string. 62 | */ 63 | using FileWriterFactory = 64 | std::function(const string& filename)>; 65 | 66 | } // namespace io 67 | } // namespace cartographer 68 | 69 | #endif // CARTOGRAPHER_IO_FILE_WRITER_H_ 70 | -------------------------------------------------------------------------------- /cartographer_annotations/io/min_max_range_filtering_points_processor.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/io/min_max_range_filtering_points_processor.h" 3 | 4 | #include "cartographer/common/lua_parameter_dictionary.h" 5 | #include "cartographer/common/make_unique.h" 6 | #include "cartographer/io/points_batch.h" 7 | 8 | namespace cartographer { 9 | namespace io { 10 | 11 | //从.lua文件加载min_range和max_range,如:0-30 12 | std::unique_ptr 13 | MinMaxRangeFiteringPointsProcessor::FromDictionary( 14 | common::LuaParameterDictionary* const dictionary, 15 | PointsProcessor* const next) { 16 | return common::make_unique( 17 | dictionary->GetDouble("min_range"), dictionary->GetDouble("max_range"), //trajectory_builder_2d.lua:0,30 18 | //assets_writer_backpack_2d.lua 1-60 19 | next); 20 | } 21 | 22 | /* 23 | 构造函数,传递min ,max2个范围参数和 PointsProcessor* next指针 24 | 25 | */ 26 | MinMaxRangeFiteringPointsProcessor::MinMaxRangeFiteringPointsProcessor( 27 | const double min_range, const double max_range, PointsProcessor* next) 28 | : min_range_(min_range), max_range_(max_range), next_(next) {} 29 | 30 | void MinMaxRangeFiteringPointsProcessor::Process( 31 | std::unique_ptr batch) 32 | 33 | { 34 | std::vector to_remove; //待移除的索引 35 | for (size_t i = 0; i < batch->points.size(); ++i) { 36 | //计算sensor到远点的l2距离 37 | const float range = (batch->points[i] - batch->origin).norm(); 38 | if (!(min_range_ <= range && range <= max_range_)) { 39 | //不在给定范围内,加入移除队列. 40 | to_remove.push_back(i); 41 | } 42 | } 43 | RemovePoints(to_remove, batch.get()); 44 | next_->Process(std::move(batch));//完成本轮过滤,接下来进行下一次过滤.即模拟流水线操作. 45 | } 46 | 47 | PointsProcessor::FlushResult MinMaxRangeFiteringPointsProcessor::Flush() { 48 | return next_->Flush(); 49 | } 50 | 51 | } // namespace io 52 | } // namespace cartographer 53 | -------------------------------------------------------------------------------- /cartographer_annotations/io/null_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ 4 | #define CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ 5 | 6 | #include "cartographer/io/points_processor.h" 7 | 8 | namespace cartographer { 9 | namespace io { 10 | /* 11 | NullPointsProcessor是PointsProcessor的第十个子类(10) 12 | 空操作类。通常用于pipline的尾端，丢弃所有的点云，表示整个处理流程已经完毕。 13 | */ 14 | 15 | // A points processor that just drops all points. The end of a pipeline usually. 16 | class NullPointsProcessor : public PointsProcessor { 17 | public: 18 | NullPointsProcessor() {} 19 | ~NullPointsProcessor() override {} 20 | 21 | //不作任何事情 22 | void Process(std::unique_ptr points_batch) override {} 23 | //返回"kFinished"状态，此操作会传递给上一个流水线。 24 | FlushResult Flush() override { return FlushResult::kFinished; } 25 | }; 26 | 27 | } // namespace io 28 | } // namespace cartographer 29 | 30 | #endif // CARTOGRAPHER_IO_NULL_POINTS_PROCESSOR_H_ 31 | -------------------------------------------------------------------------------- /cartographer_annotations/io/pcd_writing_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include "cartographer/common/lua_parameter_dictionary.h" 5 | #include "cartographer/io/file_writer.h" 6 | #include "cartographer/io/points_processor.h" 7 | /* 8 | 点云是目标表面特性的海量点集合。 9 | 根据激光测量原理得到的点云，包括三维坐标（XYZ）和激光反射强度（Intensity）。 10 | 根据摄影测量原理得到的点云，包括三维坐标（XYZ）和颜色信息（RGB）。 11 | 结合激光测量和摄影测量原理得到点云，包括三维坐标（XYZ）、激光反射强度（Intensity）和颜色信息（RGB）。 12 | 在获取物体表面每个采样点的空间坐标后，得到的是一个点的集合，称之为“点云”(Point Cloud)。 13 | 14 | pcd文件存储了每个点的x,y,z坐标以及r,g,b,a颜色信息 15 | 16 | */ 17 | namespace cartographer { 18 | namespace io { 19 | 20 | /* 21 | PcdWritingPointsProcessor是PointsProcessor的第九个子类(9) 22 | . 23 | PcdWritingPointsProcessor类将点云按照pcd格式存储在pcd文件中. 24 | 1,构造函数初始化一个文件类名, 25 | 2,成员函数FromDictionary 从配置文件读取 文件名 26 | 3,Process()和Flush()负责写入文件 27 | 4,数据成员next_指向 下一阶段的PointsProcessor点云处理类 28 | */ 29 | // Streams a PCD file to disk. The header is written in 'Flush'. 30 | class PcdWritingPointsProcessor : public PointsProcessor { 31 | public: 32 | constexpr static const char* kConfigurationFileActionName = "write_pcd"; 33 | PcdWritingPointsProcessor(std::unique_ptr file_writer, 34 | PointsProcessor* next); 35 | 36 | static std::unique_ptr FromDictionary( 37 | FileWriterFactory file_writer_factory, 38 | common::LuaParameterDictionary* dictionary, PointsProcessor* next); 39 | 40 | ~PcdWritingPointsProcessor() override {} 41 | 42 | PcdWritingPointsProcessor(const PcdWritingPointsProcessor&) = delete; 43 | PcdWritingPointsProcessor& operator=(const PcdWritingPointsProcessor&) = 44 | delete; 45 | 46 | void Process(std::unique_ptr batch) override; 47 | FlushResult Flush() override; 48 | 49 | private: 50 | PointsProcessor* const next_; 51 | 52 | int64 num_points_; 53 | bool has_colors_; 54 | std::unique_ptr file_writer_; 55 | }; 56 | 57 | } // namespace io 58 | } // namespace cartographer 59 | -------------------------------------------------------------------------------- /cartographer_annotations/io/ply_writing_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/common/lua_parameter_dictionary.h" 3 | #include "cartographer/io/file_writer.h" 4 | #include "cartographer/io/points_processor.h" 5 | 6 | namespace cartographer { 7 | namespace io { 8 | /* 9 | PlyWritingPointsProcessor是PointsProcessor的第五个子类(5). 10 | PlyWritingPointsProcessor负责将点云point以PLY的格式写入磁盘. 11 | 12 | */ 13 | // Streams a PLY file to disk. The header is written in 'Flush'. 14 | class PlyWritingPointsProcessor : public PointsProcessor { 15 | public: 16 | constexpr static const char* kConfigurationFileActionName = "write_ply"; 17 | 18 | // 指定文件写入的工厂和流水线next指针。 19 | PlyWritingPointsProcessor(std::unique_ptr file, 20 | PointsProcessor* next); 21 | 22 | static std::unique_ptr FromDictionary( 23 | FileWriterFactory file_writer_factory, 24 | common::LuaParameterDictionary* dictionary, PointsProcessor* next); 25 | 26 | ~PlyWritingPointsProcessor() override {} 27 | 28 | PlyWritingPointsProcessor(const PlyWritingPointsProcessor&) = delete; 29 | PlyWritingPointsProcessor& operator=(const PlyWritingPointsProcessor&) = 30 | delete; 31 | //按照ply格式写点云信息 。然后直接流水线到下一PointsProcessor 32 | void Process(std::unique_ptr batch) override; 33 | FlushResult Flush() override; 34 | 35 | private: 36 | PointsProcessor* const next_; 37 | 38 | int64 num_points_; 39 | bool has_colors_;//是否是RGB 40 | std::unique_ptr file_; 41 | }; 42 | 43 | } // namespace io 44 | } // namespace cartographer 45 | -------------------------------------------------------------------------------- /cartographer_annotations/io/points_batch.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/io/points_batch.h" 3 | 4 | namespace cartographer { 5 | namespace io { 6 | 7 | /* 8 | 先对to_remove做降序排列,然后按照to_remove依次移除某些point. 9 | 10 | batch->points是vector,index 是要移除的vector元素的索引. 11 | 12 | 时间复杂度:o(m*n),m是to_remove的大小,n是to_remove[0]到batch的end()的距离. 13 | 14 | 注:erase()的复杂度是: 15 | Linear on the number of elements erased (destructions)* 16 | the number of elements after the last element deleted (moving). 17 | 返回值是一个迭代器，指向删除元素下一个元素; 18 | 使用erase()要注意迭代器失效的问题. 19 | 20 | 使用降序排序,即从end开始erase()可避免失效 21 | 22 | */ 23 | void RemovePoints(std::vector to_remove, PointsBatch* batch) { 24 | std::sort(to_remove.begin(), to_remove.end(), std::greater());//降序排列 25 | for (const int index : to_remove) { 26 | batch->points.erase(batch->points.begin() + index); //移除point 27 | if (!batch->colors.empty()) { 28 | batch->colors.erase(batch->colors.begin() + index); //point对应的rgb 29 | } 30 | if (!batch->intensities.empty()) { //移除光强度 31 | batch->intensities.erase(batch->intensities.begin() + index); 32 | } 33 | } 34 | } 35 | 36 | } // namespace io 37 | } // namespace cartographer 38 | -------------------------------------------------------------------------------- /cartographer_annotations/io/points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ 3 | #define CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ 4 | 5 | #include 6 | 7 | #include "cartographer/io/points_batch.h" 8 | 9 | namespace cartographer { 10 | namespace io { 11 | 12 | /* 13 | PointsProcessor 点云虚基类,提供一种批量处理points的抽象接口,不可拷贝/赋值 14 | 2个抽象接口: 15 | 1),Process()负责对PointsBatch进行处理 16 | 2),Flush()刷新. 17 | 在assets_writer_backpack_2d.lua文件中有各个pipeline的处理流程. 18 | 19 | */ 20 | // A processor in a pipeline. It processes a 'points_batch' and hands it to the 21 | // next processor in the pipeline. 22 | class PointsProcessor { 23 | public: 24 | enum class FlushResult { 25 | kRestartStream, 26 | kFinished, 27 | }; 28 | 29 | PointsProcessor() {} 30 | virtual ~PointsProcessor() {} //必须为虚函数,不然子类无法正确析构. 31 | 32 | PointsProcessor(const PointsProcessor&) = delete; 33 | PointsProcessor& operator=(const PointsProcessor&) = delete; 34 | 35 | // Receive a 'points_batch', process it and pass it on. 36 | virtual void Process(std::unique_ptr points_batch) = 0;//纯虚函数 37 | 38 | // Some implementations will perform expensive computations and others that do 39 | // multiple passes over the data might ask for restarting the stream. 40 | virtual FlushResult Flush() = 0; 41 | }; 42 | 43 | } // namespace io 44 | } // namespace cartographer 45 | 46 | #endif // CARTOGRAPHER_IO_POINTS_PROCESSOR_H_ 47 | -------------------------------------------------------------------------------- /cartographer_annotations/io/proto_stream_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/io/proto_stream.h" 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "cartographer/common/port.h" 10 | #include "cartographer/mapping/proto/trajectory.pb.h" 11 | #include "gtest/gtest.h" 12 | 13 | namespace cartographer { 14 | namespace io { 15 | namespace { 16 | 17 | class ProtoStreamTest : public ::testing::Test { 18 | protected: 19 | void SetUp() override { 20 | const string tmpdir = P_tmpdir; 21 | test_directory_ = tmpdir + "/proto_stream_test_XXXXXX"; 22 | ASSERT_NE(mkdtemp(&test_directory_[0]), nullptr) << strerror(errno); 23 | } 24 | 25 | void TearDown() override { remove(test_directory_.c_str()); } 26 | 27 | string test_directory_; 28 | }; 29 | 30 | TEST_F(ProtoStreamTest, WriteAndReadBack) { 31 | const string test_file = test_directory_ + "/test_trajectory.pbstream"; 32 | { 33 | ProtoStreamWriter writer(test_file); 34 | for (int i = 0; i != 10; ++i) { 35 | mapping::proto::Trajectory trajectory; 36 | trajectory.add_node()->set_timestamp(i); 37 | writer.WriteProto(trajectory); 38 | } 39 | ASSERT_TRUE(writer.Close()); 40 | } 41 | { 42 | ProtoStreamReader reader(test_file); 43 | for (int i = 0; i != 10; ++i) { 44 | mapping::proto::Trajectory trajectory; 45 | ASSERT_TRUE(reader.ReadProto(&trajectory)); 46 | ASSERT_EQ(1, trajectory.node_size()); 47 | EXPECT_EQ(i, trajectory.node(0).timestamp()); 48 | } 49 | mapping::proto::Trajectory trajectory; 50 | EXPECT_FALSE(reader.ReadProto(&trajectory)); 51 | } 52 | remove(test_file.c_str()); 53 | } 54 | 55 | } // namespace 56 | } // namespace io 57 | } // namespace cartographer 58 | -------------------------------------------------------------------------------- /cartographer_annotations/io/xyz_writing_points_processor.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ 3 | #define CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include "cartographer/common/lua_parameter_dictionary.h" 10 | #include "cartographer/io/file_writer.h" 11 | #include "cartographer/io/points_processor.h" 12 | 13 | namespace cartographer { 14 | namespace io { 15 | /* 16 | 17 | XyzWriterPointsProcessor将points以{x,y,z}形式写入 FileWriter 18 | 19 | */ 20 | // Writes ASCII xyz points. 21 | class XyzWriterPointsProcessor : public PointsProcessor { 22 | public: 23 | constexpr static const char* kConfigurationFileActionName = "write_xyz"; 24 | 25 | XyzWriterPointsProcessor(std::unique_ptr, PointsProcessor* next); 26 | 27 | static std::unique_ptr FromDictionary( 28 | FileWriterFactory file_writer_factory, 29 | common::LuaParameterDictionary* dictionary, PointsProcessor* next); 30 | 31 | ~XyzWriterPointsProcessor() override {} 32 | 33 | XyzWriterPointsProcessor(const XyzWriterPointsProcessor&) = delete; 34 | XyzWriterPointsProcessor& operator=(const XyzWriterPointsProcessor&) = delete; 35 | 36 | void Process(std::unique_ptr batch) override; 37 | FlushResult Flush() override; 38 | 39 | private: 40 | PointsProcessor* const next_; 41 | std::unique_ptr file_writer_; 42 | }; 43 | 44 | } // namespace io 45 | } // namespace cartographer 46 | 47 | #endif // CARTOGRAPHER_IO_XYZ_WRITING_POINTS_PROCESSOR_H_ 48 | -------------------------------------------------------------------------------- /cartographer_annotations/kalman_filter/gaussian_distribution_test.cc: -------------------------------------------------------------------------------- 1 | #include "cartographer/kalman_filter/gaussian_distribution.h" 2 | 3 | #include "gtest/gtest.h" 4 | 5 | namespace cartographer { 6 | namespace kalman_filter { 7 | namespace { 8 | 9 | TEST(GaussianDistributionTest, testConstructor) { 10 | Eigen::Matrix2d covariance;//2*2 11 | covariance << 1., 2., 3., 4.; 12 | GaussianDistribution distribution(Eigen::Vector2d(0., 1.), 13 | //均值是0,1 14 | covariance); 15 | EXPECT_NEAR(0., distribution.GetMean()[0], 1e-9);//0列的的均值是0 16 | EXPECT_NEAR(1., distribution.GetMean()[1], 1e-9); 17 | EXPECT_NEAR(2., distribution.GetCovariance()(0, 1), 1e-9);//0行1列的方差是2 18 | } 19 | 20 | } // namespace 21 | } // namespace kalman_filter 22 | } // namespace cartographer 23 | -------------------------------------------------------------------------------- /cartographer_annotations/kalman_filter/proto/pose_tracker_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.kalman_filter.proto; 18 | 19 | message PoseTrackerOptions { 20 | // Model variances depend linearly on time. 21 | optional double position_model_variance = 1; //位置方差 22 | optional double orientation_model_variance = 2; //方向方差 23 | optional double velocity_model_variance = 3; //速度 24 | 25 | // Time constant for the orientation moving average based on observed gravity 26 | // via linear acceleration. 27 | optional double imu_gravity_time_constant = 4; //时间 28 | optional double imu_gravity_variance = 5; //重力加速度方差 g 29 | 30 | // Maximum number of previous odometry states to keep. 31 | optional int32 num_odometry_states = 6; 32 | } 33 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/detect_floors.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ 3 | #define CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ 4 | 5 | #include "cartographer/mapping/proto/trajectory.pb.h" 6 | #include "cartographer/common/time.h" 7 | 8 | namespace cartographer { 9 | namespace mapping { 10 | 11 | /* 12 | span,范围 13 | Timespan表征时间范围。 14 | */ 15 | struct Timespan { 16 | common::Time start; 17 | common::Time end; 18 | }; 19 | 20 | /* 21 | 一个楼层对应多个扫描timespan：有可能重复的扫描多次 22 | 但只有一个高度z。 23 | */ 24 | struct Floor { 25 | // The spans of time we spent on this floor. Since we might have 26 | //walked up and down many times in this place, there can be many spans 27 | // of time we spent on a particular floor. 28 | std::vector timespans; 29 | 30 | // The median z-value of this floor. 31 | double z; //z轴的中值 32 | }; 33 | 34 | /* 35 | heuristic:启发式, 36 | 使用启发式搜索寻找building的不同楼层的z值。 37 | 对楼层的要求：同一floor同一z值，只要有“楼梯”出现，即为“产生”一层 38 | */ 39 | // Uses a heuristic which looks at z-values of the poses to detect individual 40 | // floors of a building. This requires that floors are *mostly* on the same 41 | // z-height and that level changes happen *relatively* abrubtly, e.g. by taking 42 | // the stairs. 43 | std::vector DetectFloors(const proto::Trajectory& trajectory); 44 | 45 | } // namespace mapping 46 | } // namespace cartographer 47 | 48 | #endif // CARTOGRAPHER_MAPPING_DETECT_FLOORS_H_ 49 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/odometry_state_tracker.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/mapping/odometry_state_tracker.h" 3 | 4 | #include "cartographer/transform/rigid_transform.h" 5 | 6 | namespace cartographer { 7 | namespace mapping { 8 | 9 | OdometryState::OdometryState(const common::Time time, 10 | const transform::Rigid3d& odometer_pose, 11 | const transform::Rigid3d& state_pose) 12 | : time(time), odometer_pose(odometer_pose), state_pose(state_pose) {} 13 | 14 | OdometryStateTracker::OdometryStateTracker(const int window_size) 15 | : window_size_(window_size) { 16 | CHECK_GT(window_size, 0); 17 | } 18 | 19 | const OdometryStateTracker::OdometryStates& 20 | OdometryStateTracker::odometry_states() const { 21 | return odometry_states_; 22 | } 23 | 24 | //添新删旧 25 | void OdometryStateTracker::AddOdometryState( 26 | const OdometryState& odometry_state) { 27 | odometry_states_.push_back(odometry_state); 28 | while (odometry_states_.size() > window_size_) { 29 | odometry_states_.pop_front(); 30 | } 31 | } 32 | 33 | bool OdometryStateTracker::empty() const { return odometry_states_.empty(); } 34 | 35 | //返回最后一个 36 | const OdometryState& OdometryStateTracker::newest() const { 37 | return odometry_states_.back(); 38 | } 39 | 40 | } // namespace mapping 41 | } // namespace cartographer 42 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/probability_values_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/mapping/probability_values.h" 3 | #include "gtest/gtest.h" 4 | 5 | namespace cartographer { 6 | namespace mapping { 7 | namespace { 8 | 9 | TEST(ProbabilityValuesTest, OddsConversions) { 10 | EXPECT_NEAR(ProbabilityFromOdds(Odds(kMinProbability)), kMinProbability, 11 | 1e-6); 12 | EXPECT_NEAR(ProbabilityFromOdds(Odds(kMaxProbability)), kMaxProbability, 13 | 1e-6); 14 | EXPECT_NEAR(ProbabilityFromOdds(Odds(0.5)), 0.5, 1e-6); 15 | } 16 | 17 | } // namespace 18 | } // namespace mapping 19 | } // namespace cartographer 20 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/proto/map_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping/proto/sparse_pose_graph_options.proto"; 18 | 19 | package cartographer.mapping.proto; 20 | 21 | message MapBuilderOptions { 22 | optional bool use_trajectory_builder_2d = 1; 23 | optional bool use_trajectory_builder_3d = 2; 24 | 25 | // Number of threads to use for background computations. 26 | optional int32 num_background_threads = 3; 27 | optional SparsePoseGraphOptions sparse_pose_graph_options = 4; 28 | } 29 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/proto/trajectory.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.proto; 18 | 19 | import "cartographer/transform/proto/transform.proto"; 20 | 21 | option java_outer_classname = "TrajectoryOuterClass"; 22 | 23 | message Trajectory { 24 | // NEXT_ID: 7 25 | message Node { 26 | optional int64 timestamp = 1; 27 | // Transform from tracking to map frame. 28 | optional transform.proto.Rigid3d pose = 5; 29 | } 30 | 31 | message Submap { 32 | optional transform.proto.Rigid3d pose = 1; 33 | } 34 | 35 | // Time-ordered sequence of Nodes. 36 | repeated Node node = 1; 37 | 38 | // Submaps associated with the trajectory. 39 | repeated Submap submap = 2; 40 | } 41 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/proto/trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto"; 18 | import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; 19 | 20 | package cartographer.mapping.proto; 21 | 22 | message TrajectoryBuilderOptions { 23 | optional mapping_2d.proto.LocalTrajectoryBuilderOptions 24 | trajectory_builder_2d_options = 1; 25 | optional mapping_3d.proto.LocalTrajectoryBuilderOptions 26 | trajectory_builder_3d_options = 2; 27 | } 28 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/proto/trajectory_connectivity.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.proto; 18 | 19 | // This is how proto2 calls the outer class since there is already a message 20 | // with the same name in the file. 21 | option java_outer_classname = "TrajectoryConnectivityOuterClass"; 22 | 23 | // Connectivity structure between trajectories. 24 | message TrajectoryConnectivity { 25 | message ConnectedComponent { 26 | repeated int32 trajectory_id = 1; 27 | } 28 | 29 | repeated ConnectedComponent connected_component = 1; 30 | } 31 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/sparse_pose_graph/constraint_builder.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 3 | #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 4 | 5 | #include "cartographer/common/lua_parameter_dictionary.h" 6 | #include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h" 7 | 8 | namespace cartographer { 9 | namespace mapping { 10 | namespace sparse_pose_graph { 11 | 12 | //获取sparse_pose_graph.lua配置参数 13 | proto::ConstraintBuilderOptions CreateConstraintBuilderOptions( 14 | common::LuaParameterDictionary* parameter_dictionary); 15 | 16 | } // namespace sparse_pose_graph 17 | } // namespace mapping 18 | } // namespace cartographer 19 | 20 | #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_CONSTRAINT_BUILDER_H_ 21 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/sparse_pose_graph/optimization_problem_options.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h" 3 | 4 | #include "cartographer/common/ceres_solver_options.h" 5 | 6 | namespace cartographer { 7 | namespace mapping { 8 | namespace sparse_pose_graph { 9 | /* 10 | sparse_pose_graph.lua: 11 | 12 | optimization_problem = { 13 | huber_scale = 1e1, 14 | acceleration_weight = 1e3, 15 | rotation_weight = 3e5, 16 | consecutive_scan_translation_penalty_factor = 1e5, 17 | consecutive_scan_rotation_penalty_factor = 1e5, 18 | log_solver_summary = false, 19 | ceres_solver_options = { 20 | use_nonmonotonic_steps = false, 21 | max_num_iterations = 50, 22 | num_threads = 7, 23 | }, 24 | }, 25 | 26 | 参数配置如上。 27 | */ 28 | proto::OptimizationProblemOptions CreateOptimizationProblemOptions( 29 | common::LuaParameterDictionary* const parameter_dictionary) { 30 | proto::OptimizationProblemOptions options; 31 | options.set_huber_scale(parameter_dictionary->GetDouble("huber_scale")); 32 | options.set_acceleration_weight( 33 | parameter_dictionary->GetDouble("acceleration_weight")); 34 | options.set_rotation_weight( 35 | parameter_dictionary->GetDouble("rotation_weight")); 36 | options.set_consecutive_scan_translation_penalty_factor( 37 | parameter_dictionary->GetDouble( 38 | "consecutive_scan_translation_penalty_factor")); 39 | options.set_consecutive_scan_rotation_penalty_factor( 40 | parameter_dictionary->GetDouble( 41 | "consecutive_scan_rotation_penalty_factor")); 42 | options.set_log_solver_summary( 43 | parameter_dictionary->GetBool("log_solver_summary")); 44 | *options.mutable_ceres_solver_options() = 45 | common::CreateCeresSolverOptionsProto( 46 | parameter_dictionary->GetDictionary("ceres_solver_options").get()); 47 | return options; 48 | } 49 | 50 | } // namespace sparse_pose_graph 51 | } // namespace mapping 52 | } // namespace cartographer 53 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/sparse_pose_graph/optimization_problem_options.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 4 | #define CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 5 | 6 | #include "cartographer/common/lua_parameter_dictionary.h" 7 | #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" 8 | 9 | namespace cartographer { 10 | namespace mapping { 11 | namespace sparse_pose_graph { 12 | /* 13 | 从sparse_pose_graph.lua文件配置 获取参数 14 | 15 | message OptimizationProblemOptions { 16 | optional double huber_scale = 1; 17 | optional double acceleration_weight = 8; 18 | optional double rotation_weight = 9; 19 | 20 | optional double consecutive_scan_translation_penalty_factor = 2; 21 | optional double consecutive_scan_rotation_penalty_factor = 3; 22 | optional bool log_solver_summary = 5; 23 | 24 | optional common.proto.CeresSolverOptions ceres_solver_options = 7; 25 | } 26 | 27 | */ 28 | proto::OptimizationProblemOptions CreateOptimizationProblemOptions( 29 | common::LuaParameterDictionary* parameter_dictionary); 30 | 31 | } // namespace sparse_pose_graph 32 | } // namespace mapping 33 | } // namespace cartographer 34 | 35 | #endif // CARTOGRAPHER_MAPPING_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_OPTIONS_H_ 36 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/sparse_pose_graph/proto/optimization_problem_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping.sparse_pose_graph.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 11 22 | message OptimizationProblemOptions { 23 | // Scaling parameter for Huber loss function. 24 | optional double huber_scale = 1; 25 | 26 | // Scaling parameter for the IMU acceleration term. 27 | optional double acceleration_weight = 8; 28 | 29 | // Scaling parameter for the IMU rotation term. 30 | optional double rotation_weight = 9; 31 | 32 | // Penalty factors for changes to the relative pose between consecutive scans. 33 | optional double consecutive_scan_translation_penalty_factor = 2; 34 | optional double consecutive_scan_rotation_penalty_factor = 3; 35 | 36 | // If true, the Ceres solver summary will be logged for every optimization. 37 | optional bool log_solver_summary = 5; 38 | 39 | optional common.proto.CeresSolverOptions ceres_solver_options = 7; 40 | } 41 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/submaps_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/mapping/submaps.h" 3 | 4 | #include 5 | 6 | #include "gtest/gtest.h" 7 | 8 | namespace cartographer { 9 | namespace mapping { 10 | namespace { 11 | 12 | // Converts the given log odds to a probability. This function is known to be 13 | // very slow, because expf is incredibly slow. 14 | inline float Expit(float log_odds) { //求指数倍 15 | const float exp_log_odds = std::exp(log_odds); 16 | return exp_log_odds / (1.f + exp_log_odds); 17 | } 18 | 19 | TEST(SubmapsTest, LogOddsConversions) { 20 | EXPECT_NEAR(Expit(Logit(kMinProbability)), kMinProbability, 1e-6); 21 | EXPECT_NEAR(Expit(Logit(kMaxProbability)), kMaxProbability, 1e-6); 22 | EXPECT_NEAR(Expit(Logit(0.5)), 0.5, 1e-6); //变换再反变换- >不变 23 | } 24 | 25 | } // namespace 26 | } // namespace mapping 27 | } // namespace cartographer 28 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/trajectory_builder.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/mapping/trajectory_builder.h" 3 | 4 | #include "cartographer/mapping_2d/local_trajectory_builder_options.h" 5 | #include "cartographer/mapping_3d/local_trajectory_builder_options.h" 6 | 7 | namespace cartographer { 8 | namespace mapping { 9 | 10 | //获取trajectory_builder.lua的内容 11 | proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions( 12 | common::LuaParameterDictionary* const parameter_dictionary) { 13 | proto::TrajectoryBuilderOptions options; 14 | *options.mutable_trajectory_builder_2d_options() = 15 | mapping_2d::CreateLocalTrajectoryBuilderOptions( 16 | parameter_dictionary->GetDictionary("trajectory_builder_2d").get()); 17 | *options.mutable_trajectory_builder_3d_options() = 18 | mapping_3d::CreateLocalTrajectoryBuilderOptions( 19 | parameter_dictionary->GetDictionary("trajectory_builder_3d").get()); 20 | return options; 21 | } 22 | 23 | } // namespace mapping 24 | } // namespace cartographer 25 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping/trajectory_node.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 3 | #define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 4 | 5 | #include 6 | #include 7 | 8 | #include "Eigen/Core" 9 | #include "cartographer/common/time.h" 10 | #include "cartographer/sensor/range_data.h" 11 | #include "cartographer/transform/rigid_transform.h" 12 | 13 | namespace cartographer { 14 | namespace mapping { 15 | 16 | class Submaps; 17 | /* 18 | 轨迹节点TrajectoryNode类，含有一个内部类ConstantData。 19 | TrajectoryNode作用：在连续的轨迹上采样一些离散的点用于key frame，标识pose frame。 20 | 21 | 数据成员： 22 | 1), ConstantData* 23 | 2) 位姿，pose 24 | 25 | 26 | ConstantData类： 27 | 28 | 数据成员： 29 | 1，time ：时间 30 | 2，range_data_2d和range_data_3d：测量数据 31 | 3,trajectory_id:本节点所属的轨迹 32 | 4，Rigid3d ：tracking frame 到 pose frame的矩阵变换。 33 | */ 34 | struct TrajectoryNode { 35 | 36 | struct ConstantData { 37 | common::Time time; 38 | 39 | // Range data in 'pose' frame. Only used in the 2D case. 40 | sensor::RangeData range_data_2d;//测量得到的2D range数据 41 | 42 | // Range data in 'pose' frame. Only used in the 3D case. 43 | sensor::CompressedRangeData range_data_3d;//测量得到的3D range数据 44 | 45 | // Trajectory this node belongs to. 46 | int trajectory_id; //节点所属id 47 | 48 | // Transform from the 3D 'tracking' frame to the 'pose' frame of the range 49 | // data, which contains roll, pitch and height for 2D. In 3D this is always 50 | // identity. 51 | transform::Rigid3d tracking_to_pose; //涉及的3D变换 52 | }; 53 | 54 | common::Time time() const { return constant_data->time; } 55 | 56 | const ConstantData* constant_data; 57 | //常指针.指向某块内存,该内存块的数据不变，指针本身可以变。 58 | 59 | transform::Rigid3d pose; 60 | }; 61 | 62 | } // namespace mapping 63 | } // namespace cartographer 64 | 65 | #endif // CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_ 66 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_2d { 25 | 26 | proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( 27 | common::LuaParameterDictionary* parameter_dictionary); 28 | 29 | } // namespace mapping_2d 30 | } // namespace cartographer 31 | 32 | #endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 33 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/proto/cell_limits.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.proto; 18 | 19 | message CellLimits { 20 | optional int32 num_x_cells = 1; 21 | optional int32 num_y_cells = 2; 22 | } 23 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/proto/map_limits.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_2d/proto/cell_limits.proto"; 18 | import "cartographer/transform/proto/transform.proto"; 19 | 20 | package cartographer.mapping_2d.proto; 21 | 22 | message MapLimits { 23 | optional double resolution = 1; 24 | optional cartographer.transform.proto.Vector2d max = 2; 25 | optional CellLimits cell_limits = 3; 26 | } 27 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/proto/probability_grid.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_2d/proto/map_limits.proto"; 18 | 19 | package cartographer.mapping_2d.proto; 20 | 21 | message ProbabilityGrid { 22 | optional MapLimits limits = 1; 23 | // These values are actually int16s, but protos don't have a native int16 24 | // type. 25 | repeated int32 cells = 2; 26 | repeated int32 update_indices = 3; 27 | optional int32 max_x = 4; 28 | optional int32 max_y = 5; 29 | optional int32 min_x = 6; 30 | optional int32 min_y = 7; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/proto/range_data_inserter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.proto; 18 | 19 | message RangeDataInserterOptions { 20 | // Probability change for a hit (this will be converted to odds and therefore 21 | // must be greater than 0.5). 22 | optional double hit_probability = 1; 23 | 24 | // Probability change for a miss (this will be converted to odds and therefore 25 | // must be less than 0.5). 26 | optional double miss_probability = 2; 27 | 28 | // If 'false', free space will not change the probabilities in the occupancy 29 | // grid. 30 | optional bool insert_free_space = 3; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/proto/submaps_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_2d/proto/range_data_inserter_options.proto"; 18 | 19 | package cartographer.mapping_2d.proto; 20 | 21 | message SubmapsOptions { 22 | // Resolution of the map in meters. 23 | optional double resolution = 1; 24 | 25 | // Half the width/height of each submap, its "radius". 26 | optional double half_length = 2; 27 | 28 | // Number of scans before adding a new submap. Each submap will get twice the 29 | // number of scans inserted: First for initialization without being matched 30 | // against, then while being matched. 31 | optional int32 num_range_data = 3; 32 | 33 | // If enabled, submap%d.png images are written for debugging. 34 | optional bool output_debug_images = 4; 35 | 36 | optional RangeDataInserterOptions range_data_inserter_options = 5; 37 | } 38 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/ray_casting.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 18 | #define CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 19 | 20 | #include 21 | 22 | #include "cartographer/mapping_2d/map_limits.h" 23 | #include "cartographer/mapping_2d/xy_index.h" 24 | #include "cartographer/sensor/point_cloud.h" 25 | #include "cartographer/sensor/range_data.h" 26 | #include "cartographer/transform/transform.h" 27 | 28 | namespace cartographer { 29 | namespace mapping_2d { 30 | 31 | // For each ray in 'range_data', calls 'hit_visitor' and 'miss_visitor' on the 32 | // appropriate cells. Hits are handled before misses. 33 | void CastRays(const sensor::RangeData& range_data, const MapLimits& limits, 34 | const std::function& hit_visitor, 35 | const std::function& miss_visitor); 36 | 37 | } // namespace mapping_2d 38 | } // namespace cartographer 39 | 40 | #endif // CARTOGRAPHER_MAPPING_2D_RAY_CASTING_H_ 41 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 10 22 | message CeresScanMatcherOptions { 23 | // Scaling parameters for each cost functor. 24 | optional double occupied_space_weight = 1; 25 | optional double translation_weight = 2; 26 | optional double rotation_weight = 3; 27 | 28 | // Configure the Ceres solver. See the Ceres documentation for more 29 | // information: https://code.google.com/p/ceres-solver/ 30 | optional common.proto.CeresSolverOptions ceres_solver_options = 9; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | message FastCorrelativeScanMatcherOptions { 20 | // Minimum linear search window in which the best possible scan alignment 21 | // will be found. 22 | optional double linear_search_window = 3; 23 | 24 | // Minimum angular search window in which the best possible scan alignment 25 | // will be found. 26 | optional double angular_search_window = 4; 27 | 28 | // Number of precomputed grids to use. 29 | optional int32 branch_and_bound_depth = 2; 30 | } 31 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_2d.scan_matching.proto; 18 | 19 | message RealTimeCorrelativeScanMatcherOptions { 20 | // Minimum linear search window in which the best possible scan alignment 21 | // will be found. 22 | optional double linear_search_window = 1; 23 | 24 | // Minimum angular search window in which the best possible scan alignment 25 | // will be found. 26 | optional double angular_search_window = 2; 27 | 28 | // Weights applied to each part of the score. 29 | optional double translation_delta_cost_weight = 3; 30 | optional double rotation_delta_cost_weight = 4; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/ceres_pose.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/ceres_pose.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | CeresPose::CeresPose( 23 | const transform::Rigid3d& rigid, 24 | std::unique_ptr translation_parametrization, 25 | std::unique_ptr rotation_parametrization, 26 | ceres::Problem* problem) 27 | : translation_({{rigid.translation().x(), rigid.translation().y(), 28 | rigid.translation().z()}}), 29 | rotation_({{rigid.rotation().w(), rigid.rotation().x(), 30 | rigid.rotation().y(), rigid.rotation().z()}}) { 31 | problem->AddParameterBlock(translation_.data(), 3, 32 | translation_parametrization.release()); 33 | problem->AddParameterBlock(rotation_.data(), 4, 34 | rotation_parametrization.release()); 35 | } 36 | 37 | const transform::Rigid3d CeresPose::ToRigid() const { 38 | return transform::Rigid3d( 39 | Eigen::Map(translation_.data()), 40 | Eigen::Quaterniond(rotation_[0], rotation_[1], rotation_[2], 41 | rotation_[3])); 42 | } 43 | 44 | } // namespace mapping_3d 45 | } // namespace cartographer 46 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/ceres_pose.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "Eigen/Core" 24 | #include "cartographer/transform/rigid_transform.h" 25 | #include "ceres/ceres.h" 26 | 27 | namespace cartographer { 28 | namespace mapping_3d { 29 | 30 | class CeresPose { 31 | public: 32 | CeresPose( 33 | const transform::Rigid3d& rigid, 34 | std::unique_ptr translation_parametrization, 35 | std::unique_ptr rotation_parametrization, 36 | ceres::Problem* problem); 37 | 38 | CeresPose(const CeresPose&) = delete; 39 | CeresPose& operator=(const CeresPose&) = delete; 40 | 41 | const transform::Rigid3d ToRigid() const; 42 | 43 | double* translation() { return translation_.data(); } 44 | double* rotation() { return rotation_.data(); } 45 | 46 | private: 47 | std::array translation_; 48 | // Rotation quaternion as (w, x, y, z). 49 | std::array rotation_; 50 | }; 51 | 52 | } // namespace mapping_3d 53 | } // namespace cartographer 54 | 55 | #endif // CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ 56 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/imu_integration.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/imu_integration.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | IntegrateImuResult IntegrateImu( 23 | const std::deque& imu_data, const common::Time start_time, 24 | const common::Time end_time, std::deque::const_iterator* it) { 25 | return IntegrateImu(imu_data, Eigen::Affine3d::Identity(), 26 | Eigen::Affine3d::Identity(), start_time, end_time, 27 | it); 28 | } 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/kalman_local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/kalman_local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::KalmanLocalTrajectoryBuilderOptions 27 | CreateKalmanLocalTrajectoryBuilderOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | 33 | #endif // CARTOGRAPHER_MAPPING_3D_KALMAN_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 34 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/local_trajectory_builder.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/local_trajectory_builder.h" 18 | 19 | #include "cartographer/common/make_unique.h" 20 | #include "cartographer/mapping_3d/kalman_local_trajectory_builder.h" 21 | #include "cartographer/mapping_3d/optimizing_local_trajectory_builder.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | std::unique_ptr CreateLocalTrajectoryBuilder( 27 | const proto::LocalTrajectoryBuilderOptions& 28 | local_trajectory_builder_options) { 29 | switch (local_trajectory_builder_options.use()) { 30 | case proto::LocalTrajectoryBuilderOptions::KALMAN: 31 | return common::make_unique( 32 | local_trajectory_builder_options); 33 | case proto::LocalTrajectoryBuilderOptions::OPTIMIZING: 34 | return common::make_unique( 35 | local_trajectory_builder_options); 36 | } 37 | LOG(FATAL); 38 | } 39 | 40 | } // namespace mapping_3d 41 | } // namespace cartographer 42 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/local_trajectory_builder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 19 | 20 | #include 21 | #include 22 | 23 | #include "cartographer/mapping_3d/local_trajectory_builder_interface.h" 24 | #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" 25 | 26 | namespace cartographer { 27 | namespace mapping_3d { 28 | 29 | std::unique_ptr CreateLocalTrajectoryBuilder( 30 | const proto::LocalTrajectoryBuilderOptions& 31 | local_trajectory_builder_options); 32 | 33 | } // namespace mapping_3d 34 | } // namespace cartographer 35 | 36 | #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_H_ 37 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( 27 | common::LuaParameterDictionary* parameter_dictionary); 28 | 29 | } // namespace mapping_3d 30 | } // namespace cartographer 31 | 32 | #endif // CARTOGRAPHER_MAPPING_3D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 33 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/optimizing_local_trajectory_builder_options.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #include "cartographer/mapping_3d/optimizing_local_trajectory_builder_options.h" 18 | 19 | namespace cartographer { 20 | namespace mapping_3d { 21 | 22 | proto::OptimizingLocalTrajectoryBuilderOptions 23 | CreateOptimizingLocalTrajectoryBuilderOptions( 24 | common::LuaParameterDictionary* const parameter_dictionary) { 25 | proto::OptimizingLocalTrajectoryBuilderOptions options; 26 | options.set_high_resolution_grid_weight( 27 | parameter_dictionary->GetDouble("high_resolution_grid_weight")); 28 | options.set_low_resolution_grid_weight( 29 | parameter_dictionary->GetDouble("low_resolution_grid_weight")); 30 | options.set_velocity_weight( 31 | parameter_dictionary->GetDouble("velocity_weight")); 32 | options.set_translation_weight( 33 | parameter_dictionary->GetDouble("translation_weight")); 34 | options.set_rotation_weight( 35 | parameter_dictionary->GetDouble("rotation_weight")); 36 | options.set_odometry_translation_weight( 37 | parameter_dictionary->GetDouble("odometry_translation_weight")); 38 | options.set_odometry_rotation_weight( 39 | parameter_dictionary->GetDouble("odometry_rotation_weight")); 40 | return options; 41 | } 42 | 43 | } // namespace mapping_3d 44 | } // namespace cartographer 45 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/optimizing_local_trajectory_builder_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 19 | 20 | #include "cartographer/common/lua_parameter_dictionary.h" 21 | #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h" 22 | 23 | namespace cartographer { 24 | namespace mapping_3d { 25 | 26 | proto::OptimizingLocalTrajectoryBuilderOptions 27 | CreateOptimizingLocalTrajectoryBuilderOptions( 28 | common::LuaParameterDictionary* parameter_dictionary); 29 | 30 | } // namespace mapping_3d 31 | } // namespace cartographer 32 | 33 | #endif // CARTOGRAPHER_MAPPING_3D_OPTIMIZING_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ 34 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/hybrid_grid.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message HybridGrid { 20 | optional float resolution = 1; 21 | // '{x, y, z}_indices[i]' is the index of 'values[i]'. 22 | repeated sint32 x_indices = 3 [packed = true]; 23 | repeated sint32 y_indices = 4 [packed = true]; 24 | repeated sint32 z_indices = 5 [packed = true]; 25 | // The entries in 'values' should be uint16s, not int32s, but protos don't 26 | // have a uint16 type. 27 | repeated int32 values = 6 [packed = true]; 28 | } 29 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/kalman_local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/kalman_filter/proto/pose_tracker_options.proto"; 18 | import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto"; 19 | 20 | package cartographer.mapping_3d.proto; 21 | 22 | message KalmanLocalTrajectoryBuilderOptions { 23 | // Whether to solve the online scan matching first using the correlative scan 24 | // matcher to generate a good starting point for Ceres. 25 | optional bool use_online_correlative_scan_matching = 1; 26 | 27 | optional mapping_2d.scan_matching.proto.RealTimeCorrelativeScanMatcherOptions 28 | real_time_correlative_scan_matcher_options = 2; 29 | optional kalman_filter.proto.PoseTrackerOptions pose_tracker_options = 3; 30 | 31 | optional double scan_matcher_variance = 6; 32 | optional double odometer_translational_variance = 4; 33 | optional double odometer_rotational_variance = 5; 34 | } 35 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/motion_filter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message MotionFilterOptions { 20 | // Threshold above which a new scan is inserted based on time. 21 | optional double max_time_seconds = 1; 22 | 23 | // Threshold above which a new scan is inserted based on linear motion. 24 | optional double max_distance_meters = 2; 25 | 26 | // Threshold above which a new scan is inserted based on rotational motion. 27 | optional double max_angle_radians = 3; 28 | } 29 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/optimizing_local_trajectory_builder_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message OptimizingLocalTrajectoryBuilderOptions { 20 | optional double high_resolution_grid_weight = 6; 21 | optional double low_resolution_grid_weight = 7; 22 | optional double velocity_weight = 1; 23 | optional double translation_weight = 2; 24 | optional double rotation_weight = 3; 25 | optional double odometry_translation_weight = 4; 26 | optional double odometry_rotation_weight = 5; 27 | } 28 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/range_data_inserter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.proto; 18 | 19 | message RangeDataInserterOptions { 20 | // Probability change for a hit (this will be converted to odds and therefore 21 | // must be greater than 0.5). 22 | optional double hit_probability = 1; 23 | 24 | // Probability change for a miss (this will be converted to odds and therefore 25 | // must be less than 0.5). 26 | optional double miss_probability = 2; 27 | 28 | // Up to how many free space voxels are updated for scan matching. 29 | // 0 disables free space. 30 | optional int32 num_free_space_voxels = 3; 31 | } 32 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/proto/submaps_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | import "cartographer/mapping_3d/proto/range_data_inserter_options.proto"; 18 | 19 | package cartographer.mapping_3d.proto; 20 | 21 | message SubmapsOptions { 22 | // Resolution of the 'high_resolution' map in meters used for local SLAM and 23 | // loop closure. 24 | optional double high_resolution = 1; 25 | 26 | // Maximum range to filter the point cloud to before insertion into the 27 | // 'high_resolution' map. 28 | optional double high_resolution_max_range = 4; 29 | 30 | // Resolution of the 'low_resolution' version of the map in meters used for 31 | // local SLAM only. 32 | optional double low_resolution = 5; 33 | 34 | // Number of scans before adding a new submap. Each submap will get twice the 35 | // number of scans inserted: First for initialization without being matched 36 | // against, then while being matched. 37 | optional int32 num_range_data = 2; 38 | 39 | optional RangeDataInserterOptions range_data_inserter_options = 3; 40 | } 41 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/range_data_inserter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2016 The Cartographer Authors 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | #ifndef CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ 18 | #define CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ 19 | 20 | #include "cartographer/mapping_3d/hybrid_grid.h" 21 | #include "cartographer/mapping_3d/proto/range_data_inserter_options.pb.h" 22 | #include "cartographer/sensor/point_cloud.h" 23 | #include "cartographer/sensor/range_data.h" 24 | 25 | namespace cartographer { 26 | namespace mapping_3d { 27 | 28 | proto::RangeDataInserterOptions CreateRangeDataInserterOptions( 29 | common::LuaParameterDictionary* parameter_dictionary); 30 | 31 | class RangeDataInserter { 32 | public: 33 | explicit RangeDataInserter(const proto::RangeDataInserterOptions& options); 34 | 35 | RangeDataInserter(const RangeDataInserter&) = delete; 36 | RangeDataInserter& operator=(const RangeDataInserter&) = delete; 37 | 38 | // Inserts 'range_data' into 'hybrid_grid'. 39 | void Insert(const sensor::RangeData& range_data, 40 | HybridGrid* hybrid_grid) const; 41 | 42 | private: 43 | const proto::RangeDataInserterOptions options_; 44 | const std::vector hit_table_; 45 | const std::vector miss_table_; 46 | }; 47 | 48 | } // namespace mapping_3d 49 | } // namespace cartographer 50 | 51 | #endif // CARTOGRAPHER_MAPPING_3D_RANGE_DATA_INSERTER_H_ 52 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.scan_matching.proto; 18 | 19 | import "cartographer/common/proto/ceres_solver_options.proto"; 20 | 21 | // NEXT ID: 7 22 | message CeresScanMatcherOptions { 23 | // Scaling parameters for each cost functor. 24 | repeated double occupied_space_weight = 1; 25 | optional double translation_weight = 2; 26 | optional double rotation_weight = 3; 27 | 28 | // Whether only to allow changes to yaw, keeping roll/pitch constant. 29 | optional bool only_optimize_yaw = 5; 30 | 31 | // Configure the Ceres solver. See the Ceres documentation for more 32 | // information: https://code.google.com/p/ceres-solver/ 33 | optional common.proto.CeresSolverOptions ceres_solver_options = 6; 34 | } 35 | -------------------------------------------------------------------------------- /cartographer_annotations/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.mapping_3d.scan_matching.proto; 18 | 19 | message FastCorrelativeScanMatcherOptions { 20 | // Number of precomputed grids to use. 21 | optional int32 branch_and_bound_depth = 2; 22 | 23 | // Number of full resolution grids to use, additional grids will reduce the 24 | // resolution by half each. 25 | optional int32 full_resolution_depth = 8; 26 | 27 | // Number of histogram buckets for the rotational scan matcher. 28 | optional int32 rotational_histogram_size = 3; 29 | 30 | // Minimum score for the rotational scan matcher. 31 | optional double min_rotational_score = 4; 32 | 33 | // Linear search window in the plane orthogonal to gravity in which the best 34 | // possible scan alignment will be found. 35 | optional double linear_xy_search_window = 5; 36 | 37 | // Linear search window in the gravity direction in which the best possible 38 | // scan alignment will be found. 39 | optional double linear_z_search_window = 6; 40 | 41 | // Minimum angular search window in which the best possible scan alignment 42 | // will be found. 43 | optional double angular_search_window = 7; 44 | } 45 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/collator.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/sensor/collator.h" 4 | 5 | namespace cartographer { 6 | namespace sensor { 7 | 8 | /* 9 | 一个轨迹线,多个传感器 10 | 11 | */ 12 | 13 | /* 14 | struct QueueKey { 15 | int trajectory_id;// 轨线id; 16 | string sensor_id; //传感器id 17 | } 18 | 19 | */ 20 | void Collator::AddTrajectory( 21 | const int trajectory_id, 22 | const std::unordered_set& expected_sensor_ids, 23 | const Callback callback) { 24 | for (const auto& sensor_id : expected_sensor_ids) { 25 | //对于每一个轨迹线+传感器,设置一个key 26 | const auto queue_key = QueueKey{trajectory_id, sensor_id}; 27 | 28 | //添加一个名为key的队列,并设置回调函数处理data 29 | queue_.AddQueue(queue_key, 30 | [callback, sensor_id](std::unique_ptr data) { 31 | callback(sensor_id, std::move(data)); 32 | }); 33 | 34 | //map>:添加轨迹线对应的key 35 | queue_keys_[trajectory_id].push_back(queue_key); 36 | } 37 | } 38 | 39 | 40 | /*队列不再接收数据*/ 41 | void Collator::FinishTrajectory(const int trajectory_id) { 42 | for (const auto& queue_key : queue_keys_[trajectory_id]) { 43 | queue_.MarkQueueAsFinished(queue_key); 44 | } 45 | } 46 | 47 | 48 | /* 49 | 主要的操作,添加传感器数据,数据形式是:key+data 50 | */ 51 | void Collator::AddSensorData(const int trajectory_id, const string& sensor_id, 52 | std::unique_ptr data) { 53 | //找到key，再move(data) 54 | queue_.Add(QueueKey{trajectory_id, sensor_id}, std::move(data)); 55 | } 56 | 57 | void Collator::Flush() { queue_.Flush(); } 58 | 59 | int Collator::GetBlockingTrajectoryId() const { 60 | return queue_.GetBlocker().trajectory_id; 61 | } 62 | 63 | } // namespace sensor 64 | } // namespace cartographer 65 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/configuration.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 3 | #define CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 4 | 5 | #include "Eigen/Geometry" 6 | #include "cartographer/common/lua_parameter_dictionary.h" 7 | #include "cartographer/sensor/proto/configuration.pb.h" 8 | #include "cartographer/transform/rigid_transform.h" 9 | 10 | namespace cartographer { 11 | namespace sensor { 12 | 13 | //从sensor配置文件解析sensor的数据参数。主要是sensor到机器坐标的转换 14 | // Creates the configuration for a singular sensor from 'parameter_dictionary'. 15 | proto::Configuration::Sensor CreateSensorConfiguration( 16 | common::LuaParameterDictionary* parameter_dictionary); 17 | 18 | //求得多个sensor的配置的集合。 19 | // Creates the mapping from frame_id to Sensors defined in 20 | // 'parameter_dictionary'. 21 | proto::Configuration CreateConfiguration( 22 | common::LuaParameterDictionary* parameter_dictionary); 23 | 24 | //系统是否支持某一传感器。 25 | // Returns true if 'frame_id' is mentioned in 'sensor_configuration'. 26 | bool IsEnabled(const string& frame_id, 27 | const sensor::proto::Configuration& sensor_configuration); 28 | 29 | //将sensor采集的data经过3d坐标变换为机器坐标。 30 | // Returns the transform which takes data from the sensor frame to the 31 | // tracking frame. 32 | transform::Rigid3d GetTransformToTracking( 33 | const string& frame_id, 34 | const sensor::proto::Configuration& sensor_configuration); 35 | 36 | } // namespace sensor 37 | } // namespace cartographer 38 | 39 | #endif // CARTOGRAPHER_SENSOR_CONFIGURATION_H_ 40 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/data.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_MAPPING_DATA_H_ 3 | #define CARTOGRAPHER_MAPPING_DATA_H_ 4 | 5 | #include "cartographer/common/time.h" 6 | #include "cartographer/sensor/point_cloud.h" 7 | #include "cartographer/sensor/range_data.h" 8 | #include "cartographer/transform/rigid_transform.h" 9 | 10 | namespace cartographer { 11 | namespace sensor { 12 | 13 | 14 | /* 15 | Data是针对某一类的传感器的数据的封装, 16 | 数据成员含: 17 | 1,传感器类型,包括imu，雷达，里程计 18 | 2,测量时间,time 19 | 3,Imu测量值,(imu:惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置。) 20 | 4,rangefinder,测距仪, 21 | 5,odometer_pose,里程计,指(如装在汽车上的)测量行程的装置。 22 | 23 | 构造函数有3个,每一类传感器对应一个构造函数. 24 | 25 | 26 | */ 27 | // This type is a logical union, i.e. only one type of sensor data is actually 28 | // filled in. It is only used for time ordering sensor data before passing it 29 | // on. 30 | struct Data { 31 | enum class Type { kImu, kRangefinder, kOdometer }; 32 | 33 | struct Imu { 34 | Eigen::Vector3d linear_acceleration; //线性加速度,m/s2 35 | Eigen::Vector3d angular_velocity; //角速度, rad/s 36 | }; 37 | 38 | //测距仪.基本原理是向待测距的物体发射激光脉冲并开始计时，接收到反射光时停止计时。这段时间即可以转换为激光器与目标之间的距离。 39 | struct Rangefinder { 40 | Eigen::Vector3f origin; //sensor的位姿 41 | PointCloud ranges; //测距点云 42 | }; 43 | 44 | Data(const common::Time time, const Imu& imu) //传感器类型是imu 45 | : type(Type::kImu), time(time), imu(imu) {} 46 | 47 | Data(const common::Time time, const Rangefinder& rangefinder)//传感器类型是雷达 48 | : type(Type::kRangefinder), time(time), rangefinder(rangefinder) {} 49 | 50 | Data(const common::Time time, const transform::Rigid3d& odometer_pose) 51 | //传感器类型是里程计 52 | : type(Type::kOdometer), time(time), odometer_pose(odometer_pose) {} 53 | 54 | Type type; 55 | common::Time time; 56 | Imu imu; 57 | Rangefinder rangefinder; 58 | transform::Rigid3d odometer_pose; 59 | }; 60 | 61 | } // namespace sensor 62 | } // namespace cartographer 63 | 64 | #endif // CARTOGRAPHER_MAPPING_DATA_H_ 65 | 66 | /* 67 | rigidbody:刚体 68 | */ -------------------------------------------------------------------------------- /cartographer_annotations/sensor/point_cloud.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/sensor/point_cloud.h" 3 | 4 | #include "cartographer/sensor/proto/sensor.pb.h" 5 | #include "cartographer/transform/transform.h" 6 | 7 | namespace cartographer { 8 | namespace sensor { 9 | 10 | 11 | /*. 12 | 功能：根据3D变换，转换点云, 13 | 返回：转换后的新点云结果，原点云未变。 14 | 15 | */ 16 | PointCloud TransformPointCloud(const PointCloud& point_cloud, 17 | const transform::Rigid3f& transform) { 18 | PointCloud result; //vector,元素是3*1f 19 | result.reserve(point_cloud.size()); //分配内存 20 | for (const Eigen::Vector3f& point : point_cloud) { 21 | result.emplace_back(transform * point); //C=A*B //A是Rigid3f,B是3*1f 22 | } 23 | return result; 24 | } 25 | 26 | 27 | /* 28 | 功能：按范围丢弃点云，去掉z轴区域外的点云 29 | 返回：新的符合要求的点云。 30 | */ 31 | PointCloud Crop(const PointCloud& point_cloud, const float min_z, 32 | const float max_z) { 33 | PointCloud cropped_point_cloud; 34 | for (const auto& point : point_cloud) { 35 | if (min_z <= point.z() && point.z() <= max_z) {//只保留在minz~maxz之间的点云 36 | cropped_point_cloud.push_back(point); 37 | } 38 | } 39 | return cropped_point_cloud; 40 | } 41 | 42 | //序列化 43 | proto::PointCloud ToProto(const PointCloud& point_cloud) { 44 | proto::PointCloud proto; 45 | for (const auto& point : point_cloud) { 46 | proto.add_x(point.x()); 47 | proto.add_y(point.y()); 48 | proto.add_z(point.z()); 49 | } 50 | return proto; 51 | } 52 | 53 | //反序列化 54 | PointCloud ToPointCloud(const proto::PointCloud& proto) { 55 | PointCloud point_cloud; 56 | const int size = std::min({proto.x_size(), proto.y_size(), proto.z_size()}); 57 | point_cloud.reserve(size);//最小，否则以下语句有bug 58 | for (int i = 0; i != size; ++i) { 59 | point_cloud.emplace_back(proto.x(i), proto.y(i), proto.z(i)); 60 | } 61 | return point_cloud; 62 | } 63 | 64 | } // namespace sensor 65 | } // namespace cartographer 66 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/point_cloud.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 3 | #define CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 4 | 5 | #include 6 | 7 | #include "Eigen/Core" 8 | #include "cartographer/sensor/proto/sensor.pb.h" 9 | #include "cartographer/transform/rigid_transform.h" 10 | #include "glog/logging.h" 11 | 12 | /* 13 | 点云数据是指在一个三维坐标系统中的一组向量的集合。{p1,p2,p3,...} 14 | 这些向量通常以X,Y,Z三维坐标的形式表示p:{x,y,z}， 15 | 而且一般主要用来代表一个物体的外表面形状。 16 | 除{x,y,z}可以代表的几何位置信息之外， 17 | 点云数据还可以表示一个点的RGB颜色，灰度值，深度，分割结果等。 18 | 19 | Eg..Pi={Xi, Yi, Zi,…….}表示空间中的一个点， 20 | 则Point Cloud={P1, P2, P3,…..Pn}表示一组点云数据。 21 | 22 | cartographer的PointCloud是由Vector3f组成的vector。 23 | PointCloudWithIntensities则是由点云和光线强度组成的struct类。 24 | 25 | 26 | */ 27 | namespace cartographer { 28 | namespace sensor { 29 | 30 | typedef std::vector PointCloud;//vector，元素是3*1f 31 | 32 | struct PointCloudWithIntensities { //点云+光线强度,{x,y,z}+intensity 33 | PointCloud points; //3*1的vector 34 | std::vector intensities; 35 | }; 36 | 37 | // Transforms 'point_cloud' according to 'transform'. 根据三维网格参数转换点云 38 | PointCloud TransformPointCloud(const PointCloud& point_cloud, 39 | const transform::Rigid3f& transform); 40 | 41 | 42 | /*去掉z轴区域外的点云,返回一个新的点云*/ 43 | // Returns a new point cloud without points that fall outside the region defined 44 | // by 'min_z' and 'max_z'. 45 | PointCloud Crop(const PointCloud& point_cloud, float min_z, float max_z); 46 | 47 | //序列化 48 | // Converts 'point_cloud' to a proto::PointCloud. 49 | proto::PointCloud ToProto(const PointCloud& point_cloud); 50 | 51 | //反序列化 52 | // Converts 'proto' to a PointCloud. 53 | PointCloud ToPointCloud(const proto::PointCloud& proto); 54 | 55 | } // namespace sensor 56 | } // namespace cartographer 57 | 58 | #endif // CARTOGRAPHER_SENSOR_POINT_CLOUD_H_ 59 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/point_cloud_test.cc: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "cartographer/sensor/point_cloud.h" 4 | #include "cartographer/transform/transform.h" 5 | 6 | #include 7 | 8 | #include "gtest/gtest.h" 9 | 10 | namespace cartographer { 11 | namespace sensor { 12 | namespace { 13 | 14 | TEST(PointCloudTest, TransformPointCloud) { 15 | PointCloud point_cloud; 16 | point_cloud.emplace_back(0.5f, 0.5f, 1.f); //构造一个点云{0.5,0.5,1} 17 | point_cloud.emplace_back(3.5f, 0.5f, 42.f); //{3.5,0.5,42} 18 | 19 | //调用static Rigid2 Rotation(const double rotation) 20 | const PointCloud transformed_point_cloud = TransformPointCloud( 21 | point_cloud, transform::Embed3D(transform::Rigid2f::Rotation(M_PI_2))); 22 | 23 | /*绕z轴逆时针旋转 pi/2: 24 | [x',y',x']=[]*[x,y,z] 25 | 化简后： x=-y，y=x，z=z 26 | */ 27 | EXPECT_NEAR(-0.5f, transformed_point_cloud[0].x(), 1e-6); 28 | EXPECT_NEAR(0.5f, transformed_point_cloud[0].y(), 1e-6); 29 | EXPECT_NEAR(-0.5f, transformed_point_cloud[1].x(), 1e-6); 30 | EXPECT_NEAR(3.5f, transformed_point_cloud[1].y(), 1e-6); 31 | } 32 | 33 | } // namespace 34 | } // namespace sensor 35 | } // namespace cartographer 36 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/proto/adaptive_voxel_filter_options.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.sensor.proto; 18 | 19 | message AdaptiveVoxelFilterOptions { 20 | // 'max_length' of a voxel edge. 21 | optional float max_length = 1; 22 | 23 | //不少于min_num 24 | // If there are more points and not at least 'min_num_points' remain, the 25 | // voxel length is reduced trying to get this minimum number of points. 26 | optional float min_num_points = 2; 27 | 28 | // Points further away from the origin are removed. 29 | optional float max_range = 3; 30 | } 31 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/proto/configuration.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.sensor.proto; 18 | 19 | import "cartographer/transform/proto/transform.proto"; 20 | 21 | // NEXT_ID: 16 22 | message Configuration { 23 | // NEXT_ID: 4 24 | message Sensor { 25 | // 'frame_id' of the sensor. 26 | optional string frame_id = 2; 27 | 28 | // Transformation from 'frame_id' to the tracking frame. 29 | optional transform.proto.Rigid3d transform = 3; 30 | } 31 | 32 | repeated Sensor sensor = 15; 33 | } 34 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/proto/sensor.proto: -------------------------------------------------------------------------------- 1 | // Copyright 2016 The Cartographer Authors 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | syntax = "proto2"; 16 | 17 | package cartographer.sensor.proto; 18 | 19 | option java_outer_classname = "Sensor"; 20 | 21 | import "cartographer/transform/proto/transform.proto"; 22 | 23 | // Collection of 3D 'points'. 24 | message PointCloud { //点云,zyx 25 | // Points as repeated floats for efficiency. All fields have the same size. float占4位,double占8位 26 | repeated float x = 3 [packed = true]; 27 | repeated float y = 4 [packed = true]; 28 | repeated float z = 5 [packed = true]; 29 | } 30 | 31 | // Compressed variant of PointCloud. 32 | message CompressedPointCloud { //压缩:点云数量+点云值 33 | optional int32 num_points = 1; 34 | repeated int32 point_data = 3 [packed = true]; 35 | } 36 | 37 | // Proto representation of ::cartographer::sensor::RangeData 38 | message RangeData { 39 | optional transform.proto.Vector3f origin = 1;//sensor坐标 40 | optional PointCloud point_cloud = 2; 41 | optional PointCloud missing_echo_point_cloud = 3; 42 | } 43 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/range_data_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/sensor/range_data.h" 3 | 4 | #include 5 | #include 6 | 7 | #include "gmock/gmock.h" 8 | 9 | namespace cartographer { 10 | namespace sensor { 11 | namespace { 12 | 13 | using ::testing::Contains; 14 | using ::testing::PrintToString; 15 | 16 | // Custom matcher for Eigen::Vector3f entries. 17 | MATCHER_P(ApproximatelyEquals, expected, 18 | string("is equal to ") + PrintToString(expected)) { 19 | return (arg - expected).isZero(0.001f); 20 | } 21 | 22 | TEST(RangeDataTest, Compression) { 23 | const std::vector returns = {Eigen::Vector3f(0, 1, 2), 24 | Eigen::Vector3f(4, 5, 6), 25 | Eigen::Vector3f(0, 1, 2)}; 26 | //构造一个测量数据 27 | const RangeData range_data = { 28 | Eigen::Vector3f(1, 1, 1), returns, {Eigen::Vector3f(7, 8, 9)}}; 29 | //压缩再解压，减少空间占用。 30 | const RangeData actual = Decompress(Compress(range_data)); 31 | 32 | //isApprox():true if *this is approximately equal to other, within the precision determined by prec. 33 | //压缩前后，精度比较 34 | EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6)); 35 | 36 | 37 | EXPECT_EQ(3, actual.returns.size()); 38 | 39 | EXPECT_EQ(1, actual.misses.size()); 40 | 41 | EXPECT_TRUE(actual.misses[0].isApprox(Eigen::Vector3f(7, 8, 9), 0.001f)); 42 | 43 | // Returns will be reordered, so we compare in an unordered manner. 44 | EXPECT_EQ(3, actual.returns.size()); 45 | 46 | //是否包含给定值，returns是乱序的。所以使用Contains()函数。 47 | EXPECT_THAT(actual.returns, 48 | Contains(ApproximatelyEquals(Eigen::Vector3f(0, 1, 2)))); 49 | EXPECT_THAT(actual.returns, 50 | Contains(ApproximatelyEquals(Eigen::Vector3f(4, 5, 6)))); 51 | } 52 | 53 | } // namespace 54 | } // namespace sensor 55 | } // namespace cartographer 56 | -------------------------------------------------------------------------------- /cartographer_annotations/sensor/voxel_filter_test.cc: -------------------------------------------------------------------------------- 1 | 2 | #include "cartographer/sensor/voxel_filter.h" 3 | 4 | #include 5 | 6 | #include "gmock/gmock.h" 7 | 8 | namespace cartographer { 9 | namespace sensor { 10 | namespace { 11 | 12 | using ::testing::ContainerEq; 13 | 14 | TEST(VoxelFilterTest, ReturnsTheFirstPointInEachVoxel) { 15 | PointCloud point_cloud = {{0.f, 0.f, 0.f}, 16 | {0.1f, -0.1f, 0.1f}, 17 | {0.3f, -0.1f, 0.f}, 18 | {0.f, 0.f, 0.1f}}; 19 | EXPECT_THAT(VoxelFiltered(point_cloud, 0.3f),//按照max_range滤波, 20 | ContainerEq(PointCloud{point_cloud[0], point_cloud[2]})); 21 | } 22 | 23 | } // namespace 24 | } // namespace sensor 25 | } // namespace cartographer 26 | 27 | /* 28 | ContainerEq: 29 | The same as Eq(container) except that the failure message also 30 | includes which elements are in one container but not the other. 31 | 32 | */ -------------------------------------------------------------------------------- /cartographer_annotations/transform/rigid_transform_test_helpers.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 3 | #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 4 | 5 | #include 6 | 7 | #include "Eigen/Core" 8 | #include "Eigen/Geometry" 9 | #include "cartographer/common/port.h" 10 | #include "cartographer/transform/rigid_transform.h" 11 | #include "gmock/gmock.h" 12 | #include "gtest/gtest.h" 13 | 14 | namespace cartographer { 15 | namespace transform { 16 | 17 | /* 18 | 本文件用于辅助gtest 19 | 20 | Eigen::Translation:Represents an homogeneous transformation in a N dimensional space. 21 | */ 22 | 23 | // 24 | template 25 | Eigen::Transform ToEigen(const Rigid2& rigid2) { 26 | return Eigen::Translation(rigid2.translation()) * rigid2.rotation(); 27 | } 28 | 29 | template 30 | Eigen::Transform ToEigen(const Rigid3& rigid3) { 31 | return Eigen::Translation(rigid3.translation()) * rigid3.rotation(); 32 | } 33 | /*gmock模拟对象行为*/ 34 | MATCHER_P2(IsNearly, rigid, epsilon, 35 | string(string(negation ? "isn't" : "is", " nearly ") + 36 | rigid.DebugString())) { 37 | return ToEigen(arg).isApprox(ToEigen(rigid), epsilon); 38 | } 39 | 40 | } // namespace transform 41 | } // namespace cartographer 42 | 43 | #endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_TEST_HELPERS_H_ 44 | -------------------------------------------------------------------------------- /champion_nav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(champion_nav_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | nav_msgs 9 | ) 10 | 11 | 12 | #message 13 | add_message_files( 14 | FILES 15 | ChampionNavLaserScan.msg 16 | ) 17 | 18 | 19 | #message dependence 20 | generate_messages( 21 | DEPENDENCIES 22 | std_msgs 23 | geometry_msgs 24 | nav_msgs 25 | ) 26 | 27 | 28 | catkin_package( 29 | # INCLUDE_DIRS include 30 | # LIBRARIES serial 31 | CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs nav_msgs message_runtime 32 | # DEPENDS system_lib 33 | 34 | ) 35 | -------------------------------------------------------------------------------- /champion_nav_msgs/msg/ChampionNavLaserScan.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 angle_min 3 | float32 angle_max 4 | float32 scan_time 5 | float32 time_increment 6 | float32 range_min 7 | float32 range_max 8 | float32[] ranges 9 | float32[] angles 10 | float32[] intensities 11 | -------------------------------------------------------------------------------- /champion_nav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | champion_nav_msgs 3 | 1.12.12 4 | 5 | 6 | Holds the action description and relevant messages for the move_base package 7 | 8 | 9 | zengqi 10 | zq 11 | zq 12 | BSD 13 | http://wiki.ros.org/move_base_msgs 14 | 15 | catkin 16 | 17 | actionlib_msgs 18 | geometry_msgs 19 | 　　　　nav_msgs 20 | message_generation 21 | roscpp 22 | rospy 23 | std_msgs 24 | PoseWithPrecision_msg 25 | GraphConstraint_msg 26 | Edge_msg 27 | Node_msg 28 | 29 | roscpp 30 | rospy 31 | std_msgs 32 | actionlib_msgs 33 | geometry_msgs 34 | 　　　　nav_msgs 35 | message_runtime 36 | PoseWithPrecision_msg 37 | GraphConstraint_msg 38 | Edge_msg 39 | Node_msg 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /gmapping/README.assets/1555126014034.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/gmapping/README.assets/1555126014034.png -------------------------------------------------------------------------------- /gmapping/README.assets/gmapping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/gmapping/README.assets/gmapping.png -------------------------------------------------------------------------------- /gmapping/bag/gmapping.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/gmapping/bag/gmapping.bag -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/.gitignore: -------------------------------------------------------------------------------- 1 | *.d 2 | *.o 3 | bin/ 4 | lib/ 5 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package openslam_gmapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.2 (2016-04-23) 6 | ------------------ 7 | * better Windows compilation 8 | This is taken from `#9 `_ which can now be closed. 9 | * fix a few more graphics stuff for Qt5 10 | * get GUI back in shape for those interested 11 | * use srand instead of srand48 12 | srand48 is non-standard and we are using a seed that is an 13 | unsigned int so we might as well use srand 14 | * Contributors: Vincent Rabaud 15 | 16 | 0.1.1 (2015-06-25) 17 | ------------------ 18 | * fix cppcheck warnings 19 | * License from BSD to CC 20 | * Contributors: Isaac IY Saito, Vincent Rabaud 21 | 22 | 0.1.0 (2013-06-28 17:33:53 -0700) 23 | --------------------------------- 24 | - Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 25 | - Catkinized and prepared for release into the ROS ecosystem 26 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(openslam_gmapping) 3 | 4 | find_package(catkin) 5 | include_directories(${catkin_INCLUDE_DIRS}) 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | LIBRARIES gridfastslam scanmatcher sensor_base sensor_range sensor_odometry utils 9 | ) 10 | 11 | include_directories(include) 12 | 13 | add_subdirectory(gridfastslam) 14 | add_subdirectory(scanmatcher) 15 | add_subdirectory(sensor) 16 | add_subdirectory(utils) 17 | 18 | if(0) 19 | add_subdirectory(gui) 20 | endif() 21 | 22 | 23 | 24 | 25 | install(DIRECTORY include/ 26 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 27 | FILES_MATCHING PATTERN "*.h" 28 | PATTERN "*.hxx" 29 | PATTERN ".svn" EXCLUDE 30 | ) 31 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/README: -------------------------------------------------------------------------------- 1 | This is a fork from gmapping at https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 2 | It includes a few patches that could be pushed upstream if needed 3 | 4 | 5 | 6 | 7 | -----------GMapping开发文档------------ 8 | 2017-03-31 ～ 2017-04-01 9 | 1.把gmapping算法过程中的运动学更新(motion model draw)、scan-match、resample都用OpenMP修改成为并行模式。 10 | 2.删除了在registerScan()函数前必须调用computeActiveArea()函数的操作。直接在registerScan()函数的开头直接调用computeActiveArea()。 11 | 3.修改了Particle的结构体和构造函数 在里面加入了一个低分辨率的地图。为加入CSM做准备 12 | 13 | 14 | 2017-04-08 15 | 今天把GMapping中的scan-match部分由前面的梯度下降替换为了IcpOptimization()，效果还有待测试 16 | 理论上来说替换成ICP是一个比前面的拟梯度下降更合理的选择 17 | 18 | 2017-04-10 19 | 替换成ICP之后，效果变得更差了，不知道为什么.所以暂时还是换回GMapping自带的hill-climb的优化形式 20 | 21 | 2017-05-06 22 | 去掉了03-31~04~01的用低分辨率地图来进行CSM的方式 23 | 仿照karto的方式来进行CSM，维护一个running_scans。需要的时候从这里面生成localmap来进行CSM的匹配。 24 | 目前只是把代码修改完毕了，还没有进行实验 25 | 26 | 2017-05-08 27 | 在代码中加入了isChampionLaser这个参数，用来选择消息数据类型。 28 | 如果isChampionLaser这个设置为true,那么说明选择的激光数据格式为我们自己定义的ChampionNavLaserScan模式 29 | 这个提升效果不仅仅是在gmapping里面可以提升，在任何的SLAM算法里面都可以提升。 30 | 因为我们通过运动畸变矫正，极大的去除了机器人的运动畸变。提高了低成本激光雷达的畸变效果。 31 | 32 | 2017-05-10 33 | 通过实验得知，在同样使用rplidar a2的情况下，使用经过矫正的ChampionNavLaserScan建出来的办公室的地图， 34 | 明显比rplidar原本使用的ROS自带的消息类型格式构建出来的地图要好很多。但是并没有好太多。 35 | 36 | 下一步的优化准确在激光数据中提取直线，然后把这些直线上的激光点通过线性插值重新排列，以此来去除激光点的噪声造成的建图出来的墙壁比较厚的缺点。 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/build_tools/Makefile.subdirs: -------------------------------------------------------------------------------- 1 | export VERBOSE 2 | 3 | .PHONY: clean, all 4 | 5 | ifeq ($(VERBOSE), 0) 6 | QUIET=--no-print-directory 7 | endif 8 | 9 | all: 10 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done 11 | 12 | clean: 13 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done 14 | 15 | dep: 16 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done 17 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/build_tools/generate_shared_object: -------------------------------------------------------------------------------- 1 | #!/bin/tcsh 2 | 3 | echo decompressing file $1 4 | 5 | set FILELIST=`ar -t $1` 6 | echo "Object files:" 7 | foreach i ($FILELIST) 8 | echo $i 9 | end 10 | 11 | echo generating $1:r.so 12 | 13 | ar -x $1 14 | ld -shared -o $1:r.so $FILELIST 15 | 16 | rm $FILELIST 17 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/build_tools/message: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | #echo "message: verbose = $VERBOSE" 4 | 5 | if ($VERBOSE) 6 | then 7 | exit 0; 8 | fi 9 | 10 | a=$MAKELEVEL 11 | 12 | while ((0<$a)); do echo -n " "; let "a = $a - 1";done 13 | 14 | echo $1 15 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/build_tools/pretty_compiler: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | 4 | #echo "pretty: verbose = $VERBOSE" 5 | 6 | if ($VERBOSE) 7 | then 8 | echo $1; 9 | if ! eval $1 10 | then 11 | echo "Failed command was:" 12 | echo $1 13 | echo "in directory " `pwd` 14 | exit 1 15 | fi 16 | else 17 | if ! eval $1 18 | then 19 | echo "Failed command was:" 20 | echo $1 21 | echo "in directory " `pwd` 22 | exit 1 23 | fi 24 | fi 25 | 26 | exit 0 27 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/build_tools/testlib: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [ -z "$1" ]; then 3 | echo "Syntax: rtestlib " 4 | exit 1 5 | fi 6 | 7 | exit 0 8 | 9 | FNAME=`mktemp rtestlibXXXXXX` 10 | echo "int main() { return 0; }" > $FNAME.cpp 11 | 12 | g++ $1 $FNAME.cpp -o $FNAME 13 | result=$? 14 | rm -f $FNAME.cpp $FNAME 15 | 16 | exit $result 17 | 18 | #if g++ $1 $FNAME.cpp -o $FNAME 19 | #then# 20 | # rm -f $FNAME.cpp $FNAME 21 | # exit 1 22 | #else 23 | # rm -f $FNAME.cpp $FNAME 24 | # exit 0 25 | #fi 26 | 27 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gfs-carmen/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= gfs-carmen 3 | 4 | LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile 5 | CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include 6 | 7 | -include ../global.mk 8 | ifeq ($(CARMENSUPPORT), 0) 9 | APPS= 10 | .PHONY: clean all 11 | 12 | all: 13 | 14 | clean: 15 | 16 | else 17 | -include ../build_tools/Makefile.app 18 | endif 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/grid/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= map_test 3 | 4 | LDFLAGS+= 5 | CPPFLAGS+= -DNDEBUG 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.app 9 | 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/grid/graphmap.cpp: -------------------------------------------------------------------------------- 1 | #ifndef GRAPHMAP_H 2 | #define GRAPHMAP_H 3 | #include 4 | #include "../include/gmapping/utils/point.h" 5 | #include "../include/gmapping/grid/map.h" 6 | #include 7 | 8 | 9 | 10 | 11 | /* 12 | * 这个地方感觉像是用用来做图优化的。 13 | * 不过不知道为什么最终没有用来做这个。 14 | */ 15 | namespace GMapping { 16 | 17 | class RasterMap; 18 | 19 | struct GraphMapPatch 20 | { 21 | typedef typename std::list PointList; 22 | /**Renders the map relatively to the center of the patch*/ 23 | //void render(RenderMap rmap); 24 | /**returns the lower left corner of the patch, relative to the center*/ 25 | //Point minBoundary() const; 26 | /**returns the upper right corner of the patch, relative to the center*/ 27 | //Point maxBoundary() const; // 28 | 29 | OrientedPoint center; 30 | PointList m_points; 31 | }; 32 | 33 | struct Covariance3 34 | { 35 | double sxx, sxy, sxt, syy, syt ,stt; 36 | }; 37 | 38 | struct GraphMapEdge 39 | { 40 | Covariance3 covariance; 41 | GraphMapPatch* first, *second; 42 | inline operator double() const 43 | { 44 | return sqrt((first->center-second->center)*(first->center-second->center)); 45 | } 46 | }; 47 | 48 | 49 | struct GraphPatchGraph: public Graph 50 | { 51 | void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance); 52 | }; 53 | 54 | void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, 55 | const Covariance3& cov) 56 | { 57 | GraphMapEdge gme; 58 | gme.covariance=cov; 59 | gme.first=v1; 60 | gme.second=v2; 61 | return Graph::addEdge(v1,v2,gme); 62 | } 63 | 64 | struct GraphPatchDirectoryCell: public std::set 65 | { 66 | GraphPatchDirectoryCell(double); 67 | }; 68 | 69 | typedef Map, Array2D::set > 70 | 71 | }; 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/grid/map_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "map.h" 3 | #include "harray2d.h" 4 | 5 | using namespace std; 6 | using namespace GMapping; 7 | 8 | struct SimpleCell{ 9 | int value; 10 | SimpleCell(int v=0){value=v;} 11 | static const SimpleCell& Unknown(); 12 | static SimpleCell* address; 13 | }; 14 | 15 | SimpleCell* SimpleCell::address=0; 16 | 17 | const SimpleCell& SimpleCell::Unknown(){ 18 | if (address) 19 | return *address; 20 | address=new SimpleCell(-1); 21 | return *address; 22 | } 23 | 24 | typedef Map< SimpleCell, HierarchicalArray2D > CGrid; 25 | 26 | int main (int argc, char ** argv){ 27 | CGrid g1(Point(0.,0.), 200, 200, 0.1); 28 | CGrid g2(Point(10.,10.), 200, 200, 0.1); 29 | { 30 | HierarchicalArray2D::PointSet ps; 31 | IntPoint pp=g1.world2map(Point(5.1,5.1)); 32 | cout << pp.x << " " << pp.y << endl; 33 | ps.insert(pp); 34 | g1.storage().setActiveArea(ps,false); 35 | g1.storage().allocActiveArea(); 36 | g1.cell(Point(5.1,5.1)).value=5; 37 | cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; 38 | g1.resize(-150, -150, 150, 150); 39 | cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; 40 | CGrid g3(g1); 41 | g1=g2; 42 | } 43 | cerr << "copy and modify test" << endl; 44 | CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1); 45 | CGrid* gp0=new CGrid(*gp1); 46 | for (int i=1; i<10; i++){ 47 | ap=new CGrid(*gp1); 48 | delete gp1; 49 | gp1=gp0; 50 | gp0=ap; 51 | IntPoint pp=gp0->world2map(Point(5.1,5.1)); 52 | HierarchicalArray2D::PointSet ps; 53 | ps.insert(pp); 54 | gp1->storage().setActiveArea(ps,false); 55 | gp1->storage().allocActiveArea(); 56 | gp1->cell(Point(5.1,5.1)).value=i; 57 | cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl; 58 | } 59 | delete gp0; 60 | delete gp1; 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(gridfastslam 2 | gfsreader.cpp 3 | gridslamprocessor.cpp 4 | gridslamprocessor_tree.cpp 5 | motionmodel.cpp 6 | ) 7 | target_link_libraries(gridfastslam scanmatcher sensor_range) 8 | 9 | install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gfsreader.h" 9 | 10 | #define MAX_LINE_LENGHT (1000000) 11 | 12 | using namespace std; 13 | using namespace GMapping; 14 | using namespace GMapping::GFSReader; 15 | 16 | int main (int argc, const char * const * argv){ 17 | if (argc<3){ 18 | cout << "usage gfs2log [-err] [-neff] [-part] [-odom] " << endl; 19 | cout << " -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl; 20 | return -1; 21 | } 22 | bool err=0; 23 | bool neff=0; 24 | bool part=0; 25 | bool odom=0; 26 | // int particle_num; 27 | unsigned int c=1; 28 | if (!strcmp(argv[c],"-err")){ 29 | err=true; 30 | c++; 31 | } 32 | if (!strcmp(argv[c],"-neff")){ 33 | neff=true; 34 | c++; 35 | } 36 | if (!strcmp(argv[c],"-part")){ 37 | part=true; 38 | c++; 39 | } 40 | if (!strcmp(argv[c],"-odom")){ 41 | odom=true; 42 | c++; 43 | } 44 | ifstream is(argv[c]); 45 | if (!is){ 46 | cout << "could read file "<< endl; 47 | return -1; 48 | } 49 | c++; 50 | RecordList rl; 51 | rl.read(is); 52 | unsigned int bestidx=rl.getBestIdx(); 53 | cout << endl << "best index = " << bestidx<< endl; 54 | ofstream os(argv[c]); 55 | if (! os){ 56 | cout << "could write file "<< endl; 57 | return -1; 58 | } 59 | rl.printPath(os,bestidx,err,odom); 60 | if(part) 61 | rl.printLastParticles(os); 62 | os.close(); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | int main(int argc, char**argv){ 9 | if (argc<3){ 10 | cout << "usage gfs2neff " << endl; 11 | return -1; 12 | } 13 | ifstream is(argv[1]); 14 | if (!is){ 15 | cout << "could read file "<< endl; 16 | return -1; 17 | } 18 | ofstream os(argv[2]); 19 | if (! os){ 20 | cout << "could write file "<< endl; 21 | return -1; 22 | } 23 | unsigned int frame=0; 24 | double neff=0; 25 | while(is){ 26 | char buf[8192]; 27 | is.getline(buf, 8192); 28 | istringstream lineStream(buf); 29 | string recordType; 30 | lineStream >> recordType; 31 | if (recordType=="FRAME"){ 32 | lineStream>> frame; 33 | } 34 | if (recordType=="NEFF"){ 35 | lineStream>> neff; 36 | os << frame << " " << neff << endl; 37 | } 38 | } 39 | os.close(); 40 | } 41 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "gfsreader.h" 6 | 7 | using namespace std; 8 | using namespace GMapping; 9 | using namespace GMapping::GFSReader; 10 | 11 | 12 | int main(int argc, char ** argv){ 13 | if (argc<2){ 14 | cout << "usage gfs2stat " << endl; 15 | return 0; 16 | } 17 | ifstream is(argv[1]); 18 | if (!is){ 19 | cout << "no file found: " << argv[1] << endl; 20 | return 0; 21 | } 22 | ofstream os(argv[2]); 23 | if (!os){ 24 | cout << "cannot open file: " << argv[1] << endl; 25 | return 0; 26 | } 27 | cout << "loading... "<< flush; 28 | RecordList rl; 29 | rl.read(is); 30 | cout << " done" << endl; 31 | int count=-1; 32 | for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ 33 | 34 | count++; 35 | const ScanMatchRecord* rec=dynamic_cast(*it); 36 | if (!rec) 37 | continue; 38 | Gaussian3 gaussian; 39 | /* 40 | vector nweights; 41 | cout << "N"<< flush; 42 | back_insert_iterator< vector > out(nweights); 43 | toNormalForm(out,rec->weights.begin(), rec->weights.end()); 44 | cout << "G"<< flush; 45 | gaussian.computeFromSamples(rec->poses, nweights); 46 | */ 47 | gaussian.computeFromSamples(rec->poses); 48 | cout << "E"<< flush; 49 | os << count <<" "; 50 | os << gaussian.mean.x <<" "; 51 | os << gaussian.mean.y <<" "; 52 | os << gaussian.mean.theta <<" "; 53 | os << gaussian.covariance.eval[0] <<" "; 54 | os << gaussian.covariance.eval[1] <<" "; 55 | os << gaussian.covariance.eval[2] < 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gfsreader.h" 9 | #define MAX_LINE_LENGHT (1000000) 10 | 11 | using namespace std; 12 | using namespace GMapping; 13 | using namespace GMapping::GFSReader; 14 | 15 | computeBoundingBox() 16 | 17 | 18 | int main (unsigned int argc, const char * const * argv) 19 | double delta = 0.1; 20 | double skip = 2; 21 | double rotate = 0; 22 | double maxrange = 0; 23 | 24 | if (argc<3){ 25 | cout << "usage gfs2stream [-step Number] " << endl; 26 | return -1; 27 | } 28 | 29 | 30 | CMD_PARSE_BEGIN(1,argc-2); 31 | 32 | CMD_PARSE_END; 33 | 34 | if (argc<3){ 35 | cout << "usage gfs2stream [-step Number] " << endl; 36 | return -1; 37 | } 38 | bool err=0; 39 | bool neff=0; 40 | bool part=0; 41 | unsigned int c=1; 42 | if (!strcmp(argv[c],"-err")){ 43 | err=true; 44 | c++; 45 | } 46 | if (!strcmp(argv[c],"-neff")){ 47 | neff=true; 48 | c++; 49 | } 50 | if (!strcmp(argv[c],"-part")){ 51 | part=true; 52 | c++; 53 | } 54 | ifstream is(argv[c]); 55 | if (!is){ 56 | cout << "could read file "<< endl; 57 | return -1; 58 | } 59 | c++; 60 | RecordList rl; 61 | rl.read(is); 62 | unsigned int bestidx=rl.getBestIdx(); 63 | cout << endl << "best index = " << bestidx<< endl; 64 | ofstream os(argv[c]); 65 | if (! os){ 66 | cout << "could write file "<< endl; 67 | return -1; 68 | } 69 | rl.printPath(os,bestidx,err); 70 | if(part) 71 | rl.printLastParticles(os); 72 | os.close(); 73 | return 0; 74 | } 75 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Qt5Widgets) 2 | if(Qt5Widgets_FOUND) 3 | else() 4 | find_package(Qt4) 5 | include(${QT_USE_FILE}) 6 | endif() 7 | 8 | include_directories(../include/gmapping ../ ../include/gmapping/log/) 9 | 10 | #add_executable(gfs_nogui gfs_nogui.cpp) 11 | 12 | add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp) 13 | if(Qt5Widgets_FOUND) 14 | target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets) 15 | else() 16 | target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES}) 17 | endif() 18 | 19 | #add_executable(gfs2img gfs2img.cpp gridfastslam) 20 | 21 | #-include ../global.mk 22 | 23 | #OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o 24 | 25 | #APPS= gfs_nogui gfs_simplegui gfs2img 26 | #LDFLAGS+= $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread 27 | 28 | #ifeq ($(CARMENSUPPORT),1) 29 | #LDFLAGS+= -lcarmenwrapper 30 | #endif 31 | 32 | #CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include 33 | 34 | 35 | #-include ../build_tools/Makefile.generic-shared-object 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/gfs_logplayer.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #include 25 | #include "qparticleviewer.h" 26 | 27 | int main (int argc, char ** argv){ 28 | QApplication app(argc, argv); 29 | QParticleViewer * pviewer=new QParticleViewer(0); 30 | app.setMainWidget(pviewer); 31 | pviewer->show(); 32 | FILE* f=fopen(argv[1], "r"); 33 | if (!f) 34 | return -1; 35 | QTextIStream is(f); 36 | pviewer->tis=&is; 37 | pviewer->start(10); 38 | return app.exec(); 39 | std::cout << "DONE: " << argv[1] <fill(Qt::white); 8 | } 9 | 10 | void QMapPainter::resizeEvent(QResizeEvent * sizeev){ 11 | m_pixmap->resize(sizeev->size()); 12 | } 13 | 14 | QMapPainter::~QMapPainter(){ 15 | delete m_pixmap; 16 | } 17 | 18 | 19 | void QMapPainter::timerEvent(QTimerEvent * te) { 20 | if (te->timerId()==timer) 21 | update(); 22 | } 23 | 24 | void QMapPainter::start(int period){ 25 | timer=startTimer(period); 26 | } 27 | 28 | 29 | void QMapPainter::paintEvent ( QPaintEvent * ){ 30 | bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); 31 | } 32 | 33 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/qnavigatorwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QNAVIGATOR_WIDGET_H 2 | #define _QNAVIGATOR_WIDGET_H 3 | 4 | #include "qmappainter.h" 5 | #include "qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QNavigatorWidget : public QMapPainter{ 10 | public: 11 | QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QNavigatorWidget(); 13 | std::list trajectoryPoints; 14 | bool repositionRobot; 15 | GMapping::IntPoint robotPose; 16 | double robotHeading; 17 | bool confirmLocalization; 18 | bool enableMotion; 19 | bool startWalker; 20 | bool startGlobalLocalization; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool writeImages; 25 | QPixmapDumper dumper; 26 | bool drawRobot; 27 | 28 | protected: 29 | virtual void paintEvent ( QPaintEvent *paintevent ); 30 | virtual void mousePressEvent ( QMouseEvent * e ); 31 | virtual void keyPressEvent ( QKeyEvent * e ); 32 | }; 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/qpixmapdumper.cpp: -------------------------------------------------------------------------------- 1 | #include "qpixmapdumper.h" 2 | #include 3 | #include 4 | 5 | QPixmapDumper::QPixmapDumper(std::string p, int c){ 6 | format="PNG"; 7 | prefix=p; 8 | reset(); 9 | cycles=c; 10 | } 11 | 12 | void QPixmapDumper::reset(){ 13 | cycles=0; 14 | frame=0; 15 | counter=0; 16 | } 17 | 18 | #define filename_bufsize 1024 19 | 20 | bool QPixmapDumper::dump(const QPixmap& pixmap){ 21 | bool processed=false; 22 | if (!(counter%cycles)){ 23 | char buf[filename_bufsize]; 24 | sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str()); 25 | QImage image=pixmap.convertToImage(); 26 | image.save(QString(buf), format.c_str(),0); 27 | frame ++; 28 | } 29 | counter++; 30 | return processed; 31 | } 32 | 33 | 34 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/qpixmapdumper.h: -------------------------------------------------------------------------------- 1 | #ifndef _QPIXMAPDUMPER_H_ 2 | #define _QPIXMAPDUMPER_H_ 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | struct QPixmapDumper{ 9 | QPixmapDumper(std::string prefix, int cycles); 10 | void reset(); 11 | std::string prefix; 12 | std::string format; 13 | bool dump(const QPixmap& pixmap); 14 | int counter; 15 | int cycles; 16 | int frame; 17 | }; 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/gui/qslamandnavwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QSLAMANDNAV_WIDGET_H 2 | #define _QSLAMANDNAV_WIDGET_H 3 | 4 | #include "qmappainter.h" 5 | #include "qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QSLAMandNavWidget : public QMapPainter{ 10 | public: 11 | QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QSLAMandNavWidget(); 13 | std::list trajectoryPoints; 14 | GMapping::IntPoint robotPose; 15 | double robotHeading; 16 | 17 | bool slamRestart; 18 | bool slamFinished; 19 | bool enableMotion; 20 | bool startWalker; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool printHelp; 25 | bool saveGoalPoints; 26 | bool writeImages; 27 | bool drawRobot; 28 | QPixmapDumper dumper; 29 | 30 | 31 | protected: 32 | virtual void paintEvent ( QPaintEvent *paintevent ); 33 | virtual void mousePressEvent ( QMouseEvent * e ); 34 | virtual void keyPressEvent ( QKeyEvent * e ); 35 | }; 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h: -------------------------------------------------------------------------------- 1 | #ifndef ACCESSTATE_H 2 | #define ACCESSTATE_H 3 | 4 | namespace GMapping { 5 | enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; 6 | }; 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/gridfastslam/motionmodel.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONMODEL_H 2 | #define MOTIONMODEL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping { 9 | 10 | /*里程计运动模型*/ 11 | struct MotionModel{ 12 | 13 | /*给点当前坐标 里程计信息 求去新的位置*/ 14 | OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const; 15 | 16 | /*给点当前坐标 这一次的里程计信息 上一次的里程计信息 计算出来新的位置*/ 17 | OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const; 18 | 19 | Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; 20 | double srr, str, srt, stt; 21 | 22 | /* 23 | @srr 线性运动造成的线性误差的方差 24 | @srt 线性运动造成的角度误差的方差 25 | @str 旋转运动造成的线性误差的方差 26 | @stt 旋转运动造成的角度误差的方差 27 | */ 28 | }; 29 | 30 | }; 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/log/configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGURATION_H 2 | #define CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping { 8 | 9 | class Configuration{ 10 | public: 11 | virtual ~Configuration(); 12 | virtual SensorMap computeSensorMap() const=0; 13 | }; 14 | 15 | }; 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORLOG_H 2 | #define SENSORLOG_H 3 | 4 | #include 5 | #include 6 | #include "../sensor/sensor_base/sensorreading.h" 7 | #include "../sensor/sensor_odometry/odometrysensor.h" 8 | #include "../sensor/sensor_range/rangesensor.h" 9 | #include "../sensor/sensor_odometry/odometryreading.h" 10 | #include "../sensor/sensor_range/rangereading.h" 11 | #include "configuration.h" 12 | 13 | namespace GMapping 14 | { 15 | 16 | class SensorLog : public std::list{ 17 | public: 18 | SensorLog(const SensorMap&); 19 | ~SensorLog(); 20 | std::istream& load(std::istream& is); 21 | OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; 22 | protected: 23 | const SensorMap& m_sensorMap; 24 | OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; 25 | RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; 26 | }; 27 | 28 | }; 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_H 2 | #define SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping{ 8 | 9 | //传感器的基类 传入传感器的名字 10 | //这个基类的作用就是储存传感器的名字 11 | class Sensor 12 | { 13 | public: 14 | Sensor(const std::string& name=""); 15 | virtual ~Sensor(); 16 | inline std::string getName() const {return m_name;} 17 | inline void setName(const std::string& name) {m_name=name;} 18 | protected: 19 | std::string m_name; 20 | }; 21 | 22 | //传感器的地图，即车子上有多少传感器都可以存入地图里面 23 | typedef std::map SensorMap; 24 | 25 | }; //end namespace 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "sensor.h" 5 | namespace GMapping{ 6 | 7 | /* 8 | * 传感器数据类 9 | * 这个类也是一个基类 作用是存储传感器的名字+传感器读取的时间 10 | */ 11 | class SensorReading{ 12 | public: 13 | SensorReading(const Sensor* s=0, double time=0); 14 | virtual ~SensorReading(); 15 | inline double getTime() const {return m_time;} 16 | inline void setTime(double t) {m_time=t;} 17 | inline const Sensor* getSensor() const {return m_sensor;} 18 | protected: 19 | double m_time; 20 | const Sensor* m_sensor; 21 | 22 | }; 23 | 24 | }; //end namespace 25 | #endif 26 | 27 | 28 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYREADING_H 2 | #define ODOMETRYREADING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "odometrysensor.h" 8 | 9 | namespace GMapping{ 10 | 11 | //一帧里程计的信息 12 | class OdometryReading: public SensorReading{ 13 | public: 14 | OdometryReading(const OdometrySensor* odo, double time=0); 15 | inline const OrientedPoint& getPose() const {return m_pose;} 16 | inline const OrientedPoint& getSpeed() const {return m_speed;} 17 | inline const OrientedPoint& getAcceleration() const {return m_acceleration;} 18 | inline void setPose(const OrientedPoint& pose) {m_pose=pose;} 19 | inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;} 20 | inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;} 21 | 22 | protected: 23 | OrientedPoint m_pose; //位置 24 | OrientedPoint m_speed; //速度 25 | OrientedPoint m_acceleration; //加速度 26 | }; 27 | 28 | }; 29 | #endif 30 | 31 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYSENSOR_H 2 | #define ODOMETRYSENSOR_H 3 | 4 | #include 5 | #include "../sensor_base/sensor.h" 6 | 7 | namespace GMapping{ 8 | 9 | //里程计传感器 10 | class OdometrySensor: public Sensor{ 11 | public: 12 | OdometrySensor(const std::string& name, bool ideal=false); 13 | inline bool isIdeal() const { return m_ideal; } 14 | protected: 15 | bool m_ideal; //标明是否是理想的传感器 16 | }; 17 | 18 | }; 19 | 20 | #endif 21 | 22 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h: -------------------------------------------------------------------------------- 1 | #ifndef MACRO_PARAMS_H 2 | #define MACRO_PARAMS_H 3 | 4 | #define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\ 5 | qualifier: type m_##name;\ 6 | getqualifier: inline type get##name() const {return m_##name;}\ 7 | setqualifier: inline void set##name(type name) {m_##name=name;} 8 | 9 | #define PARAM_SET(type, name, qualifier, setqualifier)\ 10 | qualifier: type m_##name;\ 11 | setqualifier: inline void set##name(type name) {m_##name=name;} 12 | 13 | #define PARAM_GET(type, name, qualifier, getqualifier)\ 14 | qualifier: type m_##name;\ 15 | getqualifier: inline type get##name() const {return m_##name;} 16 | 17 | #define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 18 | getqualifier: inline type get##name() const {return member.get##name();}\ 19 | setqualifier: inline void set##name(type name) { member.set##name(name);} 20 | 21 | #define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ 22 | setqualifier: inline void set##name(type name) { member.set##name(name);} 23 | 24 | #define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 25 | getqualifier: inline type get##name() const {return member.get##name();} 26 | 27 | #define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 28 | getqualifier: inline type get##name() const {return member.name;}\ 29 | setqualifier: inline void set##name(type name) {member.name=name;} 30 | 31 | #define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ 32 | setqualifier: inline void set##name(type name) {member.name=name;} 33 | 34 | #define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 35 | getqualifier: inline type get##name() const {return member.name;}\ 36 | 37 | #define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val 38 | #endif 39 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o 2 | APPS= log_test log_plot scanstudio2carmen rdk2carmen 3 | 4 | LDFLAGS+= -lsensor_range -lsensor_odometry -lsensor_base 5 | CPPFLAGS+= -I../sensor 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.generic-shared-object 9 | 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/carmenconfiguration.h: -------------------------------------------------------------------------------- 1 | #ifndef CARMENCONFIGURATION_H 2 | #define CARMENCONFIGURATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "configuration.h" 10 | 11 | namespace GMapping { 12 | 13 | class CarmenConfiguration: public Configuration, public std::map >{ 14 | public: 15 | virtual std::istream& load(std::istream& is); 16 | virtual SensorMap computeSensorMap() const; 17 | }; 18 | 19 | }; 20 | 21 | #endif 22 | 23 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/configuration.cpp: -------------------------------------------------------------------------------- 1 | #include "configuration.h" 2 | 3 | namespace GMapping { 4 | 5 | Configuration::~Configuration(){ 6 | } 7 | 8 | }; 9 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/log_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace std; 9 | using namespace GMapping; 10 | 11 | int main(int argc, char ** argv){ 12 | if (argc<2){ 13 | cout << "usage log_test " << endl; 14 | exit (-1); 15 | } 16 | ifstream is(argv[1]); 17 | if (! is){ 18 | cout << "no file " << argv[1] << " found" << endl; 19 | exit (-1); 20 | } 21 | CarmenConfiguration conf; 22 | conf.load(is); 23 | 24 | SensorMap m=conf.computeSensorMap(); 25 | 26 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 27 | // cout << it->first << " " << it->second->getName() << endl; 28 | 29 | SensorLog log(m); 30 | is.close(); 31 | 32 | ifstream ls(argv[1]); 33 | log.load(ls); 34 | ls.close(); 35 | cerr << "log size" << log.size() << endl; 36 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 37 | RangeReading* rr=dynamic_cast(*it); 38 | if (rr){ 39 | //cerr << rr->getSensor()->getName() << " "; 40 | //cerr << rr->size()<< " "; 41 | //for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ 42 | // cerr << *it << " "; 43 | //} 44 | cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl; 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/rdk2carmen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace std; 9 | using namespace GMapping; 10 | 11 | int main(int argc, char ** argv){ 12 | if (argc<2){ 13 | cerr << "usage "< " << endl; 14 | cerr << "or "< for standard output" << endl; 15 | exit (-1); 16 | } 17 | ifstream is(argv[1]); 18 | if (! is){ 19 | cerr << "no file " << argv[1] << " found" << endl; 20 | exit (-1); 21 | } 22 | ostream *os; 23 | if (argc<3) 24 | os=&cout; 25 | else{ 26 | os=new ofstream(argv[2]); 27 | if (! os){ 28 | cerr << "no file " << argv[1] << " found" << endl; 29 | exit (-1); 30 | } 31 | } 32 | CarmenConfiguration conf; 33 | conf.load(is); 34 | 35 | SensorMap m=conf.computeSensorMap(); 36 | 37 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 38 | // cout << it->first << " " << it->second->getName() << endl; 39 | 40 | SensorLog log(m); 41 | is.close(); 42 | 43 | ifstream ls(argv[1]); 44 | log.load(ls); 45 | ls.close(); 46 | cerr << "log size" << log.size() << endl; 47 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 48 | RangeReading* rr=dynamic_cast(*it); 49 | if (rr){ 50 | *os << rr->getSensor()->getName() << " "; 51 | *os << rr->size()<< " "; 52 | for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ 53 | *os << (*it)*0.001 << " "; 54 | } 55 | *os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl; 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/log/scanstudio2carmen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define MAXLINELENGHT (10240) 9 | #define MAXREADINGS (10240) 10 | 11 | using namespace std; 12 | using namespace GMapping; 13 | 14 | int main (int argc, char** argv){ 15 | if (argc<3){ 16 | cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl; 17 | exit(1); 18 | } 19 | ifstream is(argv[1]); 20 | if (!is){ 21 | cout << "cannopt open file" << argv[1] << endl; 22 | exit(1); 23 | } 24 | 25 | ofstream os(argv[2]); 26 | 27 | double readings[MAXREADINGS]; 28 | OrientedPoint pose; 29 | int nbeams; 30 | while (is){ 31 | char buf[MAXLINELENGHT]; 32 | is.getline(buf,MAXLINELENGHT); 33 | istringstream st(buf); 34 | string token; 35 | st>>token; 36 | if (token=="RobotPos:"){ 37 | st >> pose.x >> pose.y >> pose.theta; 38 | pose.x/=1000; 39 | pose.y/=1000; 40 | } else 41 | if (token=="NumPoints:"){ 42 | st >> nbeams; 43 | assert(nbeams> angle; 50 | is >> readings[c]; 51 | readings[c]/=1000; 52 | c++; 53 | } 54 | if (c==nbeams) 55 | os << "FLASER " << nbeams << " "; 56 | c=0; 57 | while (c 5 | #include "sensorlog.h" 6 | 7 | namespace GMapping { 8 | class SensorStream{ 9 | public: 10 | SensorStream(const SensorMap& sensorMap); 11 | virtual ~SensorStream(); 12 | virtual operator bool() const=0; 13 | virtual bool rewind() = 0 ; 14 | virtual SensorStream& operator >>(const SensorReading*&) = 0; 15 | inline const SensorMap& getSensorMap() const {return m_sensorMap; } 16 | protected: 17 | const SensorMap& m_sensorMap; 18 | static SensorReading* parseReading(std::istream& is, const SensorMap& smap); 19 | static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ); 20 | static RangeReading* parseRange(std::istream& is, const RangeSensor* ); 21 | }; 22 | 23 | class InputSensorStream: public SensorStream{ 24 | public: 25 | InputSensorStream(const SensorMap& sensorMap, std::istream& is); 26 | virtual operator bool() const; 27 | virtual bool rewind(); 28 | virtual SensorStream& operator >>(const SensorReading*&); 29 | 30 | //virtual SensorStream& operator >>(SensorLog*& log); 31 | protected: 32 | std::istream& m_inputStream; 33 | }; 34 | 35 | class LogSensorStream: public SensorStream{ 36 | public: 37 | LogSensorStream(const SensorMap& sensorMap, const SensorLog* log); 38 | virtual operator bool() const; 39 | virtual bool rewind(); 40 | virtual SensorStream& operator >>(const SensorReading*&); 41 | protected: 42 | const SensorLog* m_log; 43 | SensorLog::const_iterator m_cursor; 44 | }; 45 | 46 | }; 47 | #endif 48 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | openslam_gmapping 3 | 0.1.2 4 | ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 5 | Vincent Rabaud 6 | CreativeCommons-by-nc-sa-2.0 7 | 8 | http://openslam.org/gmapping 9 | https://github.com/ros-perception/openslam_gmapping 10 | https://github.com/ros-perception/openslam_gmapping/issues 11 | 12 | Giorgio Grisetti 13 | Cyrill Stachniss 14 | Wolfram Burgard 15 | 16 | catkin 17 | 18 | 19 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(catkin REQUIRED COMPONENTS roscpp) 2 | include_directories(${catkin_INCLUDE_DIRS}) 3 | add_library(scanmatcher eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp) 4 | target_link_libraries(scanmatcher sensor_range utils) 5 | 6 | add_executable(icptest icptest.cpp) 7 | target_link_libraries(icptest ${catkin_LIBRARIES}) 8 | 9 | install(TARGETS scanmatcher DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/scanmatcher/correlationGrid.cpp: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/scanmatcher/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/scanmatcher/lumiles.h: -------------------------------------------------------------------------------- 1 | #ifndef LUMILESPROCESSOR 2 | #define LUMILESPROCESSOR 3 | 4 | namespace GMapping{ 5 | 6 | class LuMilesProcessor{ 7 | typedef std:vector PointVector; 8 | static OrientedPoint step(const PointVector& src, const PointVector& dest); 9 | }; 10 | 11 | OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){ 12 | assert(src.size()==dest.size()); 13 | unsigned int size=dest.size(); 14 | double smx=0, smy=0, dmx=0, dmy=0; 15 | for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){ 16 | smx+=it->x; 17 | smy+=it->y; 18 | } 19 | smx/=src.size(); 20 | smy/=src.size(); 21 | 22 | for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){ 23 | dmx+=it->x; 24 | dmy+=it->y; 25 | } 26 | dmx/=src.size(); 27 | dmy/=src.size(); 28 | 29 | double sxx=0, sxy=0; 30 | double syx=0, syy=0; 31 | for (unsigned int i=0; i 5 | #include 6 | #include 7 | //#include 8 | #include 9 | 10 | namespace GMapping { 11 | 12 | class ScanMatcherProcessor 13 | { 14 | public: 15 | ScanMatcherProcessor(const ScanMatcherMap& m); 16 | ScanMatcherProcessor (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta); 17 | virtual ~ScanMatcherProcessor (); 18 | virtual void processScan(const RangeReading & reading); 19 | void setSensorMap(const SensorMap& smap, std::string sensorName="FLASER"); 20 | void init(); 21 | void setMatchingParameters 22 | (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false); 23 | void setRegistrationParameters(double regScore, double critScore); 24 | OrientedPoint getPose() const; 25 | inline const ScanMatcherMap& getMap() const {return m_map;} 26 | inline ScanMatcher& matcher() {return m_matcher;} 27 | inline void setmaxMove(double mmove){m_maxMove=mmove;} 28 | bool useICP; 29 | protected: 30 | ScanMatcher m_matcher; 31 | bool m_computeCovariance; 32 | bool m_first; 33 | SensorMap m_sensorMap; 34 | double m_regScore, m_critScore; 35 | unsigned int m_beams; 36 | double m_maxMove; 37 | //state 38 | ScanMatcherMap m_map; 39 | OrientedPoint m_pose; 40 | OrientedPoint m_odoPose; 41 | int m_count; 42 | //gsl_eigen_symmv_workspace * m_eigenspace; 43 | }; 44 | 45 | }; 46 | 47 | #endif 48 | 49 | 50 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/scanmatcher/smmap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping { 4 | 5 | //地图中使用的栅格 6 | const PointAccumulator& PointAccumulator::Unknown() 7 | { 8 | if (! unknown_ptr) 9 | unknown_ptr=new PointAccumulator; 10 | return *unknown_ptr; 11 | } 12 | 13 | PointAccumulator* PointAccumulator::unknown_ptr=0; 14 | 15 | }; 16 | 17 | 18 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(sensor_base) 2 | add_subdirectory(sensor_odometry) 3 | add_subdirectory(sensor_range) 4 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/Makefile: -------------------------------------------------------------------------------- 1 | -include ../global.mk 2 | 3 | SUBDIRS=sensor_base sensor_odometry sensor_range 4 | 5 | -include ../build_tools/Makefile.subdirs 6 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(./) 2 | add_library(sensor_base sensor.cpp sensorreading.cpp) 3 | install(TARGETS sensor_base DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 4 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | //传感器基类的高沟函数 6 | Sensor::Sensor(const std::string& name){ 7 | m_name=name; 8 | } 9 | 10 | Sensor::~Sensor(){ 11 | } 12 | 13 | };// end namespace 14 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "sensor.h" 5 | namespace GMapping{ 6 | 7 | /*传感器读取的基类*/ 8 | class SensorReading{ 9 | public: 10 | SensorReading(const Sensor* s=0, double time=0); 11 | inline double getTime() const {return m_time;} 12 | inline const Sensor* getSensor() const {return m_sensor;} 13 | protected: 14 | double m_time; //时间 15 | const Sensor* m_sensor; //传感器 16 | }; 17 | 18 | }; //end namespace 19 | #endif 20 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | //构造函数 6 | SensorReading::SensorReading(const Sensor* s, double t){ 7 | m_sensor=s; 8 | m_time=t; 9 | } 10 | 11 | 12 | SensorReading::~SensorReading(){ 13 | } 14 | 15 | }; 16 | 17 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp) 2 | target_link_libraries(sensor_odometry sensor_base) 3 | 4 | install(TARGETS sensor_odometry DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 5 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | /*构造函数 用*/ 6 | OdometryReading::OdometryReading(const OdometrySensor* odo, double time): 7 | SensorReading(odo,time){} 8 | 9 | }; 10 | 11 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;} 6 | 7 | 8 | }; 9 | 10 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensor_range rangereading.cpp rangesensor.cpp) 2 | target_link_libraries(sensor_range sensor_base) 3 | 4 | install(TARGETS sensor_range DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 5 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | RangeSensor::RangeSensor(std::string name): Sensor(name){} 6 | 7 | /* 8 | beams_num 激光束的数量 9 | res 激光的角度解析度 10 | position 激光的位置 11 | span 表示激光束的发散角度(激光雷达为0) 12 | maxRange 激光束的最大距离 13 | */ 14 | RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), 15 | m_pose(position), m_beams(beams_num) 16 | { 17 | double angle=-.5*res*beams_num; 18 | for (unsigned int i=0; i 2 | #include "autoptr.h" 3 | 4 | using namespace std; 5 | using namespace GMapping; 6 | 7 | typedef autoptr DoubleAutoPtr; 8 | 9 | int main(int argc, const char * const * argv){ 10 | double* d1=new double(10.); 11 | double* d2=new double(20.); 12 | cout << "Construction test" << endl; 13 | DoubleAutoPtr pd1(d1); 14 | DoubleAutoPtr pd2(d2); 15 | cout << *pd1 << " " << *pd2 << endl; 16 | cout << "Copy Construction" << endl; 17 | DoubleAutoPtr pd3(pd1); 18 | cout << *pd3 << endl; 19 | cout << "assignment" << endl; 20 | pd3=pd2; 21 | pd1=pd2; 22 | cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl; 23 | cout << "conversion operator" << endl; 24 | DoubleAutoPtr nullPtr; 25 | cout << "conversion operator " << !(nullPtr) << endl; 26 | cout << "neg conversion operator " << nullPtr << endl; 27 | cout << "conversion operator " << (int)pd1 << endl; 28 | cout << "neg conversion operator " << !(pd1) << endl; 29 | } 30 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/utils/movement.h: -------------------------------------------------------------------------------- 1 | #ifndef FSRMOVEMENT_H 2 | #define FSRMOVEMENT_H 3 | 4 | #include 5 | 6 | namespace GMapping { 7 | 8 | /** fsr-movement (forward, sideward, rotate) **/ 9 | class FSRMovement { 10 | public: 11 | FSRMovement(double f=0.0, double s=0.0, double r=0.0); 12 | FSRMovement(const FSRMovement& src); 13 | FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2); 14 | FSRMovement(const FSRMovement& move1, const FSRMovement& move2); 15 | 16 | 17 | void normalize(); 18 | void invert(); 19 | void compose(const FSRMovement& move2); 20 | OrientedPoint move(const OrientedPoint& pt) const; 21 | 22 | 23 | /* static members */ 24 | 25 | static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1); 26 | 27 | static FSRMovement composeMoves(const FSRMovement& move1, 28 | const FSRMovement& move2); 29 | 30 | static FSRMovement moveBetweenPoints(const OrientedPoint& pt1, 31 | const OrientedPoint& pt2); 32 | 33 | static FSRMovement invertMove(const FSRMovement& move1); 34 | 35 | static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1, 36 | const OrientedPoint& reference_pt_frame2, 37 | const OrientedPoint& pt_frame1); 38 | 39 | public: 40 | double f; 41 | double s; 42 | double r; 43 | }; 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/utils/orientedboundingbox.h: -------------------------------------------------------------------------------- 1 | #ifndef ORIENTENDBOUNDINGBOX_H 2 | #define ORIENTENDBOUNDINGBOX_H 3 | 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace GMapping{ 11 | 12 | template 13 | class OrientedBoundingBox { 14 | 15 | public: 16 | OrientedBoundingBox(std::vector< point > p); 17 | double area(); 18 | 19 | protected: 20 | Point ul; 21 | Point ur; 22 | Point ll; 23 | Point lr; 24 | }; 25 | 26 | #include "orientedboundingbox.hxx" 27 | 28 | };// end namespace 29 | 30 | #endif 31 | 32 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/utils/printmemusage.cpp: -------------------------------------------------------------------------------- 1 | #include "printmemusage.h" 2 | 3 | namespace GMapping{ 4 | 5 | using namespace std; 6 | void printmemusage(){ 7 | pid_t pid=getpid(); 8 | char procfilename[1000]; 9 | sprintf(procfilename, "/proc/%d/status", pid); 10 | ifstream is(procfilename); 11 | string line; 12 | while (is){ 13 | is >> line; 14 | if (line=="VmData:"){ 15 | is >> line; 16 | cerr << "#VmData:\t" << line << endl; 17 | } 18 | if (line=="VmSize:"){ 19 | is >> line; 20 | cerr << "#VmSize:\t" << line << endl; 21 | } 22 | 23 | } 24 | } 25 | 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/utils/printmemusage.h: -------------------------------------------------------------------------------- 1 | #ifndef PRINTMEMUSAGE_H 2 | #define PRINTMEMUSAGE_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace GMapping{ 10 | void printmemusage(); 11 | }; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /gmapping/openslam_gmapping/utils/printpgm.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | using namespace std; 8 | ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){ 9 | if (!os) 10 | return os; 11 | os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl; 12 | for (int y=ysize-1; y>=0; y--){ 13 | for (int x=0;x /etc/apt/sources.list.d/ros-latest.list' 10 | - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 11 | - sudo apt-get update 12 | # Install and initialize rosdep 13 | - sudo apt-get install python-rosdep 14 | - sudo `which rosdep` init 15 | - rosdep update 16 | # Use rosdep to install rviz's dependencies 17 | - rosdep install --default-yes --from-paths ./ --rosdistro $CI_ROS_DISTRO 18 | script: 19 | - source /opt/ros/$CI_ROS_DISTRO/setup.bash 20 | - mkdir build 21 | - cd build 22 | - cmake .. -DCMAKE_INSTALL_PREFIX=./install 23 | - make -j1 24 | - make -j1 tests 25 | - make -j1 run_tests 26 | - catkin_test_results . 27 | - make -j1 install 28 | notifications: 29 | email: false 30 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nodelet ROS wrapper for OpenSlam's Gmapping. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | gmapping 3 | 1.3.8 4 | This package contains a ROS wrapper for OpenSlam's Gmapping. 5 | The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 6 | as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy 7 | grid map (like a building floorplan) from laser and pose data collected by a mobile robot. 8 | 9 | Brian Gerkey 10 | Vincent Rabaud 11 | CreativeCommons-by-nc-sa-2.0 12 | 13 | http://ros.org/wiki/gmapping 14 | 15 | catkin 16 | 17 | nav_msgs 18 | openslam_gmapping 19 | roscpp 20 | rostest 21 | tf 22 | nodelet 23 | message_generation 24 | 25 | nav_msgs 26 | openslam_gmapping 27 | roscpp 28 | tf 29 | nodelet 30 | message_runtime 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_gmapping 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | /* Author: Brian Gerkey */ 18 | 19 | #include 20 | 21 | #include "slam_gmapping.h" 22 | 23 | int 24 | main(int argc, char** argv) 25 | { 26 | ros::init(argc, argv, "slam_gmapping"); 27 | 28 | SlamGMapping gn; 29 | gn.startLiveSlam(); 30 | ros::spin(); 31 | 32 | return(0); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/src/nodelet.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * slam_gmapping 3 | * Copyright (c) 2008, Willow Garage, Inc. 4 | * 5 | * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE 6 | * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY 7 | * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS 8 | * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. 9 | * 10 | * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO 11 | * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS 12 | * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND 13 | * CONDITIONS. 14 | * 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include "slam_gmapping.h" 22 | 23 | class SlamGMappingNodelet : public nodelet::Nodelet 24 | { 25 | public: 26 | SlamGMappingNodelet() {} 27 | 28 | ~SlamGMappingNodelet() {} 29 | 30 | virtual void onInit() 31 | { 32 | NODELET_INFO_STREAM("Initialising Slam GMapping nodelet..."); 33 | sg_.reset(new SlamGMapping(getNodeHandle(), getPrivateNodeHandle())); 34 | NODELET_INFO_STREAM("Starting live SLAM..."); 35 | sg_->startLiveSlam(); 36 | } 37 | 38 | private: 39 | boost::shared_ptr sg_; 40 | }; 41 | 42 | PLUGINLIB_EXPORT_CLASS(SlamGMappingNodelet, nodelet::Nodelet) 43 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/src/tftest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "sensor_msgs/LaserScan.h" 3 | #include "ros/node.h" 4 | #include "tf/transform_listener.h" 5 | 6 | class Test 7 | { 8 | public: 9 | Test() 10 | { 11 | node_ = new ros::Node("test"); 12 | tf_ = new tf::TransformListener(*node_, true, 13 | ros::Duration(10)); 14 | tf_->setExtrapolationLimit( ros::Duration().fromSec(0.2)); 15 | 16 | node_->subscribe("base_scan", scan_, &Test::laser_cb, this, -1); 17 | } 18 | ~Test() 19 | { 20 | delete tf_; 21 | delete node_; 22 | } 23 | 24 | void laser_cb() 25 | { 26 | // Get the robot's pose 27 | tf::Stamped ident; 28 | tf::Stamped odom_pose; 29 | ident.setIdentity(); 30 | ident.frame_id_ = "base"; 31 | ident.stamp_ = scan_.header.stamp; 32 | try 33 | { 34 | this->tf_->transformPose("odom", ident, odom_pose); 35 | } 36 | catch(tf::TransformException e) 37 | { 38 | ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what()); 39 | return; 40 | } 41 | double yaw,pitch,roll; 42 | tf::Matrix3x3 mat = odom_pose.getBasis(); 43 | mat.getEulerZYX(yaw, pitch, roll); 44 | 45 | printf("%f: %.6f %.6f %.6f\n", 46 | scan_.header.stamp.toSec(), 47 | odom_pose.getOrigin().x(), 48 | odom_pose.getOrigin().y(), 49 | yaw); 50 | } 51 | void spin() { node_->spin(); } 52 | 53 | private: 54 | ros::Node* node_; 55 | tf::TransformListener* tf_; 56 | sensor_msgs::LaserScan scan_; 57 | }; 58 | 59 | int 60 | main(int argc, char** argv) 61 | { 62 | ros::init(argc, argv); 63 | 64 | Test t; 65 | t.spin(); 66 | 67 | 68 | return 0; 69 | } 70 | 71 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_laser_different_beamcount.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_stage_replay.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_stage_replay2.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_symmetry.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/gmapping/test/basic_localization_upside_down.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/slam_gmapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package slam_gmapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.3.8 (2015-07-31) 6 | ------------------ 7 | 8 | 1.3.7 (2015-07-04) 9 | ------------------ 10 | 11 | 1.3.6 (2015-06-26) 12 | ------------------ 13 | 14 | 1.3.5 (2014-08-28) 15 | ------------------ 16 | 17 | 1.3.4 (2014-08-07) 18 | ------------------ 19 | 20 | 1.3.3 (2014-06-23) 21 | ------------------ 22 | 23 | 1.3.2 (2014-01-14) 24 | ------------------ 25 | 26 | 1.3.1 (2014-01-13) 27 | ------------------ 28 | 29 | 1.3.0 (2013-06-28) 30 | ------------------ 31 | * Renamed to gmapping, adding metapackage for slam_gmapping 32 | * Contributors: Mike Ferguson 33 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/slam_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(slam_gmapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /gmapping/slam_gmapping/slam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | slam_gmapping 3 | 1.3.8 4 | slam_gmapping contains a wrapper around gmapping which provides SLAM capabilities. 5 | Brian Gerkey 6 | Vincent Rabaud 7 | CreativeCommons-by-nc-sa-2.0 8 | 9 | http://ros.org/wiki/slam_gmapping 10 | 11 | catkin 12 | 13 | openslam_gmapping 14 | gmapping 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /gmapping/第五课-基于滤波的SLAM算法.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/gmapping/第五课-基于滤波的SLAM算法.pdf -------------------------------------------------------------------------------- /gmapping/第四课-帧间匹配算法.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/gmapping/第四课-帧间匹配算法.pdf -------------------------------------------------------------------------------- /laser_undistortion/README.assets/laser_undistortion.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/laser_undistortion/README.assets/laser_undistortion.jpg -------------------------------------------------------------------------------- /laser_undistortion/README.assets/laser_undistortion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/laser_undistortion/README.assets/laser_undistortion.png -------------------------------------------------------------------------------- /laser_undistortion/bag/laser.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/laser_undistortion/bag/laser.bag -------------------------------------------------------------------------------- /laser_undistortion/launch/LaserUndistortion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /laser_undistortion/第三课-激光雷达及畸变去除.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/laser_undistortion/第三课-激光雷达及畸变去除.pdf -------------------------------------------------------------------------------- /ls_slam/README.assets/GN.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/GN.png -------------------------------------------------------------------------------- /ls_slam/README.assets/GN1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/GN1.png -------------------------------------------------------------------------------- /ls_slam/README.assets/GN2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/GN2.png -------------------------------------------------------------------------------- /ls_slam/README.assets/error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/error.png -------------------------------------------------------------------------------- /ls_slam/README.assets/xianxinghua.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/xianxinghua.png -------------------------------------------------------------------------------- /ls_slam/README.assets/ykb.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/README.assets/ykb.png -------------------------------------------------------------------------------- /ls_slam/data/test_quadrat-e.dat: -------------------------------------------------------------------------------- 1 | EDGE2 0 1 10.2 0 0 500.000000 0.000000 500.000000 5000.000000 0.000000 0.000000 2 | EDGE2 1 2 10 0 0 500.000000 0.000000 500.000000 5000.000000 0.000000 0.000000 3 | EDGE2 2 3 9.9 0 0 500.000000 0.000000 500.000000 5000.000000 0.000000 0.000000 4 | EDGE2 3 0 10.1 0 0 500.000000 0.000000 500.000000 5000.000000 0.000000 0.000000 5 | EDGE2 3 1 9.8 10.1 3.1415 500.000000 0.000000 500.000000 5000.000000 0.000000 0.000000 6 | -------------------------------------------------------------------------------- /ls_slam/data/test_quadrat-v.dat: -------------------------------------------------------------------------------- 1 | VERTEX2 0 0.5 1 0 2 | VERTEX2 1 10 0 1.570796 3 | VERTEX2 2 20 20 3.14159 4 | VERTEX2 3 0 10 -1.570796 5 | -------------------------------------------------------------------------------- /ls_slam/include/ls_slam/gaussian_newton.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSSIAN_NEWTON_H 2 | #define GAUSSIAN_NEWTON_H 3 | 4 | #include 5 | #include 6 | 7 | typedef struct edge 8 | { 9 | int xi,xj; 10 | Eigen::Vector3d measurement; 11 | Eigen::Matrix3d infoMatrix; 12 | }Edge; 13 | 14 | 15 | Eigen::VectorXd LinearizeAndSolve(std::vector& Vertexs, 16 | std::vector& Edges); 17 | 18 | double ComputeError(std::vector& Vertexs, 19 | std::vector& Edges); 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /ls_slam/include/ls_slam/readfile.h: -------------------------------------------------------------------------------- 1 | #ifndef READFILE_H 2 | #define READFILE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "gaussian_newton.h" 9 | 10 | 11 | void ReadVertexInformation(const std::string path,std::vector& nodes); 12 | void ReadEdgesInformation(const std::string path,std::vector& edges); 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /ls_slam/第六课-图优化SLAM算法.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/ls_slam/第六课-图优化SLAM算法.pdf -------------------------------------------------------------------------------- /occupany_mapping/README.assets/occupany.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/occupany_mapping/README.assets/occupany.jpg -------------------------------------------------------------------------------- /occupany_mapping/README.assets/occupany.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/occupany_mapping/README.assets/occupany.png -------------------------------------------------------------------------------- /occupany_mapping/include/occupany_mapping/occupany_mapping.h: -------------------------------------------------------------------------------- 1 | #ifndef OCCUPANY_MAPPING_H 2 | #define OCCUPANY_MAPPING_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | 11 | #include "readfile.h" 12 | 13 | typedef struct gridindex_ 14 | { 15 | int x; 16 | int y; 17 | 18 | void SetIndex(int x_,int y_) 19 | { 20 | x = x_; 21 | y = y_; 22 | } 23 | }GridIndex; 24 | 25 | 26 | 27 | typedef struct map_params 28 | { 29 | double log_occ,log_free; 30 | double resolution; 31 | double origin_x,origin_y; 32 | int height,width; 33 | int offset_x,offset_y; 34 | }MapParams; 35 | 36 | 37 | MapParams mapParams; 38 | 39 | unsigned char* pMap; 40 | 41 | 42 | 43 | 44 | 45 | 46 | #endif 47 | 48 | -------------------------------------------------------------------------------- /occupany_mapping/include/occupany_mapping/readfile.h: -------------------------------------------------------------------------------- 1 | #ifndef READFILE_H 2 | #define READFILE_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | //max 3000 10 | #define READ_DATA_NUMBER 3071 11 | 12 | typedef struct general_laser_scan 13 | { 14 | std::vector range_readings; 15 | std::vector angle_readings; 16 | }GeneralLaserScan; 17 | 18 | 19 | void ReadPoseInformation(const std::string path,std::vector& poses); 20 | void ReadLaserScanInformation(const std::string anglePath, 21 | const std::string laserPath, 22 | std::vector< GeneralLaserScan >& laserscans); 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /occupany_mapping/第七课-已知定位的构图算法.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/occupany_mapping/第七课-已知定位的构图算法.pdf -------------------------------------------------------------------------------- /occupany_mapping/第八课-3D激光SLAM介绍.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/occupany_mapping/第八课-3D激光SLAM介绍.pdf -------------------------------------------------------------------------------- /激光SLAM方案对比.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/激光SLAM方案对比.png -------------------------------------------------------------------------------- /激光SLAM知识框架.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/激光SLAM知识框架.png -------------------------------------------------------------------------------- /激光SLAM算法知识.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SFXiang/Laser_SLAM_Homework/6deb6d93a08f8a54406928a47cc9c200087773a7/激光SLAM算法知识.png --------------------------------------------------------------------------------