ch7点云配准算法加速

[gongjun136]

pointcloud_registration.zip 高博，很感谢你的新工作，在ch7中你对比了多种的点云配准方法，我想用在我自己的项目上，于是把代码导出，发现导出后的配准耗时严重，我怀疑是tbb的缘故，但是并没有发现可疑点，如上是我导出后的cmake工程，原本在您的代码中只需要几百ms，突然暴增到几千ms，请教下高博具体的缘故：

[gaoxiang12]

我不太确定你的具体情况，可以按照以下顺序来分析：

1. 编译选项是否有误，例如搞错release/debug之类？
2. 其他几类方法（如PCL）是否也有类似的耗时问题？
3. 点云的点数、稠密度也会影响配准效率。如果你给出的点云过于密集，会影响近邻搜索算法的效率。

[gongjun136]

如下是运行您的代码的时间： $ ./bin/test_icp ... I1019 16:47:01.698619 21176 test_icp.cc:64] icp p2p align success, pose: 00.0265488 -0.0107399 -0.0234792 000.999314, 0-0.068892 0-0.103292 0.00503469 I1019 16:47:01.784773 21176 sys_utils.h:32] 方法 ICP P2P 平均调用时间/次数: 218.389/1 毫秒. ... I1019 16:47:01.917838 21176 test_icp.cc:85] icp p2plane align success, pose: 00.0323446 -0.0136962 -0.0264419 000.999033, 00-0.0699088 000-0.101207 -2.92256e-06 I1019 16:47:02.007985 21176 sys_utils.h:32] 方法 ICP P2Plane 平均调用时间/次数: 223.144/1 毫秒. ... I1019 16:47:02.252468 21176 test_icp.cc:106] icp p2line align success, pose: 000.0311809 -0.00666497 0-0.0225955 0000.999236, -0.0683443 -0.0722157 0.00186888 I1019 16:47:02.340947 21176 sys_utils.h:32] 方法 ICP P2Line 平均调用时间/次数: 332.89/1 毫秒. ... I1019 16:47:02.366147 21176 test_icp.cc:131] ndt align success, pose: 000.0267126 -0.00422951 0-0.0221132 00000.99939, 0-0.072124 0-0.104462 0.00876557 I1019 16:47:02.422798 21176 sys_utils.h:32] 方法 NDT 平均调用时间/次数: 81.7884/1 毫秒. I1019 16:47:03.613003 21176 test_icp.cc:151] pose from icp pcl: 0000.029222 -0.00915236 0-0.0195184 0000.999341, -0.0636124 -0.0567144 0.00273507 I1019 16:47:03.671408 21176 test_icp.cc:157] ICP PCL pose error: 0.0483811 I1019 16:47:03.671705 21176 sys_utils.h:32] 方法 ICP PCL 平均调用时间/次数: 1248.87/1 毫秒. I1019 16:47:03.994731 21176 test_icp.cc:171] pose from ndt pcl: 0-0.0276939 -0.00399581 0-0.0326939 0000.999074, 0-0.0370633 -0.00879984 000.033367518530452800.301441 I1019 16:47:04.055038 21176 test_icp.cc:174] score: 0.301441 I1019 16:47:04.055138 21176 test_icp.cc:178] NDT PCL pose error: 0.160315 I1019 16:47:04.055691 21176 sys_utils.h:32] 方法 NDT PCL 平均调用时间/次数: 383.951/1 毫秒. 可以看到时间是没问题的，并且用您的代码我验证了自己的数据： $ ./bin/test_icp --source /home/gj/c++_study/test/data/00000330.pcd --target /home/gj/c++_study/test/data/00000190.pcd ... I1019 16:11:44.243273 1850 sys_utils.h:32] 方法 ICP P2P 平均调用时间/次数: 1069.17/1 毫秒. ... I1019 16:11:45.567184 1850 sys_utils.h:32] 方法 ICP P2Plane 平均调用时间/次数: 1323.83/1 毫秒. ... I1019 16:11:45.807898 1850 sys_utils.h:32] 方法 ICP P2Line 平均调用时间/次数: 240.636/1 毫秒. ... I1019 16:11:45.965659 1850 sys_utils.h:32] 方法 NDT 平均调用时间/次数: 157.697/1 毫秒. ... I1019 16:11:46.960400 1850 sys_utils.h:32] 方法 ICP PCL 平均调用时间/次数: 994.698/1 毫秒. ... I1019 16:11:47.442392 1850 sys_utils.h:32] 方法 NDT PCL 平均调用时间/次数: 481.969/1 毫秒. 可以看到也是数据时间虽然有所提升，因为pcd点数稠密一些，但是不会超过1500ms，在我第一条评论中有我导出的可运行的cmake工程，编译选项是release，如下是我导出到的cmake工程运行您提供的EPFL数据的结果： $ ./main /home/gj/slam_in_autonomous_driving_noted/data/ch7/EPFL/kneeling_lady_source.pcd /home/gj/slam_in_autonomous_driving_noted/data/ch7/EPFL/kneeling_lady_target.pcd ... I1019 16:54:56.457026 22787 main.cpp:76] icp p2p align success, pose: 00.02654895 -0.01073989 -0.02347944 00.99931403, -0.06889236 -0.10329247 00.00503462 I1019 16:54:56.561403 22787 sys_utils.h:32] 方法 ICP P2P 平均调用时间/次数: 8635.75/1 毫秒. ... I1019 16:55:02.199961 22787 main.cpp:101] icp p2plane align success, pose: 00.03234462 -0.01369618 -0.02644200 00.99903306, -0.06990892 -0.10120724 -0.00000295 I1019 16:55:02.307135 22787 sys_utils.h:32] 方法 ICP P2Plane 平均调用时间/次数: 5745.67/1 毫秒. ... I1019 16:55:14.573542 22787 main.cpp:127] icp p2line align success, pose: 00.03117905 -0.00665671 -0.02260152 00.99923607, -0.06835465 -0.07221784 00.00187260 I1019 16:55:14.668892 22787 sys_utils.h:32] 方法 ICP P2Line 平均调用时间/次数: 12361.7/1 毫秒. ... I1019 16:55:19.289510 22787 main.cpp:158] ndt align success, pose: 00.02671675 -0.00468531 -0.02217692 00.99938603, -0.07183547 -0.10451608 00.00864214 I1019 16:55:19.351706 22787 sys_utils.h:32] 方法 NDT 平均调用时间/次数: 4682.76/1 毫秒. I1019 16:55:21.015874 22787 main.cpp:181] pose from icp pcl: 00.02922197 -0.00915236 -0.01951841 00.99934071, -0.06361243 -0.05671443 00.00273507 I1019 16:55:21.077473 22787 sys_utils.h:32] 方法 ICP PCL 平均调用时间/次数: 1725.73/1 毫秒. I1019 16:56:35.662446 22787 main.cpp:202] pose from ndt pcl: -0.02769389 -0.00399581 -0.03269395 00.99907368, -0.03706326 -0.00879984 00.0333675118530452800.30144089 I1019 16:56:35.725908 22787 main.cpp:205] score: 0.301441 I1019 16:56:35.726606 22787 sys_utils.h:32] 方法 NDT PCL 平均调用时间/次数: 74649.1/1 毫秒. 不管PCL还是自写的都很耗时，但是我代码完全没有改动，从结果可以看出，他们的结果基本相同，就是时间暴增，运行我自己的数据也是一样的，同一个数据，同样的代码结果一致，时间暴增，困惑。

[jackzyh]

大家好 我在测试第五章的时候也遇到了这个问题 打log测试发现for_each没有起到并发的效果 耗时严重。

[gongjun136]

大家好 我在测试第五章的时候也遇到了这个问题 打log测试发现for_each没有起到并发的效果 耗时严重。

这个bug感觉很难知道原因，数据和代码没有改变，但是就是时间不同，个人感觉是extension没有发挥作用，但是我删除extension确实比现在更加耗时。同学你测试第七章源码应该不会出现耗时严重吧./bin/test_icp

[jackzyh]

大家好 我在测试第五章的时候也遇到了这个问题 打log测试发现for_each没有起到并发的效果 耗时严重。

这个bug感觉很难知道原因，数据和代码没有改变，但是就是时间不同，个人感觉是extension没有发挥作用，但是我删除extension确实比现在更加耗时。同学你测试第七章源码应该不会出现耗时严重吧./bin/test_icp

一样的 耗时非常严重 只要涉及并发就很慢 我测试的原因是livox-ros-driver包去掉就会很慢

[gongjun136]

大家好 我在测试第五章的时候也遇到了这个问题 打log测试发现for_each没有起到并发的效果 耗时严重。

这个bug感觉很难知道原因，数据和代码没有改变，但是就是时间不同，个人感觉是extension没有发挥作用，但是我删除extension确实比现在更加耗时。同学你测试第七章源码应该不会出现耗时严重吧./bin/test_icp

一样的 耗时非常严重 只要涉及并发就很慢 我测试的原因是livox-ros-driver包去掉就会很慢

啊？还有这种玄学原因，哥们你用的Ubuntu20.04还是18.04啊，你有可能tbb没有链接对，这个耗时问题应该是即使编译通过，但是也不一定真正地链接上tbb，我用的20.04是可以低耗时完成的。我导出的cmake工程不行，虽然可以编译通过，但是感觉没有正确链接tbb，哥们一起研究一下不，我微信：19507481850，被这玩意搞得焦头烂额也没搞明白原因。高博救救孩子吧😢 pointcloud_registration-tbb.zip

[gaoxiang12]

我建议手动通过cmake .. -DCMAKE_BUILD_TYPE=Release 来指定编译模式，把build删了重新编一下试试？我看到有些机器上会默认编成debug。

[gongjun136]

我建议手动通过cmake .. -DCMAKE_BUILD_TYPE=Release 来指定编译模式，把build删了重新编一下试试？我看到有些机器上会默认编成debug。

高博太牛了，这是我添加的代码，发现真没有设置：

正确的设置应该是：set(CMAKE_BUILD_TYPE Release) set(DEFAULT_BUILD_TYPE "Release")是设置一个变量DEFAULT_BUILD_TYPE 为Release，高博您的github上的代码也是如此的，感觉这是个bug，建议修复一下。再次感谢高博！！！！

[gaoxiang12]

嗯我看到了，回头去修复一下。