[feat] gicp: add convergence criteria and correspondence estimation #5287
Conversation
| @@ -102,6 +114,9 @@ class GeneralizedIterativeClosestPoint | |||
|
|
|||
| using Vector6d = Eigen::Matrix<double, 6, 1>; | |||
|
|
|||
| typename pcl::registration::GICPConvergenceCriteria<float>::Ptr | |||
There was a problem hiding this comment.
GeneralizedIterativeClosestPoint inherits from IterativeClosestPoint<PointSource, PointTarget, Scalar = float> so use float here.
| * GeneralizedIterativeClosestPoint class. This allows to check the convergence state after the | ||
| * align() method as well as to configure GICPConvergenceCriteria's parameters not | ||
| * available through the GICP API before the align() method is called. Please note that | ||
| * the align method sets max_iterations_, transformation_epsilon_ and |
There was a problem hiding this comment.
ICP version: max_iterations_, euclidean_fitness_epsilon_, transformation_epsilon_
GICP version: max_iterations_, transformation_epsilon_, rotation_epsilon_
| } | ||
|
|
||
| convergence_criteria_->setMaximumIterations(max_iterations_); | ||
| //convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_); |
There was a problem hiding this comment.
Original gicp didn't check relative rmse
| convergence_criteria_->setMaximumIterations(max_iterations_); | ||
| //convergence_criteria_->setRelativeMSE(euclidean_fitness_epsilon_); | ||
| convergence_criteria_->setTranslationThreshold(transformation_epsilon_); | ||
| convergence_criteria_->setRotationThreshold(rotation_epsilon_); |
There was a problem hiding this comment.
In ICP this line is:
if (transformation_rotation_epsilon_ > 0)
convergence_criteria_->setRotationThreshold(transformation_rotation_epsilon_);
else
convergence_criteria_->setRotationThreshold(1.0 - transformation_epsilon_);
But gicp has its own rotation_epsilon_ so changed to something like this.
| input_transformed_blob.reset(new PCLPointCloud2); | ||
| toPCLPointCloud2(*input_transformed, *input_transformed_blob); | ||
| } | ||
|
|
There was a problem hiding this comment.
In ICP, here's:
// Save the previously estimated transformation
previous_transformation_ = transformation_;But there's previous_transformation_ = transformation_; below in GICP, so delete this two lines.
| const pcl::Correspondences& correspondences) | ||
| : iterations_(iterations) | ||
| , transformation_(transform) | ||
| , previous_transformation_ (previous_transform) |
There was a problem hiding this comment.
Unlike ICP, in GICP transformation_ and previous_transformation_ are both transform from input cloud. To get the transformation in each iteration, we need both of them.
| , max_iterations_(100) // 100 iterations | ||
| , failure_after_max_iter_(false) | ||
| , rotation_threshold_(0.99999) // 0.256 degrees | ||
| , rotation_epsilon_ (2e-3) |
There was a problem hiding this comment.
Copied from gicp.h
| convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED; | ||
| } | ||
|
|
||
| bool is_similar = false; |
There was a problem hiding this comment.
Althrough in original gicp, it exit once delta < 1, but maybe exiting after max_iterations_similar_transforms_ iterations (like icp) would be better.
| if (c_delta > delta) | ||
| delta = c_delta; | ||
| } | ||
| } |
There was a problem hiding this comment.
Copied from gicp.hpp
| return (true); | ||
| } | ||
| is_similar = true; | ||
| } |
There was a problem hiding this comment.
There's no absolute mse and relative mse check in original gicp's code.
|
@mvieth Do you have any idea how long it will it take to review this pull request? Because I would like to create another PR that would revise gicp.hpp a lot(adding Scalar template variable). |
|
seems useful, is there anything in particular blocking this other than lack of time or attention? |
|
@mvieth If this PR is planned to be reviewed, I could resolve the conflicts recently. |
Attempt to fix #3329.