Report final ICP transformation as a composition individual steps

1. Center all data using centroid of fixed cloud
2. Compute transformation matrix to bring moving cloud into alignment
   with fixed
3. Undo centering step to place all data in proper global location

filters/IterativeClosestPoint.cpp

@@ -115,9 +115,9 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
     PointViewPtr tempFixed = demeanPointView(*fixed, centroid.data());
     PointViewPtr tempMoving = demeanPointView(*moving, centroid.data());
 
-    // Initialize the final_transformation to identity. In the future, it would
+    // Initialize the final_xform to identity. In the future, it would
     // be reasonable to alternately accept an initial guess.
-    Eigen::Matrix4d final_transformation = Eigen::Matrix4d::Identity();
+    Eigen::Matrix4d final_xform = Eigen::Matrix4d::Identity();
 
     // Construct 3D KD-tree of the centered, fixed PointView to facilitate
     // nearest neighbor searches in each iteration.
@@ -130,9 +130,9 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
     for (int iter = 0; iter < m_max_iters; ++iter)
     {
         // At the beginning of each iteration, transform our centered, moving
-        // PointView by the current final_transformation.
+        // PointView by the current final_xform.
         PointViewPtr tempMovingTransformed =
-            transform(*tempMoving, final_transformation.data());
+            transform(*tempMoving, final_xform.data());
 
         // Create empty lists to hold point correspondences, and initialize MSE
         // to zero.
@@ -174,11 +174,11 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
         log()->get(LogLevel::Debug2) << "Current dx: " << T.coeff(0, 3) << ", "
                                      << "dy: " << T.coeff(1, 3) << std::endl;
 
-        // Update the final_transformation and log the X and Y translations.
-        final_transformation = final_transformation * T;
+        // Update the final_xform and log the X and Y translations.
+        final_xform = final_xform * T;
         log()->get(LogLevel::Debug2)
-            << "Cumulative dx: " << final_transformation.coeff(0, 3) << ", "
-            << "dy: " << final_transformation.coeff(1, 3) << std::endl;
+            << "Cumulative dx: " << final_xform.coeff(0, 3) << ", "
+            << "dy: " << final_xform.coeff(1, 3) << std::endl;
 
         bool is_similar = false;
 
@@ -228,7 +228,7 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
         prev_mse = mse;
     }
 
-    // Apply the final_transformation to the moving PointView.
+    // Apply the final_xform to the moving PointView.
     for (PointId i = 0; i < moving->size(); ++i)
     {
         double x =
@@ -238,20 +238,20 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
         double z =
             moving->getFieldAs<double>(Dimension::Id::Z, i) - centroid.z();
         moving->setField(Dimension::Id::X, i,
-                         x * final_transformation.coeff(0, 0) +
-                             y * final_transformation.coeff(0, 1) +
-                             z * final_transformation.coeff(0, 2) +
-                             final_transformation.coeff(0, 3) + centroid.x());
+                         x * final_xform.coeff(0, 0) +
+                             y * final_xform.coeff(0, 1) +
+                             z * final_xform.coeff(0, 2) +
+                             final_xform.coeff(0, 3) + centroid.x());
         moving->setField(Dimension::Id::Y, i,
-                         x * final_transformation.coeff(1, 0) +
-                             y * final_transformation.coeff(1, 1) +
-                             z * final_transformation.coeff(1, 2) +
-                             final_transformation.coeff(1, 3) + centroid.y());
+                         x * final_xform.coeff(1, 0) +
+                             y * final_xform.coeff(1, 1) +
+                             z * final_xform.coeff(1, 2) +
+                             final_xform.coeff(1, 3) + centroid.y());
         moving->setField(Dimension::Id::Z, i,
-                         x * final_transformation.coeff(2, 0) +
-                             y * final_transformation.coeff(2, 1) +
-                             z * final_transformation.coeff(2, 2) +
-                             final_transformation.coeff(2, 3) + centroid.z());
+                         x * final_xform.coeff(2, 0) +
+                             y * final_xform.coeff(2, 1) +
+                             z * final_xform.coeff(2, 2) +
+                             final_xform.coeff(2, 3) + centroid.z());
     }
 
     // Compute the MSE one last time, using the unaltered, fixed PointView and
@@ -269,10 +269,23 @@ PointViewPtr IterativeClosestPoint::icp(PointViewPtr fixed,
     mse /= moving->size();
     log()->get(LogLevel::Debug2) << "MSE: " << mse << std::endl;
 
+    // Transformation to demean coords
+    Eigen::Matrix4d pretrans = Eigen::Matrix4d::Identity();
+    pretrans.block<3, 1>(0, 3) = -centroid;
+
+    // Transformation to return to global coords
+    Eigen::Matrix4d posttrans = Eigen::Matrix4d::Identity();
+    posttrans.block<3, 1>(0, 3) = centroid;
+
+    // The composed transformation is built from right to left in order of
+    // operations.
+    Eigen::Matrix4d composed_xform = posttrans * final_xform * pretrans;
+
     // Populate metadata nodes to capture the final transformation, convergence
     // status, and MSE.
     MetadataNode root = getMetadata();
-    root.add("transform", Eigen::MatrixXd(final_transformation.cast<double>()));
+    root.add("transform", Eigen::MatrixXd(final_xform.cast<double>()));
+    root.add("composed", Eigen::MatrixXd(composed_xform.cast<double>()));
     root.add("centroid", Eigen::MatrixXd(centroid.cast<double>()));
     root.add("converged", converged);
     root.add("fitness", mse);