Improve robustness of ICP solve when closest points and normals are degenerate (#2107)

* fix hard coded double + template

* oops wrong template type

* more robust solve

include/igl/rigid_alignment.cpp

@@ -7,7 +7,9 @@
 // obtain one at http://mozilla.org/MPL/2.0/.
 #include "rigid_alignment.h"
 #include <Eigen/Sparse>
-#include <Eigen/Cholesky>
+#include <Eigen/QR>
+// Not currently used. See below.
+//#include <Eigen/Cholesky>
 #include <vector>
 #include <iostream>
 
@@ -38,6 +40,8 @@ IGL_INLINE void igl::rigid_alignment(
   t = Derivedt::Zero(1,3);
   // See gptoolbox, each iter could be O(1) instead of O(k)
   const int max_iters = 5;
+  // Weight on point-to-point regularization.
+  Scalar w = 1e-5;
   for(int iters = 0;iters<max_iters;iters++)
   {
     MatrixXS A(k*3,6);
@@ -50,6 +54,8 @@ IGL_INLINE void igl::rigid_alignment(
       P.col(0)-X.col(0),
       P.col(1)-X.col(1),
       P.col(2)-X.col(2);
+
+
     std::vector<Eigen::Triplet<Scalar> > NNIJV;
     for(int i = 0;i<k;i++)
     {
@@ -60,11 +66,24 @@ IGL_INLINE void igl::rigid_alignment(
     }
     Eigen::SparseMatrix<Scalar> NN(k,k*3);
     NN.setFromTriplets(NNIJV.begin(),NNIJV.end());
-    A = (NN * A).eval();
-    B = (NN * B).eval();
-    VectorXS u = (A.transpose() * A).ldlt().solve(A.transpose() * B);
-    Derivedt ti = u.tail(3).transpose();
 
+    MatrixXS NA = (NN * A).eval();
+    VectorXS NB = (NN * B).eval();
+
+    MatrixXS Q = (1.0-w)*(NA.transpose() * NA) + w * A.transpose() * A;
+    VectorXS f = (1.0-w)*(NA.transpose() * NB) + w * A.transpose() * B;
+    // This could be a lot faster but isn't rank revealing and may produce wonky
+    // results when P and N are all the same point and vector.
+    //VectorXS u = (Q).ldlt().solve(f);
+    
+    Eigen::CompleteOrthogonalDecomposition<decltype(Q)> qr(Q);
+    if(qr.rank() < 6)
+    {
+      w = 1.0-(1.0-w)/2.0;
+    }
+
+    VectorXS u = qr.solve(f);
+    Derivedt ti = u.tail(3).transpose();
     Matrix3S W;
     W<<
           0, u(2),-u(1),