Jiayi(Eris) Zhang HW2

README.html

@@ -343,7 +343,7 @@ <h3 id="constantfunctionapproximation">Constant function approximation</h3>
 
 <h3 id="linearfunctionapproximation">Linear function approximation</h3>
 
-<p>If we make make a slightly more appropriate assuming that in the neighborhood
+<p>If we make make a slightly more appropriate assumption that in the neighborhood
 of the <span class="math">\(P_Y(\z₀)\)</span> the surface <span class="math">\(Y\)</span> is a plane, then we can improve this
 approximation while keeping <span class="math">\(\f\)</span> linear in <span class="math">\(\z\)</span>:</p>
 

README.md

@@ -263,7 +263,7 @@ derived our gradients geometrically.
 
 ### Linear function approximation
 
-If we make make a slightly more appropriate assuming that in the neighborhood
+If we make make a slightly more appropriate assumption that in the neighborhood
 of the  $P_Y(\z₀)$ the surface $Y$ is a plane, then we can improve this
 approximation while keeping $\f$ linear in $\z$:
 

src/closest_rotation.cpp

@@ -1,9 +1,22 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/LU>
+
+using namespace Eigen;
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+    JacobiSVD<MatrixXd> svd(M, ComputeFullU | ComputeFullV);
+    MatrixXd U = svd.matrixU();
+    MatrixXd V = svd.matrixV();
+    MatrixXd Vt = V.transpose();
+    double det = (U * Vt).determinant();
+    Eigen::Matrix3d Omega;
+    Omega << 1, 0, 0,
+             0, 1, 0,
+             0, 0, det;
+    R = U * Omega * Vt;
 }
+

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,12 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+  Eigen::MatrixXd points, N, P;
+  Eigen::VectorXd D;
+  random_points_on_mesh(n, VX, FX, points);
+  point_mesh_distance(points, VY, FY, D, P, N);
+  double lowerBound = D.maxCoeff();
+  return lowerBound;
 }
+
+

src/icp_single_iteration.cpp

@@ -1,4 +1,10 @@
 #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"
+
+using namespace Eigen;
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +16,16 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  VectorXd D;
+  MatrixXd points, P, N;
+  random_points_on_mesh(num_samples, VX, FX, points);
+  point_mesh_distance(points, VY, FY, D, P, N);
+  if (method == ICP_METHOD_POINT_TO_POINT) {
+    point_to_point_rigid_matching(points, P, R, t);
+  }
+  else{
+    point_to_plane_rigid_matching(points, P, N, R, t);
+  }
 }
+
+

src/point_mesh_distance.cpp

@@ -1,4 +1,9 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
+
+using namespace Eigen;
+using namespace std;
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +13,34 @@ void point_mesh_distance(
   Eigen::MatrixXd & P,
   Eigen::MatrixXd & N)
 {
-  // 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());
+
+  MatrixXd faceNormals;
+  igl::per_face_normals(VY, FY, Vector3d(1,1,1).normalized(), faceNormals);
+
+  double min_distance, temp_distance;
+  RowVector3d closest_point, temp_point;
+  int closest_index = 0;
+  Vector3d point;
+
+  for (int i = 0; i < X.rows(); i++) {
+    min_distance = -1;
+    point = X.row(i);
+
+    for (int j = 0; j < FY.rows(); j++) {
+      point_triangle_distance(point, VY.row(FY(j, 0)), VY.row(FY(j, 1)), VY.row(FY(j, 2)), temp_distance, temp_point);
+      if (temp_distance < min_distance || min_distance < 0) {
+        min_distance = temp_distance;
+        closest_index = j;
+        closest_point = temp_point;
+      }
+    }
+
+    D(i) = min_distance;
+    P.row(i) = closest_point;
+    N.row(i) = faceNormals.row(closest_index);
+  }
+}

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,8 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
+
+using namespace Eigen;
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +11,41 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
-}
+  // construct diag matrix
+  MatrixXd diag_matrix(X.rows(), 3 * X.rows());
+  diag_matrix << MatrixXd(N.col(0).asDiagonal()),
+                 MatrixXd(N.col(1).asDiagonal()),
+                 MatrixXd(N.col(2).asDiagonal());
+
+  // construct b = X - P
+  VectorXd b(3 * N.rows());
+  b << X.col(0) - P.col(0),
+       X.col(1) - P.col(1),
+       X.col(2) - P.col(2);
+  b = diag_matrix * b;
+
+  // construct A
+  VectorXd zeros = VectorXd::Zero(N.rows());
+  VectorXd ones = VectorXd::Ones(N.rows());
+  MatrixXd A(3 * N.rows(), 6);
+  A << zeros, X.col(2), -X.col(1), ones, zeros, zeros,
+       -X.col(2), zeros, X.col(0), zeros, ones, zeros,
+       X.col(1), -X.col(0), zeros, zeros, zeros, ones;
+  A = diag_matrix * A;
+
+  // compute u
+  VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * b);
+
+  double alpha = u(0);
+	double beta = u(1);
+	double gamma = u(2);
+
+  Matrix3d M;
+	M << 1, gamma, -beta,
+		   -gamma, 1, alpha,
+		   beta, -alpha, 1;
+
+  closest_rotation(M, R);
+  t = RowVector3d(u(3),u(4),u(5));
+
+}

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,45 @@
 #include "point_to_point_rigid_matching.h"
+#include "closest_rotation.h"
 #include <igl/polar_svd.h>
+#include <iostream>
+
+using namespace 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();
+  // construct b = X - P
+  VectorXd b(3 * X.rows());
+  b << X.col(0) - P.col(0),
+       X.col(1) - P.col(1),
+       X.col(2) - P.col(2);
+
+  // construct A
+  VectorXd zeros = VectorXd::Zero(X.rows());
+  VectorXd ones = VectorXd::Ones(X.rows());
+  MatrixXd A(3 * X.rows(), 6);
+  A << zeros, X.col(2), -X.col(1), ones, zeros, zeros,
+       -X.col(2), zeros, X.col(0), zeros, ones, zeros,
+       X.col(1), -X.col(0), zeros, zeros, zeros, ones;
+
+  // compute u
+  VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * b);
+
+  double alpha = u(0);
+	double beta = u(1);
+	double gamma = u(2);
+
+  Matrix3d M;
+	M << 1, gamma, -beta,
+		   -gamma, 1, alpha,
+		   beta, -alpha, 1;
+
+  closest_rotation(M, R);
+  t = RowVector3d(u(3),u(4),u(5));
+
 }
 
+

src/point_triangle_distance.cpp

@@ -1,4 +1,9 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
+#include <algorithm>
+
+using namespace Eigen;
+using namespace std;
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +13,42 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
-}
+  RowVector3d normal = (b - a).cross(c - b);
+  normal.normalize();
+
+  double area_ABC = normal.dot((b - a).cross(c - b));
+  double area_PBC = normal.dot((b - p).cross(c - p));
+  double area_PCA = normal.dot((c - p).cross(a - p));
+
+  double alpha = area_PBC * 1.0 / area_ABC;
+  double beta = area_PCA * 1.0 / area_ABC;
+  double gamma = 1.0 - alpha - beta;
+
+  RowVector3d p0 = x - x.dot(normal) * normal;
+
+  // reference: https://stackoverflow.com/questions/14467296/barycentric-coordinate-clamping-on-3d-triangle 
+  Eigen::RowVector3d pc;
+
+  if (gamma < 0) {
+      double t = (p0 - b).dot(a - b) / (a - b).dot(a - b);
+      t = max(0.0, min(t, 1.0));
+      pc = RowVector3d(t, 1.0 - t, 0.0);
+  }
+  else if (beta < 0) {
+      double t = (p0 - c).dot(c - a) / (c - a).dot(c - a);
+      t = max(0.0, min(t, 1.0));
+      pc = RowVector3d(1.0 - t, 0.0, t);
+  }
+  else if (alpha < 0) {
+      double t = (p0 - c).dot(b - c) / (b - c).dot(b - c);
+      t = max(0.0, min(t, 1.0));
+      pc = RowVector3d(0.0, t, 1.0 - t);
+  }
+  else {
+      pc = RowVector3d(alpha, beta, gamma);
+  }
+
+  p = pc[0] * a + pc[1] * b + pc[2] * c;
+
+  d = (x - p).norm();
+}

src/random_points_on_mesh.cpp

@@ -1,13 +1,70 @@
 #include "random_points_on_mesh.h"
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <random>
+#include <iostream>
+
+using namespace Eigen;
+using namespace std;
 
 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());
+  VectorXd doubleArea, cumArea;
+  igl::doublearea(V, F, doubleArea);
+  igl::cumsum(doubleArea / 2.0, 1, cumArea);
+  double totalArea = cumArea(F.rows() - 1);
+  cumArea = cumArea * 1.0 / totalArea;
+
+  double alpha, beta;
+  default_random_engine generator;
+  uniform_real_distribution<double> distribution(0.0, 1.0);
+
+  int first, last, mid;
+  int tri_index;
+
+  RowVector3d v1, v2, v3;
+
+  for (int i = 0; i < X.rows(); i++) {
+    // randomly pick a triangle
+    double tri_sample = distribution(generator);
+    first = 0;
+    last = F.rows() - 1;
+    // binary search
+    while (last - first > 1) {
+      mid = (first + last) / 2;
+      if (tri_sample <= cumArea(mid)) {
+        last = mid;
+      }
+      else {
+        first = mid;
+      }
+    }
+    if (tri_sample <= cumArea(0)) {
+      tri_index = 0;
+    }
+    else {
+      tri_index = last;
+    }
+
+    alpha = distribution(generator);
+    beta = distribution(generator);
+
+    if (alpha + beta > 1) {
+      alpha = 1 - alpha;
+      beta = 1 - beta;
+    }
+
+    v1 = V.row(F(tri_index, 0));
+    v2 = V.row(F(tri_index, 1));
+    v3 = V.row(F(tri_index, 2));
+
+    X.row(i) = v1 + alpha * (v2 - v1) + beta * (v3 - v1);
+  }
+
 }