added p2po p2pl

src/closest_rotation.cpp

@@ -1,12 +1,14 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/Dense>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
-  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, ComputeThinU | ComputeThinV);
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
   Eigen::Matrix3d omega;
   omega << 1, 0, 0,
   0, 1, 0,

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,6 +10,38 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  Eigen::Matrix3d dig = Eigen::Matrix3d::Zero(X.rows() * 3, 3);
+  dig.block(0,0,X.rows(),X.rows()) = N.col(0).asDiagonal();
+  dig.block(0,X.rows(),X.rows(),X.rows()) = N.col(1).asDiagonal();
+  dig.block(0,X.rows()*2,X.rows(),X.rows()) = N.col(2).asDiagonal();
+
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6);
+  for (int i = 0; i<X.rows();i ++){
+  	A(i, 1) = X(i, 2);
+  	A(i, 2) = -X(i, 1);
+  	A(i, 3) = 1;
+
+  	A(i+X.rows(), 0) = -X(i, 2);
+  	A(i+X.rows(), 2) = X(i, 0);
+  	A(i+X.rows(), 4) = 1;
+
+  	A(i+X.rows()*2, 0) = X(i, 1);
+  	A(i+X.rows()*2, 1) = -X(i, 0);
+  	A(i+X.rows()*2, 5) = 1;
+
+  }
+
+  Eigen::VectorXd B = Eigen::VectorXd::Zero(X.rows() * 3);
+  Eigen::MatrixXd XP = X-P;
+  B << XP.col(0), XP.col(1), XP.col(2);
+  Eigen::MatrixXd new_A = dig * A;
+  Eigen::MatrixXd new_B = dig * B;
+
+  Eigen::VectorXd u = (new_A.transpose() * new_A).inverse() * (-new_A.transpose() * new_B);
+  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(3, 3);
+  M << 1, -u(2), u(1),
+  	   u(2), 1, -u(0),
+  	   -u(1), u(0), 1;
+  closest_rotation(M, R);
+  t << u(3), u(4), u(5);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,5 +1,7 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -10,5 +12,30 @@ void point_to_point_rigid_matching(
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6);
+  Eigen::VectorXd B = Eigen::VectorXd::Zero(X.rows() * 3);
+  Eigen::MatrixXd XP = X-P;
+  for (int i = 0; i<X.rows();i ++){
+  	A(i, 1) = X(i, 2);
+  	A(i, 2) = -X(i, 1);
+  	A(i, 3) = 1;
+
+  	A(i+X.rows(), 0) = -X(i, 2);
+  	A(i+X.rows(), 2) = X(i, 0);
+  	A(i+X.rows(), 4) = 1;
+
+  	A(i+X.rows()*2, 0) = X(i, 1);
+  	A(i+X.rows()*2, 1) = -X(i, 0);
+  	A(i+X.rows()*2, 5) = 1;
+
+  }
+  B << XP.col(0), XP.col(1), XP.col(2);
+  Eigen::VectorXd u = (A.transpose() * A).inverse() * (- A.transpose() * B);
+  Eigen::MatrixXd M = Eigen::MatrixXd::Zero(3, 3);
+  M << 1, -u(2), u(1),
+  	   u(2), 1, -u(0),
+  	   -u(1), u(0), 1;
+  closest_rotation(M, R);
+  t << u(3), u(4), u(5);
 }