search

MIT-SPARK / TEASER-plusplus

A fast and robust point cloud registration library
MIT License
1.81k stars 344 forks source link

Add hierarchical instance, Quatro, in `registration.h` #149

Closed LimHyungTae closed 1 year ago

LimHyungTae commented 2 years ago

Hello @jingnanshi , hope you are doing well.

I'd like to request this PR to add Quatro to your repository.

Originally, we used the PCL library, however, I think it would be great cooperation if my method is provided in your repository as well. (our original repo: https://github.com/url-kaist/Quatro)

There's no contributing file, so I just tried to follow your coding styles, the way to describe references, and so on.

In addition, I did thorough tests to check whether our implementation operates well or not: https://github.com/LimHyungTae/quatro-cpp-fpfh.

Thank you in advance!

jingnanshi commented 2 years ago

Dear @LimHyungTae, thank you for your contribution and PR. Quatro is a great paper that uses TEASER++. Will take a look soon.

LimHyungTae commented 2 years ago

Dear @jingnanshi Thank you for your reply and your kind service as well :) But there are two issues; cloud you provide some advice to me for a successful PR?

  1. TEST(RegistrationTest, CliqueFinderModes) part is not relevant to what I modified. (in test/teaser/registration-test.cc). So, I'm not sure why the test failed.
  2. To check the thorough test, I made a new docker system and test my codes again. However, in my docker system, all tests were passed, i.e. the tests were not reproduced as follows:
root@286b2d5d57c8:/home/git/TEASER-plusplus# cd build && ctest all
Test project /home/git/TEASER-plusplus/build
      Start  1: UtilsTest.RandomSample
 1/48 Test  #1: UtilsTest.RandomSample ...............................   Passed    0.00 sec
      Start  2: UtilsTest.EigenMatrixRemoveRowColumn
 2/48 Test  #2: UtilsTest.EigenMatrixRemoveRowColumn .................   Passed    0.00 sec
      Start  3: UtilsTest.CalculatePointClusterDiameter
 3/48 Test  #3: UtilsTest.CalculatePointClusterDiameter ..............   Passed    0.00 sec
      Start  4: IOTest.ImportPLY
 4/48 Test  #4: IOTest.ImportPLY .....................................   Passed    0.00 sec
      Start  5: IOTest.ImportBigPLY
 5/48 Test  #5: IOTest.ImportBigPLY ..................................   Passed    0.25 sec
      Start  6: PointXYZTest.SimpleOperations
 6/48 Test  #6: PointXYZTest.SimpleOperations ........................   Passed    0.00 sec
      Start  7: PointCloudTest.SimpleOperations
 7/48 Test  #7: PointCloudTest.SimpleOperations ......................   Passed    0.00 sec
      Start  8: TLSTest.TLSEstimate
 8/48 Test  #8: TLSTest.TLSEstimate ..................................   Passed    0.00 sec
      Start  9: TLSTest.TLSEstimateTiled
 9/48 Test  #9: TLSTest.TLSEstimateTiled .............................   Passed    0.02 sec
      Start 10: ScaleSolverTest.UnknownScale
10/48 Test #10: ScaleSolverTest.UnknownScale .........................   Passed    0.00 sec
      Start 11: ScaleSolverTest.FixedScale
11/48 Test #11: ScaleSolverTest.FixedScale ...........................   Passed    0.00 sec
      Start 12: RotationSolverTest.FGRRotation
12/48 Test #12: RotationSolverTest.FGRRotation .......................   Passed    0.00 sec
      Start 13: RotationSolverTest.GNCTLS
13/48 Test #13: RotationSolverTest.GNCTLS ............................   Passed    0.00 sec
      Start 14: TranslationTest.TLSTranslation
14/48 Test #14: TranslationTest.TLSTranslation .......................   Passed    0.00 sec
      Start 15: RegistrationTest.LargeModel
15/48 Test #15: RegistrationTest.LargeModel ..........................   Passed    0.02 sec
      Start 16: RegistrationTest.SolveForScale
16/48 Test #16: RegistrationTest.SolveForScale .......................   Passed    0.02 sec
      Start 17: RegistrationTest.SolveForRotation
17/48 Test #17: RegistrationTest.SolveForRotation ....................   Passed    0.00 sec
      Start 18: RegistrationTest.SolveRegistrationProblemDecoupled
18/48 Test #18: RegistrationTest.SolveRegistrationProblemDecoupled ...   Passed    0.03 sec
      Start 19: RegistrationTest.OutlierDetection
19/48 Test #19: RegistrationTest.OutlierDetection ....................   Passed    0.00 sec
      Start 20: RegistrationTest.NoMaxClique
20/48 Test #20: RegistrationTest.NoMaxClique .........................   Passed    0.01 sec
      Start 21: RegistrationTest.CliqueFinderModes
21/48 Test #21: RegistrationTest.CliqueFinderModes ...................   Passed    0.03 sec
      Start 22: GraphTest.BasicFunctions
22/48 Test #22: GraphTest.BasicFunctions .............................   Passed    0.00 sec
      Start 23: PMCTest.FindMaximumClique1
23/48 Test #23: PMCTest.FindMaximumClique1 ...........................   Passed    0.00 sec
      Start 24: PMCTest.FindMaximumClique2
24/48 Test #24: PMCTest.FindMaximumClique2 ...........................   Passed    0.00 sec
      Start 25: PMCTest.FindMaximumClique3
25/48 Test #25: PMCTest.FindMaximumClique3 ...........................   Passed    0.00 sec
      Start 26: MaxCliqueSolverTest.FindMaxClique
26/48 Test #26: MaxCliqueSolverTest.FindMaxClique ....................   Passed    0.00 sec
      Start 27: DRSCertifierTest.GetOmega1
27/48 Test #27: DRSCertifierTest.GetOmega1 ...........................   Passed    0.03 sec
      Start 28: DRSCertifierTest.GetBlockDiagOmega
28/48 Test #28: DRSCertifierTest.GetBlockDiagOmega ...................   Passed    0.03 sec
      Start 29: DRSCertifierTest.GetQCost
29/48 Test #29: DRSCertifierTest.GetQCost ............................   Passed    0.03 sec
      Start 30: DRSCertifierTest.GetLambdaGuess
30/48 Test #30: DRSCertifierTest.GetLambdaGuess ......................   Passed    0.03 sec
      Start 31: DRSCertifierTest.GetLinearProjection
31/48 Test #31: DRSCertifierTest.GetLinearProjection .................   Passed    0.03 sec
      Start 32: DRSCertifierTest.GetOptimalDualProjection
32/48 Test #32: DRSCertifierTest.GetOptimalDualProjection ............   Passed    0.03 sec
      Start 33: DRSCertifierTest.ComputeSubOptimalityGap
33/48 Test #33: DRSCertifierTest.ComputeSubOptimalityGap .............   Passed    0.03 sec
      Start 34: DRSCertifierTest.Certify
34/48 Test #34: DRSCertifierTest.Certify .............................   Passed    0.09 sec
      Start 35: DRSCertifierTest.LargeInstance
35/48 Test #35: DRSCertifierTest.LargeInstance .......................   Passed   16.75 sec
      Start 36: DRSCertifierTest.Random100Points
36/48 Test #36: DRSCertifierTest.Random100Points .....................   Passed    0.72 sec
      Start 37: DRSCertifierTest.RandomLargeInstsances
37/48 Test #37: DRSCertifierTest.RandomLargeInstsances ...............   Passed   15.50 sec
      Start 38: LinalgTest.HatMap
38/48 Test #38: LinalgTest.HatMap ....................................   Passed    0.00 sec
      Start 39: LinalgTest.VectorKronFixedSize
39/48 Test #39: LinalgTest.VectorKronFixedSize .......................   Passed    0.01 sec
      Start 40: LinalgTest.VectorKronDynamicSize
40/48 Test #40: LinalgTest.VectorKronDynamicSize .....................   Passed    0.02 sec
      Start 41: LinalgTest.GetNearestPSDRandom
41/48 Test #41: LinalgTest.GetNearestPSDRandom .......................   Passed    0.17 sec
      Start 42: LinalgTest.GetNearestPSDSimple
42/48 Test #42: LinalgTest.GetNearestPSDSimple .......................   Passed    0.00 sec
      Start 43: RegistrationBenchmark.Benchmark1
43/48 Test #43: RegistrationBenchmark.Benchmark1 .....................   Passed    0.72 sec
      Start 44: RegistrationBenchmark.Benchmark2
44/48 Test #44: RegistrationBenchmark.Benchmark2 .....................   Passed    0.71 sec
      Start 45: RegistrationBenchmark.Benchmark3
45/48 Test #45: RegistrationBenchmark.Benchmark3 .....................   Passed    0.62 sec
      Start 46: RegistrationBenchmark.Benchmark4
46/48 Test #46: RegistrationBenchmark.Benchmark4 .....................   Passed    1.45 sec
      Start 47: RegistrationBenchmark.Benchmark5
47/48 Test #47: RegistrationBenchmark.Benchmark5 .....................   Passed    0.52 sec
      Start 48: RegistrationBenchmark.Benchmark6
48/48 Test #48: RegistrationBenchmark.Benchmark6 .....................   Passed    0.58 sec

100% tests passed, 0 tests failed out of 48

You don't need to be hurry to respond to it. I'm looking forward to your prompt reply.

jingnanshi commented 2 years ago

@LimHyungTae I rerun the tests and it seems to be passing right now. It might be a timeout error.

jingnanshi commented 2 years ago

Before merging, is it possible for you to add a few unit tests for the Quatro code? Something basic is fine, for example generating random points with random rotations. Thanks!

LimHyungTae commented 2 years ago

Dear Jingnashi, I upload the following content:

  1. In the ctest, Quatro is uploaded (75710df)
  2. quatro_cpp_fpfh.cc is added (9cdb561)
  3. And I found that the unit of teaser_cpp_fpfh.cc is wrong, so I modified it. The one additional issue is that I realized that TEASER++ in teaser_cpp_fpfh.cc fails to estimate the rotation. Please check it (please kindly note that it is not my fault, but it originally estimates the wrong rotation).

And we don't need to hurry, so please check them after the submission of CVPR! Thanks, bro~

jingnanshi commented 2 years ago

@LimHyungTae Can you try this branch for the fpfh example: https://github.com/MIT-SPARK/TEASER-plusplus/tree/bugfix/examples I also noticed the problem and it seems to be due to the noise being too high.

LimHyungTae commented 1 year ago

Dear @jingnanshi, sorry for the late reply. In fact, after finishing my dense, currently, I'm working for Prof. Stachniss lab as a visiting scholar, so I have little time to implement that codes...So please check for a moment. Anw, I'll check that codes and do PR ASAP. Have a merry Christmas!

LimHyungTae commented 1 year ago

Hey @jingnanshi Hope you having nice days. I modified the codes following your bugfix commit. And I checked the performance as follows:

From teaser_cpp_fpfh.cc

=====================================
          TEASER++ Results           
=====================================
Expected rotation: 
  0.996927  0.0668736 -0.0406664
 -0.066129   0.997618  0.0194009
 0.0418676 -0.0166518   0.998978
Estimated rotation: 
  0.997015  0.0662911 -0.0395803
-0.0655119   0.997638  0.0206715
 0.0408572 -0.0180169   0.999003
Error (rad): 0.004494

Expected translation: 
 -0.115577
-0.0387705
  0.114875
Estimated translation: 
 -0.115578
-0.0388336
  0.114961
Error (m): 0.000106482

Number of correspondences: 1889
Number of outliers: 1700
Time taken (s): 0.001061

From teaser_cpp_ply.cc

=====================================
          TEASER++ Results           
=====================================
Expected rotation: 
  0.996927  0.0668736 -0.0406664
 -0.066129   0.997618  0.0194009
 0.0418676 -0.0166518   0.998978
Estimated rotation: 
   0.99692  0.0670416 -0.0406821
 -0.066302   0.997614  0.0192679
 0.0418768 -0.0165112   0.998986
Error (rad): 0.004128

Expected translation: 
 -0.115577
-0.0387705
  0.114875
Estimated translation: 
 -0.115577
-0.0387808
   0.11487
Error (m): 1.12306e-05

Number of correspondences: 1889
Number of outliers: 1700
Time taken (s): 0.187157

In summary, the errors are resolved! :) Take your time and please check my commit! Thanks