Sajjad Nezhadi HW2

src/closest_rotation.cpp

@@ -1,9 +1,18 @@
 #include "closest_rotation.h"
-
+#include <iostream>
+#include <Eigen/SVD>
+#include <Eigen/Dense>
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+std::cout << "ss2" << std::endl;
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+std::cout << "pp2" << std::endl;
+  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(3, 3);
+std::cout << "tll" << std::endl;
+  I(2,2) = (svd.matrixU()*svd.matrixV().transpose()).determinant();
+
+  R = (svd.matrixU() * I  * svd.matrixV().transpose()).transpose();
+
 }

src/hausdorff_lower_bound.cpp

@@ -1,4 +1,6 @@
 #include "hausdorff_lower_bound.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 double hausdorff_lower_bound(
   const Eigen::MatrixXd & VX,
@@ -7,6 +9,14 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+
+  Eigen::MatrixXd X(n, 3);
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P;
+  Eigen::MatrixXd N;
+
+  random_points_on_mesh(n, VX, FX, X);
+  point_mesh_distance(X, VY, FY, D, P, N);
+
+  return D.maxCoeff();
 }

src/icp_single_iteration.cpp

@@ -1,4 +1,9 @@
 #include "icp_single_iteration.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_mesh_distance.h"
+#include "point_to_point_rigid_matching.h"
+#include "random_points_on_mesh.h"
+#include <iostream> 
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +15,25 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  Eigen::MatrixXd X;
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P; 
+  Eigen::MatrixXd N;
+
+
+  std::cout << "1" << std::endl;
+
+  random_points_on_mesh(num_samples, VX, FX, X);
+  std::cout << "2" << std::endl;
+  point_mesh_distance(X, VY, FY, D, P, N);
+  std::cout << "3" << std::endl;
+  if (method == ICP_METHOD_POINT_TO_POINT){
+    std::cout << "4" << std::endl;
+    point_to_point_rigid_matching(X, P, R, t);
+  } 
+  else{
+    std::cout << "5" << std::endl;
+    point_to_plane_rigid_matching(X, P, N, R, t);
+  }
+  std::cout << "6" << std::endl;
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,7 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
+#include <iostream> 
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -10,7 +13,32 @@ void point_mesh_distance(
 {
   // Replace with your code
   P.resizeLike(X);
-  N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
-  for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
-  D = (X-P).rowwise().norm();
+  N.resizeLike(X);
+  D.resize(X.rows());
+
+  Eigen::MatrixXd normals;
+  igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),normals);
+
+  for(int i = 0;i<X.rows();i++) {
+    Eigen::RowVector3d proj_y;
+    double distance = std::numeric_limits<double>::max();;
+    int index_tr = 0;
+
+    for(int j = 0;j<FY.rows();j++){
+      Eigen::RowVector3d temp_proj_y;
+      double temp_distance = 1.79769e+308;
+
+      point_triangle_distance(X.row(i), VY.row(FY(j,0)), VY.row(FY(j,1)), VY.row(FY(j,2)), temp_distance, temp_proj_y);
+
+      if (temp_distance < distance){
+        distance = temp_distance;
+        proj_y = temp_proj_y;
+        index_tr = j;
+      }
+    }
+
+    P.row(i) = proj_y;
+    D(i) = distance;
+    N.row(i) = normals.row(index_tr);
+  }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
+#include <iostream> 
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +10,37 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+      std::cout << "a" << std::endl;
+  Eigen::MatrixXd A(3*X.rows(), 6);
+  Eigen::MatrixXd L(X.rows(), 3*X.rows());
+  Eigen::VectorXd C(3*X.rows());
+  Eigen::MatrixXd M(3,3);
+  Eigen::VectorXd U;
+  Eigen::MatrixXd N0 = N.col(0).asDiagonal();
+  Eigen::MatrixXd N1 = N.col(1).asDiagonal();
+  Eigen::MatrixXd N2 = N.col(2).asDiagonal();
+  Eigen::VectorXd T0 = Eigen::VectorXd::Zero(X.rows());
+  Eigen::VectorXd T1 = Eigen::VectorXd::Ones(X.rows());
+
+      std::cout << "b" << std::endl;
+  L << N0, N1, N2;
+        std::cout << "bb" << std::endl;
+  C << X.col(0) - P.col(0),
+     X.col(1) - P.col(1),
+     X.col(2) - P.col(2);
+           std::cout << "bc" << std::endl;
+  A << T0, X.col(2), -X.col(1), T1, T0, T0,
+     -X.col(2), T0, X.col(0), T0, T1, T0,
+     X.col(1), -X.col(0), T0, T0, T0, T1;
+    std::cout << "c" << std::endl;
+  U = (A.transpose() * L.transpose() * L * A).inverse() * (-A.transpose() * L.transpose() * L * C);
+    std::cout << "d" << std::endl;
+  M << 0, -U(2), U(1),
+     U(2), 0, -U(0),
+     -U(1), U(0), 0;
+    std::cout << "e" << std::endl;
+  closest_rotation(M, R);
+      std::cout << "f" << std::endl;
+  t << U(3), U(4), U(5);
+
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,37 @@
 #include "point_to_point_rigid_matching.h"
+#include "closest_rotation.h"
 #include <igl/polar_svd.h>
+#include <iostream> 
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
   const Eigen::MatrixXd & P,
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	Eigen::Matrix3d M;
+	Eigen::RowVector3d P_hat;
+	Eigen::RowVector3d X_hat;
+	Eigen::MatrixXd P_norm;
+	Eigen::MatrixXd X_norm;
+
+    std::cout << "t1" << std::endl;
+ 	X_hat = X.colwise().sum();
+ 	P_hat = P.colwise().sum();
+ 	X_hat /= X.rows();
+ 	P_hat /= P.rows();
+ 	X_norm = X.rowwise() - X_hat;
+ 	P_norm = P.rowwise() - P_hat;
+ 	std::cout << P_norm.rows() << std::endl;
+ 	std::cout << "t2" << std::endl;
+    std::cout << X_norm.rows() << std::endl;
+ 	M = P_norm.transpose() * X_norm;
+ 	std::cout << M << std::endl;
+ 	closest_rotation(M, R);
+ 	    std::cout << "t4" << std::endl;
+    t = P_hat.transpose() - (R * X_hat.transpose());
+        std::cout << "t5" << std::endl;
+
+
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_triangle_distance.h"
+#include <iostream> 
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -9,6 +10,50 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+	Eigen::RowVector3d AB = b - a;
+	Eigen::RowVector3d AC = c - a;
+	Eigen::RowVector3d AX = x - a;
+	Eigen::RowVector3d BX = x - b;
+	Eigen::RowVector3d BC = c - b;
+	Eigen::RowVector3d CX = x - c;
+
+	double s = AX.dot(AB) / AB.dot(AB);
+	double t = AX.dot(AC) / AC.dot(AC);
+
+	if (t > 0 and s > 0 and t+s < 1){
+		p = a + (s*AB) + (t*AC);
+	}
+	else {
+		Eigen::RowVector3d v4 = BC * (BX.dot(BC)/(BC.norm())) + b;
+		Eigen::RowVector3d v5 = AB * (AX.dot(AB)/(AB.norm())) + a;
+		Eigen::RowVector3d v6 = AC * (AX.dot(AC)/(AC.norm())) + a;
+
+		double dis[] = {AX.norm(), BX.norm(), CX.norm(), (v4-x).norm(), (v5-x).norm(), (v6-x).norm()};
+
+		const int N = sizeof(dis) / sizeof(double);
+
+		int min_index = std::distance(dis, std::min_element(dis, dis + N));
+
+		if (min_index == 0){
+			p = a;
+		}
+		else if (min_index == 1){
+			p = b;
+		}
+		else if (min_index == 2){
+			p = c;
+		}
+		else if (min_index == 3){
+			p = v4;
+		}
+		else if (min_index == 4){
+			p = v5;
+		}
+		else {
+			p = v6;
+		}
+	}
+
+  d = (p-x).norm();
+
 }

src/random_points_on_mesh.cpp

@@ -1,4 +1,24 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+
+int bs(const Eigen::VectorXd & SumArea, double x)
+{
+  int A = 0, B = SumArea.rows() - 1;
+  while (A <= B)
+  {
+    int C = (A+B)/2;
+    if (SumArea(C) > x){
+      B = C - 1;
+    }
+    else{
+      A = C + 1;
+    }
+  }
+  return B + 1;
+}
+
 
 void random_points_on_mesh(
   const int n,
@@ -7,7 +27,38 @@ void random_points_on_mesh(
   Eigen::MatrixXd & X)
 {
   // REPLACE WITH YOUR CODE:
-  X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+    X.resize(n,3);
+
+    Eigen::MatrixXd rand = Eigen::VectorXd::Random(n);
+    rand = (rand + Eigen::VectorXd::Constant(n,1)) * .5;
+
+
+    Eigen::VectorXd Areas(F.rows()); 
+    Eigen::VectorXd SumArea(F.rows()); 
+
+    igl::doublearea(V,F,Areas);
+    igl::cumsum(Areas, 1, SumArea);
+    SumArea /= SumArea.maxCoeff();
+
+
+
+    for(int i = 0;i<n;i++) {
+        int index = bs(SumArea, rand(i));
+
+        if (index > (SumArea.rows() - 1)){
+          index = SumArea.rows() - 1;
+        }
+
+        Eigen::MatrixXd r = Eigen::VectorXd::Random(2);
+        r = (r + Eigen::VectorXd::Constant(2,1)) * .5;
+
+        if (r(0) + r(1) > 1){
+          r(0) = 1-r(0);
+          r(1) = 1-r(1);
+        }
+        X.row(i) = V.row(F(index, 0)) + (r(0)*(V.row(F(index, 1)) - V.row(F(index, 0)))) + (r(1)*(V.row(F(index, 2)) - V.row(F(index, 0))));
+    }
+
+
 }