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

@@ -269,10 +269,25 @@ 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_transformation =
+        posttrans * final_transformation * 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("composed",
+             Eigen::MatrixXd(composed_transformation.cast<double>()));
     root.add("centroid", Eigen::MatrixXd(centroid.cast<double>()));
     root.add("converged", converged);
     root.add("fitness", mse);