Extend registration&icp to fourth template parameter

Author: mcopik

registration/include/pcl/registration/icp.h

@@ -90,8 +90,9 @@ namespace pcl
     * \author Radu B. Rusu, Michael Dixon
     * \ingroup registration
     */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
+  template <typename PointSource, typename PointTarget, typename Scalar = float,
+		  typename MatrixType = Eigen::Matrix<Scalar,4,4>  >
+  class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar, MatrixType>
   {
     public:
       typedef typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource PointCloudSource;
@@ -105,7 +106,7 @@ namespace pcl
       typedef PointIndices::Ptr PointIndicesPtr;
       typedef PointIndices::ConstPtr PointIndicesConstPtr;
 
-      typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
+      typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType> > Ptr;
       typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
 
       using Registration<PointSource, PointTarget, Scalar>::reg_name_;
@@ -255,14 +256,14 @@ namespace pcl
       virtual void 
       transformCloud (const PointCloudSource &input, 
                       PointCloudSource &output, 
-                      const Matrix4 &transform);
+                      const MatrixType &transform);
 
       /** \brief Rigid transformation computation method  with initial guess.
         * \param output the transformed input point cloud dataset using the rigid transformation found
         * \param guess the initial guess of the transformation to compute
         */
       virtual void 
-      computeTransformation (PointCloudSource &output, const Matrix4 &guess);
+      computeTransformation (PointCloudSource &output, const MatrixType &guess);
 
       /** \brief XYZ fields offset. */
       size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
@@ -286,20 +287,21 @@ namespace pcl
     * \author Radu B. Rusu
     * \ingroup registration
     */
-  template <typename PointSource, typename PointTarget, typename Scalar = float>
-  class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
+  template <typename PointSource, typename PointTarget, typename Scalar = float,
+		  typename MatrixType = Eigen::Matrix<Scalar,4,4>  >
+  class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>
   {
     public:
-      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource PointCloudSource;
-      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget PointCloudTarget;
-      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
+      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::PointCloudSource PointCloudSource;
+      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::PointCloudTarget PointCloudTarget;
+      typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::Matrix4 Matrix4;
 
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_;
-      using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
+      using IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::reg_name_;
+      using IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::transformation_estimation_;
+      using IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType>::correspondence_rejectors_;
 
-      typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
-      typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
+      typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType> > Ptr;
+      typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar, MatrixType> > ConstPtr;
 
       /** \brief Empty constructor. */
       IterativeClosestPointWithNormals () 
@@ -323,7 +325,7 @@ namespace pcl
       virtual void 
       transformCloud (const PointCloudSource &input, 
                       PointCloudSource &output, 
-                      const Matrix4 &transform);
+                      const MatrixType &transform);
   };
 }