GICP: add Newton optimizer

registration/include/pcl/registration/gicp.h

@@ -49,8 +49,9 @@ namespace pcl {
  * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
  * The approach is based on using anisotropic cost functions to optimize the alignment
  * after closest point assignments have been made.
- * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and
- * FLANN.
+ * The original code uses GSL and ANN while in ours we use FLANN and Newton's method
+ * for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton
+ * is usually faster and more accurate).
  * \author Nizar Sallem
  * \ingroup registration
  */
@@ -111,6 +112,7 @@ class GeneralizedIterativeClosestPoint
   using Matrix3 = typename Eigen::Matrix<Scalar, 3, 3>;
   using Matrix4 =
       typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
+  using Matrix6d = Eigen::Matrix<double, 6, 6>;
   using AngleAxis = typename Eigen::AngleAxis<Scalar>;
 
   PCL_MAKE_ALIGNED_OPERATOR_NEW
@@ -135,7 +137,7 @@ class GeneralizedIterativeClosestPoint
                                               const PointCloudTarget& cloud_tgt,
                                               const pcl::Indices& indices_tgt,
                                               Matrix4& transformation_matrix) {
-      estimateRigidTransformationBFGS(
+      estimateRigidTransformationNewton(
           cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
     };
   }
@@ -214,6 +216,23 @@ class GeneralizedIterativeClosestPoint
                                   const pcl::Indices& indices_tgt,
                                   Matrix4& transformation_matrix);
 
+  /** \brief Estimate a rigid rotation transformation between a source and a target
+   * point cloud using an iterative non-linear Newton approach.
+   * \param[in] cloud_src the source point cloud dataset
+   * \param[in] indices_src the vector of indices describing
+   * the points of interest in \a cloud_src
+   * \param[in] cloud_tgt the target point cloud dataset
+   * \param[in] indices_tgt the vector of indices describing
+   * the correspondences of the interest points from \a indices_src
+   * \param[in,out] transformation_matrix the resultant transformation matrix
+   */
+  void
+  estimateRigidTransformationNewton(const PointCloudSource& cloud_src,
+                                    const pcl::Indices& indices_src,
+                                    const PointCloudTarget& cloud_tgt,
+                                    const pcl::Indices& indices_tgt,
+                                    Matrix4& transformation_matrix);
+
   /** \brief \return Mahalanobis distance matrix for the given point index */
   inline const Eigen::Matrix3d&
   mahalanobis(std::size_t index) const
@@ -276,6 +295,21 @@ class GeneralizedIterativeClosestPoint
     return k_correspondences_;
   }
 
+  /** \brief Use BFGS optimizer instead of default Newton optimizer
+   */
+  void
+  useBFGS()
+  {
+    rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
+                                              const pcl::Indices& indices_src,
+                                              const PointCloudTarget& cloud_tgt,
+                                              const pcl::Indices& indices_tgt,
+                                              Matrix4& transformation_matrix) {
+      estimateRigidTransformationBFGS(
+          cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+    };
+  }
+
   /** \brief Set maximum number of iterations at the optimization step
    * \param[in] max maximum number of iterations for the optimizer
    */
@@ -447,6 +481,8 @@ class GeneralizedIterativeClosestPoint
     df(const Vector6d& x, Vector6d& df) override;
     void
     fdf(const Vector6d& x, double& f, Vector6d& df) override;
+    void
+    dfddf(const Vector6d& x, Vector6d& df, Matrix6d& ddf);
     BFGSSpace::Status
     checkGradient(const Vector6d& g) override;
 
@@ -459,6 +495,26 @@ class GeneralizedIterativeClosestPoint
                      const pcl::Indices& tgt_indices,
                      Matrix4& transformation_matrix)>
       rigid_transformation_estimation_;
+
+private:
+  void
+  getRDerivatives(double phi,
+                  double theta,
+                  double psi,
+                  Eigen::Matrix3d& dR_dPhi,
+                  Eigen::Matrix3d& dR_dTheta,
+                  Eigen::Matrix3d& dR_dPsi) const;
+
+  void
+  getR2ndDerivatives(double phi,
+                     double theta,
+                     double psi,
+                     Eigen::Matrix3d& ddR_dPhi_dPhi,
+                     Eigen::Matrix3d& ddR_dPhi_dTheta,
+                     Eigen::Matrix3d& ddR_dPhi_dPsi,
+                     Eigen::Matrix3d& ddR_dTheta_dTheta,
+                     Eigen::Matrix3d& ddR_dTheta_dPsi,
+                     Eigen::Matrix3d& ddR_dPsi_dPsi) const;
 };
 } // namespace pcl