Ziheng Liang HW02

include/icp_single_iteration.h

@@ -37,5 +37,4 @@ void icp_single_iteration(
   const ICPMethod method,
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t);
-
 #endif

include/point_mesh_distance.h

@@ -1,6 +1,8 @@
 #ifndef POINT_MESH_DISTANCE_H
 #define POINT_MESH_DISTANCE_H
 #include <Eigen/Core>
+#include <igl/per_face_normals.h>
+#include "point_triangle_distance.h"
 // Compute the distances `D` between a set of given points `X` and their
 // closest points `P` on a given mesh with vertex positions `VY` and face
 // indices `FY`.

include/point_triangle_distance.h

@@ -19,4 +19,11 @@ void point_triangle_distance(
   const Eigen::RowVector3d & c,
   double & d,
   Eigen::RowVector3d & p);
+
+void dist_helper(const Eigen::RowVector3d & x,
+                 const Eigen::RowVector3d & px,
+                 const Eigen::RowVector3d & a,
+                 const Eigen::RowVector3d & b,
+                 double & d,
+                 Eigen::RowVector3d & p);
 #endif

include/random_points_on_mesh.h

@@ -1,6 +1,11 @@
 #ifndef RANDOM_POINTS_ON_MESH_H
 #define RANDOM_POINTS_ON_MESH_H
 #include <Eigen/Core>
+#include <iostream>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <stdlib.h>
+
 // RANDOM_POINTS_ON_MESH Randomly sample a mesh (V,F) n times.
 //
 // Inputs:

main.cpp

@@ -21,6 +21,7 @@ int main(int argc, char *argv[])
   int num_samples = 100;
   bool show_samples = true;
   ICPMethod method = ICP_METHOD_POINT_TO_POINT;
+  // ICPMethod method = ICP_METHOD_POINT_TO_PLANE;
 
   igl::viewer::Viewer viewer;
   std::cout<<R"(

src/closest_rotation.cpp

@@ -1,9 +1,17 @@
 #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, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::Matrix3d omega;
+  omega << 1, 0, 0,
+  0, 1, 0,
+  0, 0, (svd.matrixU() * svd.matrixV().transpose()).determinant();
+  R = svd.matrixV() * omega * svd.matrixU().transpose();
 }

src/hausdorff_lower_bound.cpp

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

src/icp_single_iteration.cpp

@@ -1,4 +1,9 @@
 #include "icp_single_iteration.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
+#include <iostream>
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -13,4 +18,15 @@ void icp_single_iteration(
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+  Eigen::MatrixXd X;
+  random_points_on_mesh(num_samples, VX, FX, X);
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P;
+  Eigen::MatrixXd N;
+  point_mesh_distance(X, VY, FY, D, P, N);
+  switch(method)
+  {
+    case ICP_METHOD_POINT_TO_POINT: point_to_point_rigid_matching(X, P, R, t);   break;
+    case ICP_METHOD_POINT_TO_PLANE: point_to_plane_rigid_matching(X, P, N, R, t); break;
+  }
 }

src/point_mesh_distance.cpp

@@ -1,5 +1,6 @@
 #include "point_mesh_distance.h"
-
+#include <limits>
+#include <iostream>
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
   const Eigen::MatrixXd & VY,
@@ -9,8 +10,26 @@ void point_mesh_distance(
   Eigen::MatrixXd & N)
 {
   // Replace with your code
-  P.resizeLike(X);
+  D = Eigen::VectorXd::Zero(X.rows());
+  P = Eigen::MatrixXd::Zero(X.rows(), 3);
+  N = Eigen::MatrixXd::Zero(X.rows(), 3);
+
   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();
+  double d;
+  Eigen::RowVector3d p;
+  Eigen::MatrixXd pfn;
+  igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),pfn);
+
+  for(int i = 0;i<X.rows();i++) {
+    D(i) = std::numeric_limits<double>::max();
+    for(int j = 0;j<FY.rows();j++) {
+      point_triangle_distance(X.row(i), VY.row(FY(j,0)), VY.row(FY(j,1)), VY.row(FY(j,2)), d, p);
+      if (D(i) > d) {
+        D(i) = d;
+        P.row(i) = p;
+        N.row(i) = pfn.row(j);
+      }
+    }
+  }
+
 }

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,
@@ -8,6 +11,40 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  Eigen::MatrixXd dig = Eigen::MatrixXd::Zero(X.rows(), X.rows() *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);
+  std::cout << R << std::endl;
+  std::cout << t << std::endl;
 }

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);
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,7 @@
 #include "point_triangle_distance.h"
+#include <limits>
+#include <iostream>
+#include <stdlib.h>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +11,63 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  // following algorithm from below link
+  // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf
+  Eigen::RowVector3d ab = a-b;
+  Eigen::RowVector3d ba = b-a;
+  Eigen::RowVector3d ac = a-c;
+  Eigen::RowVector3d ca = c-a;
+  Eigen::RowVector3d bc = b-c;
+  Eigen::RowVector3d cb = c-b;
+  Eigen::RowVector3d ax = a-x;
+  Eigen::RowVector3d n = ba.cross(cb);
+  double area = n.norm();
+  double angle = ax.dot(n) / n.norm();
+  Eigen::RowVector3d projectx = x - angle * n / n.norm();
+
+  Eigen::RowVector3d pb = projectx - b;
+  Eigen::RowVector3d pc = projectx - c;
+  Eigen::RowVector3d pa = projectx - a;
+
+  double alpha = (pb.cross(pc)).norm() / area; 
+  double beta = (pc.cross(pa)).norm() / area;
+  double gamma = 1 - alpha - beta;
+
+  if(0 <= alpha && alpha <= 1 &&
+  	0 <= beta && beta <= 1 &&
+  	0 <= gamma && gamma <= 1) {
+  	p = projectx;
+  	d = (x - p).norm();
+  	return ;
+  }
+  d = std::numeric_limits<double>::max();
+  dist_helper(x, projectx, a, b, d, p);
+  dist_helper(x, projectx, b, c, d, p);
+  dist_helper(x, projectx, c, a, d, p);
+}
+
+void dist_helper(const Eigen::RowVector3d & x,
+                 const Eigen::RowVector3d & px,
+                 const Eigen::RowVector3d & a,
+                 const Eigen::RowVector3d & b,
+                 double & d,
+                 Eigen::RowVector3d & p) {
+  Eigen::RowVector3d pa = a + ((px-a).dot(b-a)) / ((b-a).dot(b-a)) * (b-a);
+  double distance;
+  if (abs((pa-a).norm() + (a-b).norm() - (pa-b).norm()) < 1e-10) {
+    distance = (x-a).norm();
+    p = a;
+  }
+  else if (abs((pa-b).norm() + (b-a).norm() - (pa-a).norm()) <1e-10) {
+    distance = (x-b).norm();
+    p = b;
+  }
+  else {
+    distance = (x-pa).norm();
+    p = pa;
+  }
+
+  if (d > distance) {
+    d = distance;
+  }
 }

src/random_points_on_mesh.cpp

@@ -1,4 +1,6 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
+#include <random>
 
 void random_points_on_mesh(
   const int n,
@@ -8,6 +10,55 @@ void random_points_on_mesh(
 {
   // REPLACE WITH YOUR CODE:
   X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+  Eigen::MatrixXd area;
+  Eigen::MatrixXd cumsumA;
+  igl::doublearea(V, F, area);
+  igl::cumsum(area, 1, cumsumA);
+
+  double ttl = cumsumA(cumsumA.rows() - 1, 0);
+  for(int i=0;i<n;i++) {
+
+    static std::default_random_engine generator;
+    std::uniform_real_distribution<double> distribution(0.,ttl);
+    double randx = distribution(generator);
+
+  	int idx = cumsumA.rows() / 2;
+  	int gap = cumsumA.rows() / 4;
+  	while(idx >= 1 && idx <= cumsumA.rows() - 1 && gap != 0) {
+      if (cumsumA(idx - 1, 0) < randx && cumsumA(idx, 0) > randx) {
+        break;
+      }
+  		if (randx > cumsumA(idx)) {
+  			idx += gap;
+  		}
+  		else {
+  			idx -= gap;
+  		}
+  		gap = gap / 2;
+      if (gap == 0) {
+        gap = 1;
+      }
+  	}
+    std::uniform_real_distribution<double> distribution_alpha(0.,1.);
+    std::uniform_real_distribution<double> distribution_beta(0.,1.);
+  	double alpha = distribution_alpha(generator);
+  	double beta = distribution_beta(generator);
+
+  	if (alpha + beta > 1) {
+  		alpha = 1 - alpha;
+  		beta = 1 - beta;
+  	}
+
+    if (idx < 0) {
+      idx = 0;
+    }
+    if (idx > cumsumA.rows() - 1) {
+      idx = cumsumA.rows() - 1;
+    }
+
+  	X.row(i) = V.row(F(idx, 0)) + 
+  			   alpha * (V.row(F(idx, 1)) - V.row(F(idx, 0))) +
+  			   beta * (V.row(F(idx, 2)) - V.row(F(idx, 0)));
+  }
 }