first attempt

src/closest_rotation.cpp

@@ -1,9 +1,26 @@
 #include "closest_rotation.h"
+#include <Eigen/Eigen>
+#include <iostream>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  // SVD inspired by Eigen::JacobiSVD library
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, 
+          Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Eigen::Matrix3d U = svd.matrixU();
+  Eigen::Matrix3d V = svd.matrixV();
+
+  double det = (U * V.transpose()).determinant();
+
+  Eigen::Matrix3d mid;
+  mid << 1, 0, 0,
+         0, 1, 0,
+         0, 0, det;
+
+  // As ISSUE #38 poited out, should times Vx by R.T
+  // Fix here since I cannot change main.cpp
+  R = (U * mid * V.transpose()).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,
@@ -7,6 +9,11 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+  Eigen::MatrixXd X, P, N;
+  Eigen::VectorXd D;
+
+  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,8 @@
 #include "icp_single_iteration.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +14,16 @@ 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, P, N;
+  random_points_on_mesh(num_samples, VX, FX, X);
+  Eigen::VectorXd D;
+  point_mesh_distance(X, VY, FY, D, P, N);
+  if (method == ICP_METHOD_POINT_TO_PLANE) {
+    // To Plane
+    point_to_plane_rigid_matching(X, P, N, R, t);
+    return;
+  }
+
+  // To Point
+  point_to_point_rigid_matching(X, P, R, t);
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,6 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +10,40 @@ void point_mesh_distance(
   Eigen::MatrixXd & P,
   Eigen::MatrixXd & N)
 {
-  // Replace with your code
+  D.resize(X.rows());
   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);
+
+  // Since assume all points in P is inside the face, could directly
+  // compute all normals:
+  Eigen::MatrixXd All_normal;
+  igl::per_face_normals(VY, FY, 
+          Eigen::Vector3d(1,1,1).normalized(), All_normal);
+
+  // Compute closest points
+  for (int i = 0; i < X.rows(); i ++) {
+    Eigen::RowVector3d x = X.row(i);
+    double min = std::numeric_limits<double>::max();
+    Eigen::RowVector3d projection;
+    Eigen::RowVector3d normal;
+
+    // Loop over faces to find the closest point
+    for(int j = 0; j < FY.rows(); j ++) {
+      Eigen::RowVector3d a = VY.row(FY(j, 0));
+      Eigen::RowVector3d b = VY.row(FY(j, 1));
+      Eigen::RowVector3d c = VY.row(FY(j, 2));
+      double cur_min;
+      Eigen::RowVector3d cur_p;
+      point_triangle_distance(x, a, b, c, cur_min, cur_p);
+      if (min > cur_min) {
+        min = cur_min;
+        projection = cur_p;
+        normal = All_normal.row(j);
+      }
+    }
+    // Able to add to the result
+    D(i) = min;
+    P.row(i) = projection;
+    N.row(i) = normal;
+  }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Eigen>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +9,48 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    // Fetch some useful variables
+  int rows = X.rows();
+  Eigen::VectorXd X1 = X.col(0);
+  Eigen::VectorXd X2 = X.col(1);
+  Eigen::VectorXd X3 = X.col(2);
+  Eigen::VectorXd diff1 = X1 - P.col(0);
+  Eigen::VectorXd diff2 = X2 - P.col(1);
+  Eigen::VectorXd diff3 = X3 - P.col(2);
+  Eigen::MatrixXd N1 = N.col(0).asDiagonal();
+  Eigen::MatrixXd N2 = N.col(1).asDiagonal();
+  Eigen::MatrixXd N3 = N.col(2).asDiagonal();
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(rows);
+  Eigen::VectorXd one = Eigen::VectorXd::Ones(rows);
+
+  // Construct Matrix A and right hand side value:
+  Eigen::MatrixXd left_N(rows, 3*rows);
+  left_N << N1, N2, N3;
+
+  Eigen::MatrixXd A(3*rows, 6);
+  A << zero, X3, -X2, one, zero, zero,
+       -X3, zero, X1, zero, one, zero,
+       X2, -X1, zero, zero, zero, one;
+  Eigen::MatrixXd new_A = left_N * A;
+
+  Eigen::VectorXd rhs(3*rows);
+  rhs << diff1,
+         diff2,
+         diff3;
+  Eigen::VectorXd new_rhs = left_N * rhs;
+
+  // Compute u:
+  Eigen::MatrixXd AAT = new_A.transpose()*new_A;
+  new_rhs = -1 * (new_A.transpose()*new_rhs);
+  Eigen::VectorXd u = AAT.colPivHouseholderQr().solve(new_rhs);
+
+  // Construct M:
+  Eigen::Matrix3d M;
+  M << 1, -u(2), u(1),
+       u(2), 1, -u(0),
+       -u(1), u(0), 1;
+
+  // Calculate R and t:
+  closest_rotation(M, R);
+  t << u(3), u(4), u(5);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,49 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/Eigen>
 
 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();
+  // Fetch some useful variables
+  int rows = X.rows();
+  Eigen::VectorXd X1 = X.col(0);
+  Eigen::VectorXd X2 = X.col(1);
+  Eigen::VectorXd X3 = X.col(2);
+  Eigen::VectorXd diff1 = X1 - P.col(0);
+  Eigen::VectorXd diff2 = X2 - P.col(1);
+  Eigen::VectorXd diff3 = X3 - P.col(2);
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(rows);
+  Eigen::VectorXd one = Eigen::VectorXd::Ones(rows);
+
+  // Construct Matrix A and right hand side value:
+  Eigen::MatrixXd A(3*rows, 6);
+  A << zero, X3, -X2, one, zero, zero,
+       -X3, zero, X1, zero, one, zero,
+       X2, -X1, zero, zero, zero, one;
+
+  Eigen::VectorXd rhs(3*rows);
+  rhs << diff1,
+         diff2,
+         diff3;
+
+  // Compute u:
+  Eigen::MatrixXd AAT = A.transpose()*A;
+  rhs = -1 * (A.transpose()*rhs);
+  Eigen::VectorXd u = AAT.colPivHouseholderQr().solve(rhs);
+
+  // Construct M:
+  Eigen::Matrix3d M;
+  M << 1, -u(2), u(1),
+       u(2), 1, -u(0),
+       -u(1), u(0), 1;
+
+  // Calculate R and t:
+  closest_rotation(M, R);
+  t << u(3), u(4), u(5);
 }
 

src/point_triangle_distance.cpp

@@ -8,7 +8,37 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  Eigen::RowVector3d edge_ba = b - a;
+  Eigen::RowVector3d edge_ca = c - a;
+  Eigen::RowVector3d edge_xa = x - a;
+
+  // Find the normal vector and projection
+  Eigen::RowVector3d norm_vec = (edge_ba.cross(edge_ca)).normalized();
+  Eigen::RowVector3d px = x - (norm_vec.dot(edge_xa))*norm_vec;
+
+  // Use Barycentric algorithm to compute distance:
+  // Idea inspired by https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
+  Eigen::RowVector3d edge_pxa = px - a;
+  double edge_ba_norm = edge_ba.dot(edge_ba);
+  double edge_ca_norm = edge_ca.dot(edge_ca);
+  double edge_ba_ca = edge_ba.dot(edge_ca);
+  double edge_ba_pxa = edge_ba.dot(edge_pxa);
+  double edge_ca_pxa = edge_ca.dot(edge_pxa);
+
+  double denominator = edge_ba_norm*edge_ca_norm - edge_ba_ca*edge_ba_ca;
+
+  double c2 = (edge_ca_norm*edge_ba_pxa - edge_ba_ca*edge_ca_pxa) / denominator;
+  double c3 = (edge_ba_norm*edge_ca_pxa - edge_ba_ca*edge_ba_pxa) / denominator;
+  double c1 = 1 - c2 - c3;
+
+  // Handle if not inside the triangle by clamping these coordinates:
+  c1 = c1 < 0 ? 0 : c1;
+  c1 = c1 > 1 ? 1 : c1;
+  c2 = c2 < 0 ? 0 : c2;
+  c2 = c2 > 1 ? 1 : c2;
+  c3 = 1 - c1 - c2;
+
+  // Find value:
+  p = c1 * a + c2 * b + c3 * c;
+  d = (p - x).norm();
 }

src/random_points_on_mesh.cpp

@@ -1,13 +1,73 @@
 #include "random_points_on_mesh.h"
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <iostream>
+#include <random>
+
+int find_index(Eigen::MatrixXd &C, double r) {
+  // Handle edge case:
+  if (r < C(0)) {
+    return 0;
+  }
+
+  // Binary Search:
+  int begin = 0;
+  int end = C.rows() - 1;
+  int mid = int((end + begin) / 2);
+
+  while(begin + 1 < end) {
+    double cur_mid = C(mid);
+    if (cur_mid == r) {
+      return mid + 1;
+    }
+    else if (cur_mid > r){
+      if (C(mid-1)<r)
+        return mid;
+      end = mid;
+    }
+    else {
+      begin = mid;
+    }
+    mid = int((end + begin) / 2);
+  }
+  return end;
+}
 
 void random_points_on_mesh(
   const int n,
   const Eigen::MatrixXd & V,
   const Eigen::MatrixXi & F,
   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());
+
+  // Compute Area:
+  Eigen::MatrixXd all_area;
+  Eigen::MatrixXd cum_area;
+  igl::doublearea(V, F, all_area);
+  igl::cumsum(all_area, 1, cum_area);
+  cum_area.array().rowwise() /= cum_area.row(cum_area.rows()-1).array();
+
+  // Uniform Random inspired by C++ library
+  std::default_random_engine random_generator;
+  std::uniform_real_distribution<double> uniform_distribution(0.0, 1.0);
+
+  for(int i = 0; i < X.rows(); i ++) {
+    // Find x:
+    double threshold = uniform_distribution(random_generator);
+    int ind = find_index(cum_area, threshold);
+    Eigen::RowVectorXd v0 = V.row(F(ind, 0));
+    Eigen::RowVectorXd v1 = V.row(F(ind, 1));
+    Eigen::RowVectorXd v2 = V.row(F(ind, 2));
+
+    double alpha = uniform_distribution(random_generator);
+    double beta = uniform_distribution(random_generator);
+    if (alpha + beta > 1) {
+      alpha = 1 - alpha;
+      beta = 1 - beta;
+    }
+
+    X.row(i) = v0 + alpha*(v1 - v0) + beta*(v2 - v0);
+  }
 }