p2plane also works now

src/hausdorff_lower_bound.cpp

@@ -11,7 +11,6 @@ double hausdorff_lower_bound(
   // Replace with your code
     Eigen::MatrixXd sampled_X;
     random_points_on_mesh(n,VX,FX,sampled_X);
-
     Eigen::VectorXd D;
     Eigen::MatrixXd P;
     Eigen::MatrixXd N;

src/point_to_plane_rigid_matching.cpp

@@ -1,6 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
 #include <Eigen/Dense>
 #include "closest_rotation.h"
+#include <iostream>
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
   const Eigen::MatrixXd & P,
@@ -10,8 +11,6 @@ void point_to_plane_rigid_matching(
 {
   // Replace with your code
   int k = X.rows();
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
 
   Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * k, 6);
   A.block(k,0, k,1) = -X.col(2);
@@ -34,8 +33,8 @@ void point_to_plane_rigid_matching(
 
   Eigen::MatrixXd C = Eigen::MatrixXd::Zero(k, 3 * k);
   C.block(0,0,k,k) = N.col(0).asDiagonal();
-  C.block(k,0,k,k) = N.col(1).asDiagonal();
-  C.block(2*k,0,k,k) = N.col(2).asDiagonal();
+  C.block(0,k,k,k) = N.col(1).asDiagonal();
+  C.block(0,2*k,k,k) = N.col(2).asDiagonal();
 
   A = C * A;
   B = C * B;
@@ -44,10 +43,9 @@ void point_to_plane_rigid_matching(
 
   u = (A.transpose() * A).inverse() * (- A.transpose() * B);
   Eigen::Matrix3d M;
-  M << 1 , -u(0,2), u(0, 1),
-       u(0,2), 1, -u(0,0),
-       -u(0,1), u(0,0), 1;
-
+  M << 1 , -u(2,0), u(1,0),
+       u(2,0), 1, -u(0,0),
+       -u(1,0), u(0,0), 1;
   closest_rotation(M, R);
-  t << u(0,3), u(0,4), u(0,5); 
+  t << u(3,0), u(4,0), u(5,0); 
 }

src/point_to_point_rigid_matching.cpp

@@ -10,46 +10,34 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  std::cout << "3.1" <<std::endl;
   int k = X.rows();
   //R = Eigen::Matrix3d::Identity();
   ///t = Eigen::RowVector3d::Zero();
-  std::cout << "3.2" <<std::endl;
   Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * k, 6);
 
   A.block(k,0, k,1) = -X.col(2);
   A.block(2*k,0, k,1) = X.col(1);
- std::cout << "3.2.1" <<std::endl;
   A.block(0,1, k,1) = X.col(2);
   A.block(2*k,1, k,1) = -X.col(0);
- std::cout << "3.2.2" <<std::endl;
   A.block(0,2, k,1) = -X.col(1);
   A.block(k,2, k,1) = X.col(0);
- std::cout << "3.2.3" <<std::endl;
   A.block(0,3, k,1) = Eigen::MatrixXd::Ones(k,1);
   A.block(k,4, k,1) = Eigen::MatrixXd::Ones(k,1);
   A.block(k*2,5, k,1) = Eigen::MatrixXd::Ones(k,1);
- std::cout << "3.2.4" <<std::endl;
   Eigen::MatrixXd B = Eigen::MatrixXd::Zero(3 * k, 1);
   B.block(0,0, k,1) = X.col(0) - P.col(0);
   B.block(k,0, k,1) = X.col(1) - P.col(1);
   B.block(2*k,0, k,1) = X.col(2) - P.col(2);
  //  std::cout << "X - P" << X.col(0) - P.col(0) << std::endl;
  //  std::cout << "X - P" << X.col(1) - P.col(1) << std::endl;
  // std::cout << "X - P" << X.col(2) - P.col(2) << std::endl;
-  std::cout << "dim" << (X.col(0) - P.col(0)).cols() <<std::endl;
- std::cout << "B"<<B <<std::endl;
   Eigen::Matrix<double, 6, 1> u;
-  std::cout << "3.3" <<std::endl;
   u = (A.transpose() * A).inverse() * (- A.transpose() * B);
-  std::cout << "u" << u << std::endl;
   Eigen::Matrix3d M;
   M << 1 , -u(2,0), u(1,0),
        u(2,0), 1, -u(0,0),
        -u(1,0), u(0,0), 1;
-  std::cout << "3.4" <<std::endl;
   closest_rotation(M, R);
   t << u(3,0), u(4,0), u(5,0); 
-  std::cout << "3.5" <<std::endl;
 }