[registration] GICP alternates optimizing trans and rot part

registration/include/pcl/registration/gicp.h

@@ -100,7 +100,7 @@ class GeneralizedIterativeClosestPoint
   using ConstPtr =
       shared_ptr<const GeneralizedIterativeClosestPoint<PointSource, PointTarget>>;
 
-  using Vector6d = Eigen::Matrix<double, 6, 1>;
+  using Vector6d = typename Eigen::Matrix<double, 6, 1>;
 
   /** \brief Empty constructor. */
   GeneralizedIterativeClosestPoint()
@@ -111,6 +111,8 @@ class GeneralizedIterativeClosestPoint
   , max_inner_iterations_(20)
   , translation_gradient_tolerance_(1e-2)
   , rotation_gradient_tolerance_(1e-2)
+  , is_alternating_(false)
+  , is_translation_turn_(true)
   {
     min_number_correspondences_ = 4;
     reg_name_ = "GeneralizedIterativeClosestPoint";
@@ -314,6 +316,26 @@ class GeneralizedIterativeClosestPoint
     return rotation_gradient_tolerance_;
   }
 
+  /** \brief Set whether or not optimizing translation part and rotation part
+   * alternatively
+   * \param[in] is_alternating whether or not optimizing translation part
+   * and rotation part alternatively
+   */
+  void
+  setIsAlternating(bool is_alternating)
+  {
+    is_alternating_ = is_alternating;
+  }
+
+  /** \brief Return whether or not optimizing translation part and rotation part
+   * alternatively
+   */
+  bool
+  getIsAlternating() const
+  {
+    return is_alternating_;
+  }
+
 protected:
   /** \brief The number of neighbors used for covariances computation.
    * default: 20
@@ -365,6 +387,16 @@ 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 or not optimizing translation part and rotation part alternatively
+   */
+  bool is_alternating_;
+
+  /** \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
@@ -440,6 +472,41 @@ class GeneralizedIterativeClosestPoint
     const GeneralizedIterativeClosestPoint* gicp_;
   };
 
+  /// \brief optimization functor structure for translation part
+  struct OptimizationFunctorWithIndicesTranslation
+  : public BFGSDummyFunctor<double, 3> {
+    OptimizationFunctorWithIndicesTranslation(GeneralizedIterativeClosestPoint* gicp)
+    : 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 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_;
+  };
+
   std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
                      const pcl::Indices& src_indices,
                      const pcl::PointCloud<PointTarget>& cloud_tgt,