Feature/arun scaled: sim3 for PC aligment using the Arun method (#44)

* sim3 for PC aligment using the Arun method.

* Fix minor issue, now scaled Arun tested

Author: g-ferrer

python_examples/PC_scaled_align.py

@@ -0,0 +1,53 @@
+#
+# add path to local library mrob on bashr_rc: "export PYTHONPATH=${PYTHONPATH}:${HOME}/mrob/mrob/lib"
+import pybind as mrob
+import numpy as np
+import open3d
+
+# generate random data
+N = 10
+X =  np.random.rand(N,3)
+theta = np.random.randn(3)
+t = np.array([0,0,0])
+T = mrob.geometry.SE3(mrob.geometry.SO3(theta),t)
+Y = T.transform_array(X)
+scale = 1000
+t = np.random.randn(3)
+Y = scale*Y + t + np.random.rand(N,3)*0.01
+
+S = T.T()
+S[:3,:3] *= scale
+S[:3,3] = t
+
+print('X = \n', X,'\n S = \n', S,'\n Y =\n', Y)
+
+
+
+
+def pcd_1(X, color, T = np.identity(4)):
+    pcd = open3d.geometry.PointCloud()
+    pcd.points = open3d.utility.Vector3dVector(X)
+    pcd.transform(T)
+    pcd.paint_uniform_color(color)
+    return pcd
+
+def vis_her(X, Y, T = np.identity(4)):
+    blue = np.array([0,0,1], dtype='float64')
+    red = np.array([1,0,0], dtype='float64')
+    open3d.visualization.draw_geometries([pcd_1(X,red,T), pcd_1(Y,blue)])
+
+def vis_arr(X):
+	pcd = open3d.PointCloud()
+	pcd.points =open3d.utility.Vector3dVector(X)
+	pcd.paint_uniform_color(np.random.rand(3,).astype(np.float64))
+	open3d.visualization.draw_geometries([pcd])
+
+
+
+# solve the problem
+vis_her(X,Y)
+S_sol = mrob.registration.scaled_arun(X,Y)
+print('Arun solution =\n', S_sol)
+print('Initial transformation =\n',S)
+vis_her(X,Y,S_sol)
+

src/PCRegistration/CMakeLists.txt

@@ -6,6 +6,7 @@ SET(sources
     arun.cpp
     gicp.cpp
     weight_point.cpp
+    scaled_arun.cpp
     factors/factor1PosePoint2Point.cpp
     factors/factor1PosePoint2Plane.cpp
 )

src/PCRegistration/arun.cpp

@@ -31,7 +31,7 @@
 using namespace mrob;
 using namespace Eigen;
 
-int PCRegistration::arun(const Ref<const MatX> X, const Ref<const MatX> Y, SE3 &T)
+int PCRegistration::arun(MatRefConst X, MatRefConst Y, SE3 &T)
 {
     assert(X.cols() == 3  && "PCRegistration::Arun: Incorrect sizing, we expect Nx3");
     assert(X.rows() >= 3  && "PCRegistration::Arun: Incorrect sizing, we expect at least 3 correspondences (not aligned)");

src/PCRegistration/mrob/pc_registration.hpp

@@ -96,5 +96,19 @@ int weighted_point(MatRefConst X, MatRefConst Y,
                    VectRefConst w, SE3 &T, double tol = 1e-4);
 
 
+/**
+ *  This solution is based on the paper by Arun et al. "Least-squares fitting of two 3-D point sets", 1987
+ *  Given N points x = x_1,x_2,x_3 and their correspondences y_i, calculate the
+ *  transformation S = [sR t], such as: min sum (y_i - (sR * x_i + t))^2
+ *
+ *  An improvement over it was proposed by Sinji Umeyama 1991, which uniqueness determine R
+ *
+ *  3 points in x and y are the minimum requirements, and provide the right solution IF
+ *  there is no noise.
+ *
+ *  Returns 0 if failed and 1 if a correct solution was found
+ */
+int scaled_arun(MatRefConst X, MatRefConst Y, Mat4 &S);
+
 }}//namespace
 #endif /* PC_REGISTRATION_HPP_ */

src/PCRegistration/scaled_arun.cpp

@@ -0,0 +1,124 @@
+/* Copyright (c) 2022, Gonzalo Ferrer
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * arun.cpp
+ *
+ *  Created on: July 12, 2023
+ *      Author: Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab, Skoltech 
+ */
+
+#include <Eigen/LU>
+#include <Eigen/SVD>
+
+#include <memory>
+#include <iostream>
+#include "mrob/pc_registration.hpp"
+#include <cmath>
+
+using namespace mrob;
+using namespace Eigen;
+
+int PCRegistration::scaled_arun(MatRefConst X, MatRefConst Y, Mat4 &S)
+{
+    assert(X.cols() == 3  && "PCRegistration::Arun: Incorrect sizing, we expect Nx3");
+    assert(X.rows() >= 3  && "PCRegistration::Arun: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
+    assert(Y.rows() == X.rows()  && "PCRegistration::Arun: Same number of correspondences");
+    uint_t N = X.rows();
+    /** Algorithm:
+     *  1) calculate centroids cx = sum x_i. cy = sum y_i
+     *  2) calculate dispersion from centroids qx = x_i - cx
+     *  3) calculate matrix H = sum qx_i * qy_i^T
+     *  4) svd decomposition: H = U*D*V'
+     *      4.5) look for co-linear solutions, that is 2 of the 3 singular values are equal
+     *  5) Calculate the rotation solution R = V*U'
+     *      5.5) check for correct solution (det = +1) or reflection (det = -1)
+     *      step 5.5 is actually unnecessary IF applying Umeyama technique
+     *  6) NEW calcualte scale as
+     *        s = 
+     *  7) NEW calculate translation as: t = cy - s R * cx
+     */
+    // We have already asserted in base_T that they are 3xN matrices. (and the same length).
+
+    //std::cout << "X: \n" << X << "\nY:\n" << Y << std::endl;
+    // 1) calculate centroids cx = E{x_i}. cy = E{y_i}
+    //More efficient than creating a matrix of ones when on Release mode (not is Debug mode)
+    Mat13 cxm = X.colwise().sum();
+    cxm /= (double)N;
+    Mat13 cym = Y.colwise().sum();
+    cym /= (double)N;
+
+    // 2)  calculate dispersion from centroids qx = x_i - cx
+    MatX qx = X.rowwise() - cxm;
+    MatX qy = Y.rowwise() - cym;
+
+
+    // 3) calculate matrix H = sum qx_i * qy_i^T (noting that we are obtaingin row vectors)
+    Mat3 H = qx.transpose() * qy;
+
+    // 4) svd decomposition: H = U*D*V'
+    JacobiSVD<Matrix3d> SVD(H, ComputeFullU | ComputeFullV);//Full matrices indicate Square matrices
+
+    //test: prints results so far
+    /*std::cout << "Checking matrix SVD: \n" << SVD.singularValues() <<
+                 ",\n U = " << SVD.matrixU() <<
+                 ",\n V = " << SVD.matrixV() << std::endl;*/
+
+
+    // 4.5) look for co-linear solutions, that is 2 of the 3 singular values are equal
+    double l_prev = SVD.singularValues()(0), l;
+    for(int i =1; i < 3; ++i)
+    {
+        l = SVD.singularValues()(i);
+        if (fabs(l - l_prev) < 1e-6)
+
+            return 0; //they are co-linear, there exist infinite transformations
+        else
+            l_prev = l;//this works because we assume that they singular values are ordered.
+    }
+
+    // 5) Calculate the rotation solution R = V*U'
+    Mat3 R = SVD.matrixV() * SVD.matrixU().transpose();
+
+    // 5.5) check for correct solution (det = +1) or reflection (det = -1)
+    // that is, solve the problem for co-planar set of points and centroid, when is l1 > l2 > l3 = 0
+    // Since H = D1*u1*v1' + D2*u2*v2' + D3*u3*v3',    and D3 = 0, we can swap signs in V
+    // such as Vp = [v1,v2,-v3] and the solution is still minimal, but we want a valid rotation R \in SO(3)
+    if (R.determinant() < 0.0 )
+    {
+        Mat3 Vn;
+        Vn << SVD.matrixV().topLeftCorner<3,2>(), -SVD.matrixV().topRightCorner<3,1>();
+        R << Vn * SVD.matrixU().transpose();
+        //std::cout << "R value = " << R << std::endl;
+    }
+
+
+    // 6) calculate scale
+    matData_t scale;
+    matData_t qx2_sum = qx.col(0).squaredNorm() + qx.col(1).squaredNorm() + qx.col(2).squaredNorm();
+    matData_t qy2_sum = qy.col(0).squaredNorm() + qy.col(1).squaredNorm() + qy.col(2).squaredNorm();
+    scale = std::sqrt(qy2_sum / qx2_sum);
+
+    // 7) calculate translation as: t = cy - scale *R * cx
+    Mat31 t = cym.transpose() - scale*R*cxm.transpose();
+    //std::cout << "t = " << t << std::endl;
+
+    // 7) return result
+    S << scale * R, t,
+              0,0,0,1;
+
+    return 1;
+}

src/pybind/PCRegistrationPy.cpp

@@ -64,11 +64,20 @@ SE3 weighted_solve(const py::EigenDRef<const MatX> X, const py::EigenDRef<const
     return res;
 }
 
+
+Mat4 scaled_arun_solve(const py::EigenDRef<const MatX> X, const py::EigenDRef<const MatX> Y)
+{
+    Mat4 res;
+    PCRegistration::scaled_arun(X,Y,res);
+    return res;
+}
+
 void init_PCRegistration(py::module &m)
 {
     m.def("arun", &arun_solve);
     m.def("gicp", &gicp_solve);
     m.def("weighted", &weighted_solve);
+    m.def("scaled_arun", &scaled_arun_solve);
 }