Simeon Krastnikov's HW2 Submission

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,17 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/LU>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix3d U = svd.matrixU();
+  Eigen::Matrix3d V = svd.matrixV();
+  Eigen::Matrix3d Sigma(3,3);
+  Sigma << 1, 0, 0,
+           0, 1, 0,
+           0, 0, (U * V.transpose()).determinant();
+  R = U * Sigma * V.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,13 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+  Eigen::MatrixXd X(n, 3);
+  random_points_on_mesh(n, VX, FX, X);
+
+  Eigen::VectorXd D(n);
+  Eigen::MatrixXd P(n, 3);
+  Eigen::MatrixXd N(n, 3);
+  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_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +14,15 @@ 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;
+  random_points_on_mesh(num_samples, VX, FX, X);
+
+  Eigen::MatrixXd P, N;
+  Eigen::VectorXd D;
+  point_mesh_distance(X, VY, FY, D, P, N);
+
+  if (method == ICP_METHOD_POINT_TO_POINT)
+    point_to_point_rigid_matching(X, P, R, t);
+  else
+    point_to_plane_rigid_matching(X, P, N, 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,33 @@ 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();
+  int n = X.rows();
+  int m = FY.rows();
+  D.resize(n);
+  P.resize(n, 3);
+  N.resize(n, 3);
+
+  for (int i = 0; i < n; i++) {
+    Eigen::RowVector3d x = X.row(i);
+    double closest_d = INT_MAX;
+    Eigen::RowVector3d closest_p;
+    Eigen::RowVector3d closest_p_n;
+    for (int j = 0; j < m; j++) {
+      double d;
+      Eigen::RowVector3d p;
+      Eigen::Vector3d a = VY.row(FY(j, 0));
+      Eigen::Vector3d b = VY.row(FY(j, 1));
+      Eigen::Vector3d c = VY.row(FY(j, 2));
+      point_triangle_distance(x, a, b, c, d, p);
+      if (d < closest_d) {
+        closest_d = d;
+        closest_p = p;
+        closest_p_n = (c-a).cross(c-b);
+        closest_p_n /= closest_p_n.norm();
+      }
+    }
+    D(i) = closest_d;
+    P.row(i) = closest_p;
+    N.row(i) = closest_p_n;
+  }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/LU>
+using namespace Eigen;
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +10,31 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  int k = X.rows();
+  MatrixXd A(3*k, 6);
+  VectorXd a(3*k);
+  MatrixXd D(k, 3*k);
+  VectorXd zeros = VectorXd::Zero(k);
+  VectorXd ones = VectorXd::Ones(k);
+  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 << X.col(0) - P.col(0), 
+       X.col(1) - P.col(1), 
+       X.col(2) - P.col(2);
+  D << MatrixXd(N.col(0).asDiagonal()), 
+       MatrixXd(N.col(1).asDiagonal()), 
+       MatrixXd(N.col(2).asDiagonal());
+
+  A = D * A;
+  a = D * a;
+  VectorXd u = (A.transpose() * A).lu().solve(-A.transpose() * a);
+
+  Matrix3d M;
+  M << 1, u(2), -u(1),
+       -u(2), 1, u(0),
+       u(1), -u(0), 1;  
+  closest_rotation(M, R);
+
+  t = u.tail(3);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,44 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/LU>
+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();
+  /*
+  // this is the closed-form version of the point-to-point minimizer
+  Eigen::RowVector3d x_h = X.colwise().mean();
+  Eigen::RowVector3d p_h = P.colwise().mean();
+  Eigen::Matrix3d M = (P.rowwise() - p_h).transpose() * (X.rowwise() - x_h);
+  closest_rotation(M, R);
+  t = (p_h.transpose() - R * x_h.transpose()).transpose(); */
+
+  int k = X.rows();
+  MatrixXd A(3*k, 6);
+  VectorXd a(3*k);
+  VectorXd zeros = VectorXd::Zero(k);
+  VectorXd ones = VectorXd::Ones(k);
+  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 << X.col(0) - P.col(0), 
+       X.col(1) - P.col(1), 
+       X.col(2) - P.col(2);
+
+  VectorXd u = (A.transpose() * A).lu().solve(-A.transpose() * a);
+
+  Matrix3d M;
+
+  // I am not sure why I needed to flip the signs here
+  M << 1, u(2), -u(1),
+       -u(2), 1, u(0),
+       u(1), -u(0), 1;  
+  closest_rotation(M, R);
+
+  t = u.tail(3);
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +9,38 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  // obtain the normal to abc
+  Eigen::RowVector3d n = (c-a).cross(c-b);
+  n /= n.norm();
+
+  // obtain a candidate p0 by projecting x onto triangle normal
+  Eigen::RowVector3d p0 = x - (x-a).dot(n) * n;
+
+  bool left_of_ab = (b-a).cross(p0-a).dot(n) >= 0;
+  bool left_of_bc = (c-b).cross(p0-b).dot(n) >= 0;
+  bool left_of_ca = (a-c).cross(p0-c).dot(n) >= 0;
+  if (left_of_ab and left_of_bc and left_of_ca) {
+    p = p0;  // p is inside the triangle
+  }
+  else {
+    // obtain three more candidates by projecting p onto triangle edges
+    Eigen::RowVector3d p_ab = a + (p0-a).dot(b-a) * (b-a);
+    Eigen::RowVector3d p_bc = b + (p0-b).dot(c-b) * (c-b);
+    Eigen::RowVector3d p_ca = c + (p0-c).dot(a-c) * (a-c);
+    // set p according to region p0 is in
+    if (left_of_ca && left_of_bc)
+      p = p_ab;
+    else if (left_of_ab && left_of_ca)
+      p = p_bc;
+    else if (left_of_bc && left_of_ab)
+      p = p_ca;
+    else if (!left_of_ca && !left_of_ab)
+      p = a;
+    else if (!left_of_ab && !left_of_bc)
+      p = b;
+    else if (!left_of_bc && !left_of_ca)
+      p = c;
+  }
+
+  d = (x - p).norm();
 }

src/random_points_on_mesh.cpp

@@ -1,13 +1,56 @@
 #include "random_points_on_mesh.h"
+#include <algorithm>
+#include <stdlib.h>
+#include <igl/doublearea.h>
+#include <iostream>
 
 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());
+
+  int m = F.rows();
+
+  Eigen::VectorXd A;
+  igl::doublearea(V, F, A);
+  A /= 2;
+  A /= A.sum();
+
+  // initialize the cumulative area array C
+  double total_area;
+  struct tr_area { int Fi; double area; };
+  tr_area C[m];
+  for (int i = 0; i < m; i++) {
+    C[i] = tr_area{.Fi = i, .area = A[i] + (i > 0 ? C[i-1].area : 0)};
+  }
+  std::sort(C, C + m, 
+    [](tr_area t1, tr_area t2){return t1.area < t2.area;});
+
+  for (int i = 0; i < n; i++) {
+
+    // obtain random numbers between 0 and 1
+    double r = std::rand() / double(RAND_MAX);
+    double alpha = std::rand() / double(RAND_MAX);
+    double beta = std::rand() / double(RAND_MAX);
+
+    // perform binary search to find the first entry C[t] in C > r
+    int t = std::lower_bound(C, C + m, tr_area{.Fi = -1, .area = r}, 
+      [](tr_area t1, tr_area t2){return t1.area < t2.area;}) - C;
+    Eigen::Vector3d v1 = V.row(F(C[t].Fi, 0));
+    Eigen::Vector3d v2 = V.row(F(C[t].Fi, 1));
+    Eigen::Vector3d v3 = V.row(F(C[t].Fi, 2));
+
+    // pick a random point on the triangle
+    if (alpha + beta > 1) {
+      alpha = 1 - alpha;
+      beta = 1 - beta;
+    }
+    Eigen::Vector3d x = v1 + alpha * (v2 - v1) + beta * (v3 - v1);
+
+    X.row(i) = x;
+  }
 }