Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[registration] GICP alternates optimizing trans and rot part #5269

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

[registration] GICP alternates optimizing trans and rot part #5269

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
44577cc
[feat] gicp alternates optimizing trans and rot part
keineahnung2345 May 20, 2022
014cb9d
[fix] error: missing 'template' keyword prior to dependent template n…
keineahnung2345 May 20, 2022
1c29c4d
Fix "error: missing 'template' keyword prior to dependent template na…
keineahnung2345 May 30, 2022
156451f
Merge branch 'master' into gicp_alternating
keineahnung2345 Jun 7, 2022
bbe217f
[feat] add is_alternating_ flag to turn alternating algorithm on/off
keineahnung2345 Jun 8, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 34 additions & 9 deletions registration/include/pcl/registration/gicp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,7 @@ class GeneralizedIterativeClosestPoint
, max_inner_iterations_(20)
, translation_gradient_tolerance_(1e-2)
, rotation_gradient_tolerance_(1e-2)
, is_translation_turn_(true)
{
min_number_correspondences_ = 4;
reg_name_ = "GeneralizedIterativeClosestPoint";
Expand Down Expand Up @@ -365,6 +366,12 @@ class GeneralizedIterativeClosestPoint
/** \brief minimal rotation gradient for early optimization stop */
double rotation_gradient_tolerance_;

/** \brief current transformation of source point cloud */
Vector6d current_transformation_;

/** \brief whether current iteration is optimizing translation part or not */
bool is_translation_turn_;

/** \brief compute points covariances matrices according to the K nearest
* neighbors. K is set via setCorrespondenceRandomness() method.
* \param cloud pointer to point cloud
Expand Down Expand Up @@ -423,21 +430,39 @@ class GeneralizedIterativeClosestPoint
void
applyState(Eigen::Matrix4f& t, const Vector6d& x) const;

/// \brief optimization functor structure
struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double, 6> {
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint* gicp)
: BFGSDummyFunctor<double, 6>(), gicp_(gicp)
/// \brief optimization functor structure for translation part
struct OptimizationFunctorWithIndicesTranslation
: public BFGSDummyFunctor<double, 3> {
OptimizationFunctorWithIndicesTranslation(GeneralizedIterativeClosestPoint* gicp)
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Need to update gicp_->current_transformation_ so const from const GeneralizedIterativeClosestPoint* removed.

: BFGSDummyFunctor<double, 3>(), gicp_(gicp)
{}
double
operator()(const Eigen::Vector3d& x) override;
void
df(const Eigen::Vector3d& x, Eigen::Vector3d& df) override;
void
fdf(const Eigen::Vector3d& x, double& f, Eigen::Vector3d& df) override;
BFGSSpace::Status
checkGradient(const Eigen::Vector3d& g) override;

GeneralizedIterativeClosestPoint* gicp_;
};

/// \brief optimization functor structure for rotation part
struct OptimizationFunctorWithIndicesRotation : public BFGSDummyFunctor<double, 3> {
OptimizationFunctorWithIndicesRotation(GeneralizedIterativeClosestPoint* gicp)
: BFGSDummyFunctor<double, 3>(), gicp_(gicp)
{}
double
operator()(const Vector6d& x) override;
operator()(const Eigen::Vector3d& x) override;
void
df(const Vector6d& x, Vector6d& df) override;
df(const Eigen::Vector3d& x, Eigen::Vector3d& df) override;
void
fdf(const Vector6d& x, double& f, Vector6d& df) override;
fdf(const Eigen::Vector3d& x, double& f, Eigen::Vector3d& df) override;
BFGSSpace::Status
checkGradient(const Vector6d& g) override;
checkGradient(const Eigen::Vector3d& g) override;

const GeneralizedIterativeClosestPoint* gicp_;
GeneralizedIterativeClosestPoint* gicp_;
};

std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
Expand Down
Loading