/
registration.h
898 lines (786 loc) · 29.9 KB
/
registration.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
/**
* Copyright 2020, Massachusetts Institute of Technology,
* Cambridge, MA 02139
* All Rights Reserved
* Authors: Jingnan Shi, et al. (see THANKS for the full author list)
* See LICENSE for the license information
*/
#pragma once
#include <memory>
#include <vector>
#include <tuple>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Geometry>
#include "omp.h"
#include "teaser/graph.h"
#include "teaser/geometry.h"
// TODO: might be a good idea to template Eigen::Vector3f and Eigen::VectorXf such that later on we
// can decide to use double if we want. Double vs float might give nontrivial differences..
namespace teaser {
/**
* Struct to hold solution to a registration problem
*/
struct RegistrationSolution {
bool valid = true;
double scale;
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* Abstract virtual class for decoupling specific scale estimation methods with interfaces.
*/
class AbstractScaleSolver {
public:
virtual ~AbstractScaleSolver() {}
/**
* Pure virtual method for solving scale. Different implementations may have different assumptions
* about input data.
* @param src
* @param dst
* @return estimated scale (s)
*/
virtual void solveForScale(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst, double* scale,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) = 0;
};
/**
* Abstract virtual class for decoupling specific rotation estimation method implementations with
* interfaces.
*/
class AbstractRotationSolver {
public:
virtual ~AbstractRotationSolver() {}
/**
* Pure virtual method for solving rotation. Different implementations may have different
* assumptions about input data.
* @param src
* @param dst
* @return estimated rotation matrix (R)
*/
virtual void solveForRotation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Matrix3d* rotation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) = 0;
};
/**
* Abstract virtual class for decoupling specific translation estimation method implementations with
* interfaces.
*/
class AbstractTranslationSolver {
public:
virtual ~AbstractTranslationSolver() {}
/**
* Pure virtual method for solving translation. Different implementations may have different
* assumptions about input data.
* @param src
* @param dst
* @return estimated translation vector
*/
virtual void solveForTranslation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Vector3d* translation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) = 0;
};
/**
* Performs scalar truncated least squares estimation
*/
class ScalarTLSEstimator {
public:
ScalarTLSEstimator() = default;
/**
* Use truncated least squares method to estimate true x given measurements X
* TODO: call measurements Z or Y to avoid confusion with x
* TODO: specify which type/size is x and X in the comments
* @param X Available measurements
* @param ranges Maximum admissible errors for measurements X
* @param estimate (output) pointer to a double holding the estimate
* @param inliers (output) pointer to a Eigen row vector of inliers
*/
void estimate(const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, double* estimate,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers);
/**
* A slightly different implementation of TLS estimate. Use loop tiling to achieve potentially
* faster performance.
* @param X Available measurements
* @param ranges Maximum admissible errors for measurements X
* @param s scale for tiling
* @param estimate (output) pointer to a double holding the estimate
* @param inliers (output) pointer to a Eigen row vector of inliers
*/
void estimate_tiled(const Eigen::RowVectorXd& X, const Eigen::RowVectorXd& ranges, const int& s,
double* estimate, Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers);
};
/**
* Perform scale estimation using truncated least-squares (TLS)
*/
class TLSScaleSolver : public AbstractScaleSolver {
public:
TLSScaleSolver() = delete;
explicit TLSScaleSolver(double noise_bound, double cbar2)
: noise_bound_(noise_bound), cbar2_(cbar2) {
assert(noise_bound > 0);
assert(cbar2 > 0);
};
/**
* Use TLS (adaptive voting) to solve for scale. Assume dst = s * R * src
* @param src
* @param dst
* @return a double indicating the estimated scale
*/
void solveForScale(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst, double* scale,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
private:
double noise_bound_;
double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio
ScalarTLSEstimator tls_estimator_;
};
/**
* Perform outlier pruning / inlier selection based on scale. This class does not perform scale
* estimation. Rather, it estimates outliers based on the assumption that there is no scale
* difference between the two provided vector of points.
*/
class ScaleInliersSelector : public AbstractScaleSolver {
public:
ScaleInliersSelector() = delete;
explicit ScaleInliersSelector(double noise_bound, double cbar2)
: noise_bound_(noise_bound), cbar2_(cbar2){};
/**
* Assume dst = src + noise. The scale output will always be set to 1.
* @param src [in] a vector of points
* @param dst [in] a vector of points
* @param scale [out] a constant of 1
* @param inliers [out] a row vector of booleans indicating whether a measurement is an inlier
*/
void solveForScale(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst, double* scale,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
private:
double noise_bound_;
double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio
};
/**
* Perform translation estimation using truncated least-squares (TLS)
*/
class TLSTranslationSolver : public AbstractTranslationSolver {
public:
TLSTranslationSolver() = delete;
explicit TLSTranslationSolver(double noise_bound, double cbar2)
: noise_bound_(noise_bound), cbar2_(cbar2){};
/**
* Estimate translation between src and dst points. Assume dst = src + t.
* @param src
* @param dst
* @param translation output parameter for the translation vector
* @param inliers output parameter for detected outliers
*/
void solveForTranslation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Vector3d* translation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
private:
double noise_bound_;
double cbar2_; // maximal allowed residual^2 to noise bound^2 ratio
ScalarTLSEstimator tls_estimator_;
};
/**
* Base class for GNC-based rotation solvers
*/
class GNCRotationSolver : public AbstractRotationSolver {
public:
struct Params {
size_t max_iterations;
double cost_threshold;
double gnc_factor;
double noise_bound;
};
GNCRotationSolver(Params params) : params_(params) {}
Params getParams() { return params_; }
void setParams(Params params) { params_ = params; }
/**
* Return the cost of the GNC solver at termination. Details of the cost function is dependent on
* the specific solver implementation.
*
* @return cost at termination of the GNC solver. Undefined if run before running the solver.
*/
double getCostAtTermination() { return cost_; }
protected:
Params params_;
double cost_;
};
/**
* Use GNC-TLS to solve rotation estimation problems.
*
* For more information, please refer to:
* H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for Robust Spatial
* Perception: From Non-Minimal Solvers to Global Outlier Rejection,” arXiv:1909.08605 [cs, math],
* Sep. 2019.
*/
class GNCTLSRotationSolver : public GNCRotationSolver {
public:
GNCTLSRotationSolver() = delete;
/**
* Parametrized constructor
* @param params
*/
explicit GNCTLSRotationSolver(Params params) : GNCRotationSolver(params){};
/**
* Estimate rotation between src & dst using GNC-TLS method
* @param src
* @param dst
* @param rotation
* @param inliers
*/
void solveForRotation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Matrix3d* rotation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
};
/**
* Use Fast Global Registration to solve for pairwise registration problems
*
* For more information, please see the original paper on FGR:
* Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer Vision – ECCV 2016,
* Cham, 2016, vol. 9906, pp. 766–782.
* Notice that our implementation differs from the paper on the estimation of T matrix. We
* only estimate rotation, instead of rotation and translation.
*
*/
class FastGlobalRegistrationSolver : public GNCRotationSolver {
public:
/**
* Remove default constructor
*/
FastGlobalRegistrationSolver() = delete;
/**
* Parametrized constructor
* @param params
* @param rotation_only
*/
explicit FastGlobalRegistrationSolver(Params params) : GNCRotationSolver(params){};
/**
* Solve a pairwise registration problem given two sets of points.
* Notice that we assume no scale difference between v1 & v2.
* @param src
* @param dst
* @return a RegistrationSolution struct.
*/
void solveForRotation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Matrix3d* rotation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
};
/**
* Use Quatro to solve for pairwise registration problems avoiding degeneracy
*
* For more information, please see the original paper on Quatro:
* H. Lim et al., "A Single Correspondence Is Enough: Robust Global Registration
* to Avoid Degeneracy in Urban Environments," in Robotics - ICRA 2022,
* Accepted. To appear. arXiv:2203.06612 [cs], Mar. 2022.
* Quatro and TEASER++ differ in the estimation of rotation. Quatro forgoes roll and pitch estimation,
* yet it is empirically found that it makes the algorithm more robust against degeneracy.
*/
class QuatroSolver : public GNCRotationSolver {
public:
/**
* Remove default constructor
*/
QuatroSolver() = delete;
/**
* Parametrized constructor
* @param params
* @param rotation_only
*/
explicit QuatroSolver(Params params) : GNCRotationSolver(params){};
/**
* Solve a pairwise registration problem given two sets of points.
* Notice that we assume no scale difference between v1 & v2.
* @param src
* @param dst
* @return a RegistrationSolution struct.
*/
void solveForRotation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst,
Eigen::Matrix3d* rotation,
Eigen::Matrix<bool, 1, Eigen::Dynamic>* inliers) override;
};
/**
* Solve registration problems robustly.
*
* For more information, please refer to:
* H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,”
* arXiv:2001.07715 [cs, math], Jan. 2020.
*/
class RobustRegistrationSolver {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* An enum class representing the available GNC rotation estimation algorithms.
*
* GNC_TLS: see H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated Non-Convexity for
* Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection,”
* arXiv:1909.08605 [cs, math], Sep. 2019.
*
* FGR: see Q.-Y. Zhou, J. Park, and V. Koltun, “Fast Global Registration,” in Computer Vision –
* ECCV 2016, Cham, 2016, vol. 9906, pp. 766–782. and H. Yang, P. Antonante, V. Tzoumas, and L.
* Carlone, “Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to
* Global Outlier Rejection,” arXiv:1909.08605 [cs, math], Sep. 2019.
*
* QUATRO: H. Lim et al., "A Single Correspondence Is Enough: Robust Global Registration
* to Avoid Degeneracy in Urban Environments," in Robotics -
* ICRA 2022, pp. 8010-8017
* arXiv:2203.06612 [cs], Mar. 2022.
*/
enum class ROTATION_ESTIMATION_ALGORITHM {
GNC_TLS = 0,
FGR = 1,
QUATRO = 2,
};
/**
* Enum representing the type of graph-based inlier selection algorithm to use
*
* PMC_EXACT: Use PMC to find exact clique from the inlier graph
* PMC_HEU: Use PMC's heuristic finder to find approximate max clique
* KCORE_HEU: Use k-core heuristic to select inliers
* NONE: No inlier selection
*/
enum class INLIER_SELECTION_MODE {
PMC_EXACT = 0,
PMC_HEU = 1,
KCORE_HEU = 2,
NONE = 3,
};
/**
* Enum representing the formulation of the TIM graph after maximum clique filtering.
*
* CHAIN: formulate TIMs by only calculating the TIMs for consecutive measurements
* COMPLETE: formulate a fully connected TIM graph
*/
enum class INLIER_GRAPH_FORMULATION {
CHAIN = 0,
COMPLETE = 1,
};
/**
* A struct representing params for initializing the RobustRegistrationSolver
*
* Note: the default values needed to be changed accordingly for best performance.
*/
struct Params {
/**
* A bound on the noise of each provided measurement.
*/
double noise_bound = 0.01;
/**
* Square of the ratio between acceptable noise and noise bound. Usually set to 1.
*/
double cbar2 = 1;
/**
* Whether the scale is known. If set to False, the solver assumes no scale differences
* between the src and dst points. If set to True, the solver will first solve for scale.
*
* When the solver does not estimate scale, it solves the registration problem much faster.
*/
bool estimate_scaling = true;
/**
* Which algorithm to use to estimate rotations.
*/
ROTATION_ESTIMATION_ALGORITHM rotation_estimation_algorithm =
ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
/**
* Factor to multiple/divide the control parameter in the GNC algorithm.
*
* For FGR: the algorithm divides the control parameter by the factor every iteration.
* For GNC-TLS: the algorithm multiples the control parameter by the factor every iteration.
*/
double rotation_gnc_factor = 1.4;
/**
* Maximum iterations allowed for the GNC rotation estimators.
*/
size_t rotation_max_iterations = 100;
/**
* Cost threshold for the GNC rotation estimators.
*
* For FGR / GNC-TLS algorithm, the cost thresholds represent different meanings.
* For FGR: the cost threshold compares with the computed cost at each iteration
* For GNC-TLS: the cost threshold compares with the difference between costs of consecutive
* iterations.
*/
double rotation_cost_threshold = 1e-6;
/**
* Type of TIM graph given to GNC rotation solver
*/
INLIER_GRAPH_FORMULATION rotation_tim_graph = INLIER_GRAPH_FORMULATION::CHAIN;
/**
* \brief Type of the inlier selection
*/
INLIER_SELECTION_MODE inlier_selection_mode = INLIER_SELECTION_MODE::PMC_EXACT;
/**
* \brief The threshold ratio for determining whether to skip max clique and go straightly to
* GNC rotation estimation. Set this to 1 to always use exact max clique selection, 0 to always
* skip exact max clique selection.
*
* \attention Note that the use_max_clique option takes precedence. In other words, if
* use_max_clique is set to false, then kcore_heuristic_threshold will be ignored. If
* use_max_clique is set to true, then the following will happen: if the max core number of the
* inlier graph is lower than the kcore_heuristic_threshold as a percentage of the total nodes
* in the inlier graph, then the code will preceed to call the max clique finder. Otherwise, the
* graph will be directly fed to the GNC rotation solver.
*
*/
double kcore_heuristic_threshold = 0.5;
/**
* \deprecated Use inlier_selection_mode instead
* Set this to true to enable max clique inlier selection, false to skip it.
*/
bool use_max_clique = true;
/**
* \deprecated Use inlier_selection_mode instead
* Set this to false to enable heuristic only max clique finding.
*/
bool max_clique_exact_solution = true;
/**
* Time limit on running the max clique algorithm (in seconds).
*/
double max_clique_time_limit = 3600;
/**
* Number of threads used for the maximum clique solver
*/
int max_clique_num_threads = omp_get_max_threads();
};
RobustRegistrationSolver() = default;
/**
* A constructor that takes in parameters and initialize the estimators accordingly.
*
* This is the preferred way of initializing the different estimators, instead of setting
* each estimator one by one.
* @param params
*/
RobustRegistrationSolver(const Params& params);
/**
* Given a 3-by-N matrix representing points, return Translation Invariant Measurements (TIMs)
* @param v a 3-by-N matrix
* @return a 3-by-(N-1)*N matrix representing TIMs
*/
Eigen::Matrix<double, 3, Eigen::Dynamic>
computeTIMs(const Eigen::Matrix<double, 3, Eigen::Dynamic>& v,
Eigen::Matrix<int, 2, Eigen::Dynamic>* map);
/**
* Solve for scale, translation and rotation.
*
* @param src_cloud source point cloud (to be transformed)
* @param dst_cloud target point cloud (after transformation)
* @param correspondences A vector of tuples representing the correspondences between pairs of
* points in the two clouds
*/
RegistrationSolution solve(const teaser::PointCloud& src_cloud,
const teaser::PointCloud& dst_cloud,
const std::vector<std::pair<int, int>> correspondences);
/**
* Solve for scale, translation and rotation. Assumes dst is src after transformation.
* @param src
* @param dst
*/
RegistrationSolution solve(const Eigen::Matrix<double, 3, Eigen::Dynamic>& src,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& dst);
/**
* Solve for scale. Assume v2 = s * R * v1, this function estimates s.
* @param v1
* @param v2
*/
double solveForScale(const Eigen::Matrix<double, 3, Eigen::Dynamic>& v1,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& v2);
/**
* Solve for translation.
* @param v1
* @param v2
*/
Eigen::Vector3d solveForTranslation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& v1,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& v2);
/**
* Solve for rotation. Assume v2 = R * v1, this function estimates find R.
* @param v1
* @param v2
*/
Eigen::Matrix3d solveForRotation(const Eigen::Matrix<double, 3, Eigen::Dynamic>& v1,
const Eigen::Matrix<double, 3, Eigen::Dynamic>& v2);
/**
* Return the cost at termination of the GNC rotation solver. Can be used to
* assess the quality of the solution.
*
* @return cost at termination of the GNC solver. Undefined if run before running the solver.
*/
inline double getGNCRotationCostAtTermination() {
return rotation_solver_->getCostAtTermination();
}
/**
* Return the solution to the registration problem.
* @return
*/
inline RegistrationSolution getSolution() { return solution_; };
/**
* Set the scale estimator used
* @param estimator
*/
inline void setScaleEstimator(std::unique_ptr<AbstractScaleSolver> estimator) {
scale_solver_ = std::move(estimator);
}
/**
* Set the rotation estimator used.
*
* Note: due to the fact that rotation estimator takes in a noise bound that is dependent on the
* estimated scale, make sure to set the correct params before calling solve.
* @param estimator
*/
inline void setRotationEstimator(std::unique_ptr<GNCRotationSolver> estimator) {
rotation_solver_ = std::move(estimator);
}
/**
* Set the translation estimator used.
* @param estimator
*/
inline void setTranslationEstimator(std::unique_ptr<AbstractTranslationSolver> estimator) {
translation_solver_ = std::move(estimator);
}
/**
* Return a boolean Eigen row vector indicating whether specific measurements are inliers
* according to scales.
*
* @return a 1-by-(number of TIMs) boolean Eigen matrix
*/
inline Eigen::Matrix<bool, 1, Eigen::Dynamic> getScaleInliersMask() {
return scale_inliers_mask_;
}
/**
* Return the index map for scale inliers (equivalent to the index map for TIMs).
*
* @return a 2-by-(number of TIMs) Eigen matrix. Entries in one column represent the indices of
* the two measurements used to calculate the corresponding TIM.
*/
inline Eigen::Matrix<int, 2, Eigen::Dynamic> getScaleInliersMap() { return src_tims_map_; }
/**
* Return inlier TIMs from scale estimation
*
* @return a vector of tuples. Entries in each tuple represent the indices of
* the two measurements used to calculate the corresponding TIM: measurement at indice 0 minus
* measurement at indice 1.
*/
inline std::vector<std::tuple<int, int>> getScaleInliers() {
std::vector<std::tuple<int, int>> result;
for (size_t i = 0; i < scale_inliers_mask_.cols(); ++i) {
if (scale_inliers_mask_(i)) {
result.emplace_back(src_tims_map_(0, i), src_tims_map_(1, i));
}
}
return result;
}
/**
* Return a boolean Eigen row vector indicating whether specific measurements are inliers
* according to the rotation solver.
*
* @return a 1-by-(size of TIMs used in rotation estimation) boolean Eigen matrix. It is
* equivalent to a binary mask on the TIMs used in rotation estimation, with true representing
* that the measurement is an inlier after rotation estimation.
*/
inline Eigen::Matrix<bool, 1, Eigen::Dynamic> getRotationInliersMask() {
return rotation_inliers_mask_;
}
/**
* Return the index map for translation inliers (equivalent to max clique).
* /TODO: This is obsolete now. Remove or update
*
* @return a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original
* measurements.
*/
inline Eigen::Matrix<int, 1, Eigen::Dynamic> getRotationInliersMap() {
Eigen::Matrix<int, 1, Eigen::Dynamic> map = Eigen::Map<Eigen::Matrix<int, 1, Eigen::Dynamic>>(
max_clique_.data(), 1, max_clique_.size());
return map;
}
/**
* Return inliers from rotation estimation
*
* @return a vector of indices of TIMs passed to rotation estimator deemed as inliers by rotation
* estimation. Note that depending on the rotation_tim_graph parameter, number of TIMs passed to
* rotation estimation will be different.
*/
inline std::vector<int> getRotationInliers() { return rotation_inliers_; }
/**
* Return a boolean Eigen row vector indicating whether specific measurements are inliers
* according to translation measurements.
*
* @return a 1-by-(size of max clique) boolean Eigen matrix. It is equivalent to a binary mask on
* the inlier max clique, with true representing that the measurement is an inlier after
* translation estimation.
*/
inline Eigen::Matrix<bool, 1, Eigen::Dynamic> getTranslationInliersMask() {
return translation_inliers_mask_;
}
/**
* Return the index map for translation inliers (equivalent to max clique).
*
* @return a 1-by-(size of max clique) Eigen matrix. Entries represent the indices of the original
* measurements.
*/
inline Eigen::Matrix<int, 1, Eigen::Dynamic> getTranslationInliersMap() {
Eigen::Matrix<int, 1, Eigen::Dynamic> map = Eigen::Map<Eigen::Matrix<int, 1, Eigen::Dynamic>>(
max_clique_.data(), 1, max_clique_.size());
return map;
}
/**
* Return inliers from translation estimation
*
* @return a vector of indices of measurements deemed as inliers by translation estimation
*/
inline std::vector<int> getTranslationInliers() { return translation_inliers_; }
/**
* Return input-ordered inliers from translation estimation
*
* @return a vector of indices of given input correspondences deemed as inliers
* by translation estimation.
*/
inline std::vector<int> getInputOrderedTranslationInliers() {
if (params_.rotation_estimation_algorithm == ROTATION_ESTIMATION_ALGORITHM::FGR) {
throw std::runtime_error(
"This function is not supported when using FGR since FGR does not use max clique.");
}
std::vector<int> translation_inliers;
translation_inliers.reserve(translation_inliers_.size());
for (const auto& i : translation_inliers_) {
translation_inliers.emplace_back(max_clique_[i]);
}
return translation_inliers;
}
/**
* Return a boolean Eigen row vector indicating whether specific measurements are inliers
* according to translation measurements.
* @return
*/
inline std::vector<int> getInlierMaxClique() { return max_clique_; }
inline std::vector<std::vector<int>> getInlierGraph() { return inlier_graph_.getAdjList(); }
/**
* Get TIMs built from source point cloud.
* @return
*/
inline Eigen::Matrix<double, 3, Eigen::Dynamic> getSrcTIMs() { return src_tims_; }
/**
* Get TIMs built from target point cloud.
* @return
*/
inline Eigen::Matrix<double, 3, Eigen::Dynamic> getDstTIMs() { return dst_tims_; }
/**
* Get src TIMs built after max clique pruning.
* @return
*/
inline Eigen::Matrix<double, 3, Eigen::Dynamic> getMaxCliqueSrcTIMs() { return pruned_src_tims_; }
/**
* Get dst TIMs built after max clique pruning.
* @return
*/
inline Eigen::Matrix<double, 3, Eigen::Dynamic> getMaxCliqueDstTIMs() { return pruned_dst_tims_; }
/**
* Get the index map of the TIMs built from source point cloud.
* @return
*/
inline Eigen::Matrix<int, 2, Eigen::Dynamic> getSrcTIMsMap() { return src_tims_map_; }
/**
* Get the index map of the TIMs built from target point cloud.
* @return
*/
inline Eigen::Matrix<int, 2, Eigen::Dynamic> getDstTIMsMap() { return dst_tims_map_; }
/**
* Get the index map of the TIMs used in rotation estimation.
* @return
*/
inline Eigen::Matrix<int, 2, Eigen::Dynamic> getSrcTIMsMapForRotation() {
return src_tims_map_rotation_;
}
/**
* Get the index map of the TIMs used in rotation estimation.
* @return
*/
inline Eigen::Matrix<int, 2, Eigen::Dynamic> getDstTIMsMapForRotation() {
return dst_tims_map_rotation_;
}
/**
* Reset the solver using the provided params
* @param params a Params struct
*/
void reset(const Params& params) {
params_ = params;
// Initialize the scale estimator
if (params_.estimate_scaling) {
setScaleEstimator(
std::make_unique<teaser::TLSScaleSolver>(params_.noise_bound, params_.cbar2));
} else {
setScaleEstimator(
std::make_unique<teaser::ScaleInliersSelector>(params_.noise_bound, params_.cbar2));
}
// Initialize the rotation estimator
teaser::GNCRotationSolver::Params rotation_params{
params_.rotation_max_iterations, params_.rotation_cost_threshold,
params_.rotation_gnc_factor, params_.noise_bound};
switch (params_.rotation_estimation_algorithm) {
case ROTATION_ESTIMATION_ALGORITHM::GNC_TLS: { // GNC-TLS method
setRotationEstimator(std::make_unique<teaser::GNCTLSRotationSolver>(rotation_params));
break;
}
case ROTATION_ESTIMATION_ALGORITHM::FGR: { // FGR method
setRotationEstimator(std::make_unique<teaser::FastGlobalRegistrationSolver>(rotation_params));
break;
}
case ROTATION_ESTIMATION_ALGORITHM::QUATRO: { // Quatro method
setRotationEstimator(std::make_unique<teaser::QuatroSolver>(rotation_params));
break;
}
}
// Initialize the translation estimator
setTranslationEstimator(
std::make_unique<teaser::TLSTranslationSolver>(params_.noise_bound, params_.cbar2));
// Clear member variables
max_clique_.clear();
rotation_inliers_.clear();
translation_inliers_.clear();
inlier_graph_.clear();
}
/**
* Return the params
* @return a Params struct
*/
Params getParams() { return params_; }
private:
Params params_;
RegistrationSolution solution_;
// Inlier Binary Vectors
Eigen::Matrix<bool, 1, Eigen::Dynamic> scale_inliers_mask_;
Eigen::Matrix<bool, 1, Eigen::Dynamic> rotation_inliers_mask_;
Eigen::Matrix<bool, 1, Eigen::Dynamic> translation_inliers_mask_;
// TIMs
// TIMs used for scale estimation/pruning
Eigen::Matrix<double, 3, Eigen::Dynamic> src_tims_;
Eigen::Matrix<double, 3, Eigen::Dynamic> dst_tims_;
// TIMs used for rotation estimation
Eigen::Matrix<double, 3, Eigen::Dynamic> pruned_src_tims_;
Eigen::Matrix<double, 3, Eigen::Dynamic> pruned_dst_tims_;
// TIM maps
// for scale estimation
Eigen::Matrix<int, 2, Eigen::Dynamic> src_tims_map_;
Eigen::Matrix<int, 2, Eigen::Dynamic> dst_tims_map_;
// for rotation estimation
Eigen::Matrix<int, 2, Eigen::Dynamic> src_tims_map_rotation_;
Eigen::Matrix<int, 2, Eigen::Dynamic> dst_tims_map_rotation_;
// Max clique vector
std::vector<int> max_clique_;
// Inliers after rotation estimation
std::vector<int> rotation_inliers_;
// Inliers after translation estimation (final inliers)
std::vector<int> translation_inliers_;
// Inlier graph
teaser::Graph inlier_graph_;
// Ptrs to Solvers
std::unique_ptr<AbstractScaleSolver> scale_solver_;
std::unique_ptr<GNCRotationSolver> rotation_solver_;
std::unique_ptr<AbstractTranslationSolver> translation_solver_;
};
} // namespace teaser