- Optimization设置会严重影响cartographer性能。从/Od到/O2可能是50倍的性能提升。
- Program Database中的/Zi、None,对性能基本没影响。
- 在windows、ubuntu(x86架构),是否使用SuiteSparse对cartographer性能没多大影响。android待测。
cartographer实时建图须要极大计算量。总的来说,计算量主要花在cartographer如何处理slam传感器发来sensor_msgs::LaserScan消息,即Node::HandleLaserScanMessage。这里衡量性能,也就是看一次HandleLaserScanMessage用了多少时间。
<cartographer_ros>/cartographer_ros/node.cc ------ void Node::HandleLaserScanMessage(const int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { absl::MutexLock lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { return; } // 以下三条语句是自写的,用于输出HandleLaserScanMessage的耗时信息 static int times = 0; static double total_cost = 0; trosverbose verbose("Node::HandleLaserScanMessage", times, total_cost); map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleLaserScanMessage(sensor_id, msg); }
ros最终是要用在android,但调试主要是在windows。为此先列出windows下的测试结果。在windows,编译器自然是visual studio 2019。
一、visual studio 2019下的性能测试
编译器设置
- Optimization。“C/C++”——“Optimization”。只分两种测试:1)不优化:Disabled (/Od),2)开启优化:Maximum Optimization (Favor Speed) (/O2)。
- Debug Information Format。“C/C++”——“General”。只分两种测试:1)Program Database(/Zi)。2)None。
- Runtime Library。“C/C++”——“Code Generation”。固定使用“Multi-threaded (/MT)”。
- ceres是否使用SUITESPARSE。使用SUITESPARSE,指的是同时不定义三个宏:CERES_NO_LAPACK, CERES_NO_SUITESPARSE, CERES_NO_CXSPARSE。不使用则是指同时定义这三个宏。
Optimization | Debug Information Format | ceres | HandleLaserScanMessage |
不优化 | /Zi | 使用SUITESPARSE | 平均耗时850ms。其中ceres::Solve耗时199.564 |
优化 | /Zi | 使用SUITESPARSE | 平均耗时14ms。 其中ceres::Solve耗时2.465 |
优化 | None | 使用SUITESPARSE | 平均耗时14.2ms。 其中ceres::Solve耗时2.134 |
优化 | /Zi | 不使用SUITESPARSE | 平均耗时14.28ms。 其中ceres::Solve耗时2.203 |
HandleLaserScanMessage会调用到CeresScanMatcher2D::Match,Match中会一次ceres::Solve,表中的“ceres::Solve”专指这次Solve。下面给出表中第一行、第二行时的更细节的耗时记录。
Optimization: Disabled (/Od) Debug Information Format: Program Database(/Zi) --- Node::HandleLaserScanMessage ... GlobalTrajectoryBuilder::AddSensorData LocalTrajectoryBuilder2D::AddRangeData LocalTrajectoryBuilder2D::AddAccumulatedRangeData LocalTrajectoryBuilder2D::ScanMatch (850.425毫秒) [1/3]RealTimeCorrelativeScanMatcher2D::Match, 634.036 scan_matching::GenerateRotatedScans, 47.538 scan_matching::DiscretizeScans, 92.902 RealTimeCorrelativeScanMatcher2D::ScoreCandidates, 492.412 [2/3]CeresScanMatcher2D::Match(当中有一个ceres::Solve(...)), 199.564 [3/3]pose_observation, 0.003 Optimization: Maximum Optimization (Favor Speed) (/O2) Debug Information Format: Program Database(/Zi) --- Node::HandleLaserScanMessage ... GlobalTrajectoryBuilder::AddSensorData LocalTrajectoryBuilder2D::AddRangeData LocalTrajectoryBuilder2D::AddAccumulatedRangeData LocalTrajectoryBuilder2D::ScanMatch 12.220 [1/3]RealTimeCorrelativeScanMatcher2D::Match, 9.721 scan_matching::GenerateRotatedScans, 0.249 scan_matching::DiscretizeScans, 1.452 RealTimeCorrelativeScanMatcher2D::ScoreCandidates, 7.904 [2/3]CeresScanMatcher2D::Match(当中有一个ceres::Solve(...)), 2.465 [3/3]pose_observation, 0.000
二、SuiteSparse
SuiteSparse封装了用来处理空间稀疏矩阵数据的工具函数。只要有可能,ceres优先使用这个库。当然,可以在config.h定义相关宏:CERES_NO_LAPACK, CERES_NO_SUITESPARSE, CERES_NO_CXSPARSE,让不使用SuiteSparse。
不使用SuiteSparse的config.h示例 ------ #ifndef CERES_PUBLIC_INTERNAL_CONFIG_H_ #define CERES_PUBLIC_INTERNAL_CONFIG_H_ #define CERES_NO_SUITESPARSE #define CERES_NO_CXSPARSE #define CERES_NO_ACCELERATE_SPARSE #define CERES_NO_LAPACK #define CERES_USE_EIGEN_SPARSE #define CERES_USE_CXX_THREADS #endif // CERES_PUBLIC_INTERNAL_CONFIG_H_
最终要看android下的测试结果,至少到目前,倾向不使用SuiteSparse。1)从性能测试上看,是否使用SuiteSparse对HandleLaserScanMessage耗时并没多大影响。2)编译SuiteSparse还有点麻烦,尤其还要背上libblas.lib、liblapack.lib,不用能减少编译难度。
贴下一旦使用SuiteSparse,须要额外链接的库。
../../../linker/windows/lib/libamdd.lib ../../../linker/windows/lib/libcamdd.lib ../../../linker/windows/lib/libccolamdd.lib ../../../linker/windows/lib/libcholmodd.lib ../../../linker/windows/lib/libcolamdd.lib ../../../linker/windows/lib/libcxsparsed.lib ../../../linker/windows/lib/libumfpackd.lib ../../../linker/windows/lib/suitesparseconfigd.lib ../../../linker/windows/lib/metisd.lib ../../../linker/windows/lib/libblas.lib ../../../linker/windows/lib/liblapack.lib
额外说下,即使config.h定义了#define CERES_NO_LAPACK,但还是必须链接libblas.lib、liblapack.lib
三、ubuntu(x86架构)

HandleLaserScanMessage平均耗时4毫秒。是否使用SuiteSparse对性能基本没影响