Adrian 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,16 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/LU>
+#include <iostream>
 
 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 Omega = (svd.matrixU() * svd.matrixV().transpose());
+  R(2,2) = Omega.determinant();
+  R = svd.matrixU() * R * svd.matrixV().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,16 @@ double hausdorff_lower_bound(
   const int n)
 {
   // Replace with your code
-  return 0;
+  // Sample points from X mesh
+  Eigen::MatrixXd sample_P;
+  random_points_on_mesh(n, VX, FX, sample_P);
+
+  // Figure out their closest points to the Y mesh
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P;
+  Eigen::MatrixXd N;
+  point_mesh_distance(sample_P, VY, FY, D, P, N);
+
+  // compute max D for the hausdorff lb
+  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,
@@ -11,6 +15,23 @@ void icp_single_iteration(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+
+  // Sample source mesh
+  Eigen::MatrixXd X;
+  random_points_on_mesh(num_samples, VX, FX, X);
+
+  // Project sample points onto mesh Y
+  Eigen::VectorXd D;
+  Eigen::MatrixXd Py;
+  Eigen::MatrixXd Ny;
+  point_mesh_distance(X, VY, FY, D, Py, Ny);
+
+  // Do one iteration of the chosen method to obtain an R and a t
+  if (method == ICP_METHOD_POINT_TO_POINT) {
+	point_to_point_rigid_matching(X, Py, R, t);
+  }
+
+  else if (method == ICP_METHOD_POINT_TO_PLANE) {
+	point_to_plane_rigid_matching(X, Py, Ny, 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,
@@ -9,8 +11,36 @@ void point_mesh_distance(
   Eigen::MatrixXd & N)
 {
   // Replace with your code
+  D.resize(X.rows());
   P.resizeLike(X);
+  Eigen::MatrixXd NY = Eigen::MatrixXd::Zero(FY.rows(), 3);
+  igl::per_face_normals(VY, FY, Eigen::Vector3d::Zero(), NY);
   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();
+  Eigen::RowVectorXd distances(FY.rows());
+  Eigen::MatrixXd points(FY.rows(), 3);
+
+  for (int i = 0; i < X.rows(); i++) {
+	Eigen::RowVector3d x = X.row(i);
+        double d = 0;
+        Eigen::RowVector3d p;
+
+        // Find closest point of x to each face 
+        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));
+	 	point_triangle_distance(x, a, b, c, d, p);
+		distances(j) = d;
+                points.row(j) = p;
+        }
+
+	// Get distance, normal, and point information from that search
+        D(i) = distances.minCoeff();
+        for (int j = 0; j < FY.rows(); j++) {
+		if (distances(j) == D(i)) {
+			P.row(i) = points.row(j);
+			N.row(i) = NY.row(j); 
+		}
+        }
+  }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +9,45 @@ 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 matrix A containing points 
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * X.rows(), 6);
+  Eigen::VectorXd b(3 * X.rows());
+  A.block(0, 1, X.rows(), 1) = -X.col(2);
+  A.block(0, 2, X.rows(), 1) = X.col(1);
+  A.block(0, 3, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+  A.block(X.rows(), 0, X.rows(), 1) = X.col(2);
+  A.block(X.rows(), 2, X.rows(), 1) = -X.col(0);
+  A.block(X.rows(), 4, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+  A.block(2 * X.rows(), 0, X.rows(), 1) = -X.col(1);
+  A.block(2 * X.rows(), 1, X.rows(), 1) = X.col(0);
+  A.block(2 * X.rows(), 5, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+
+  // Construct matrix D of normals
+  Eigen::MatrixXd D(X.rows(), 3 * X.rows());
+  Eigen::MatrixXd D_1 = N.col(0).asDiagonal();
+  Eigen::MatrixXd D_2 = N.col(1).asDiagonal();
+  Eigen::MatrixXd D_3 = N.col(2).asDiagonal();
+  D.block(0, 0, X.rows(), X.rows()) = D_1;
+  D.block(0, X.rows(), X.rows(), X.rows()) = D_2;
+  D.block(0, 2 * X.rows(), X.rows(), X.rows()) = D_3;
+
+  // Construct vector P -X
+  b << P.col(0) - X.col(0),
+       P.col(1) - X.col(1),
+       P.col(2) - X.col(2);
+
+  // Solve for the least squares solution of ||DAx - D(P-X)||^2. 
+  A = D * A;
+  b = D * b;
+  Eigen::MatrixXd L = A.transpose() * A;
+  Eigen::VectorXd r = A.transpose() * b;
+  Eigen::VectorXd u = L.fullPivLu().solve(r);
+
+  // Extract R and t from solution of optimization problem
+  Eigen::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,5 +1,6 @@
 #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,
@@ -8,7 +9,36 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+
+  // Construct matrix A and vector P - X
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * X.rows(), 6);
+  Eigen::VectorXd b(3 * X.rows());
+  A.block(0, 1, X.rows(), 1) = -X.col(2);
+  A.block(0, 2, X.rows(), 1) = X.col(1);
+  A.block(0, 3, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+  A.block(X.rows(), 0, X.rows(), 1) = X.col(2);
+  A.block(X.rows(), 2, X.rows(), 1) = -X.col(0);
+  A.block(X.rows(), 4, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+  A.block(2 * X.rows(), 0, X.rows(), 1) = -X.col(1);
+  A.block(2 * X.rows(), 1, X.rows(), 1) = X.col(0);
+  A.block(2 * X.rows(), 5, X.rows(), 1) = Eigen::MatrixXd::Constant(X.rows(), 1, 1);
+
+  b << P.col(0) - X.col(0),
+       P.col(1) - X.col(1),
+       P.col(2) - X.col(2);
+
+  // Solve for the least squares solution u, which minimizes ||Au - (P - X)||^2. 
+  Eigen::MatrixXd L = A.transpose() * A;
+  b = A.transpose() * b;
+  Eigen::VectorXd u = L.fullPivLu().solve(b);
+
+  // Extract the new R and t
+  Eigen::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_triangle_distance.cpp

@@ -9,6 +9,49 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+
+  // Project on plane spanned by triangle and see if it lies in it
+  // Consulted formula at https://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle 
+
+  // Project x on plane containing the triangle
+  Eigen::RowVector3d u = b - a;
+  Eigen::RowVector3d v = c - a;
+  Eigen::RowVector3d tri_norm = u.cross(v);
+  tri_norm = tri_norm / tri_norm.norm();
+  Eigen::RowVector3d w = x - a;
+  double gamma = (u.cross(w)).dot(tri_norm);
+  double beta = (w.cross(v)).dot(tri_norm);
+  double alpha = 1 - gamma - beta;
+
+  // if projection lies outside triangle, project on different lines of the triangle
+
+  if (alpha > 1 || alpha < 0 || beta > 1 || beta < 0 || gamma > 0 || gamma < 1) {
+ 	// consulted formula at https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line 
+	double proj_ab = (a - x).dot(u) / (u.norm() * u.norm()); 
+	double proj_ac = (a - x).dot(v) / (v.norm() * v.norm());
+	double proj_bc = (b - x).dot(v-u) / ((v-u).norm() * (v-u).norm());
+        proj_ab = std::min(std::max(proj_ab, 0.0), 1.0);
+        proj_ac = std::min(std::max(proj_ac, 0.0), 1.0);
+        proj_bc = std::min(std::max(proj_bc, 0.0), 1.0);
+  	double dist_ab = ((a-x) - (proj_ab * u)).norm();
+  	double dist_ac = ((a-x) - (proj_ac * v)).norm();
+  	double dist_bc = ((b-x) - (proj_bc * (v-u))).norm();
+	double min_dist = std::min(std::min(dist_ab, dist_ac), dist_bc);
+	if (min_dist == dist_ac) {
+		d = dist_ac;
+		p = proj_ab * u + a;
+	}
+	if (min_dist == dist_ac) {
+		d = dist_ac;
+		p = proj_ac * v + a;
+	}
+	if (min_dist == dist_bc) {
+		d = dist_bc;
+		p = proj_bc * (v-u) + b;
+	}
+  } 
+  else { 
+  	p = alpha * a + beta * b + gamma * c;
+  	d = (x - p).norm();
+  }
 }

src/random_points_on_mesh.cpp

@@ -1,4 +1,7 @@
 #include "random_points_on_mesh.h"
+#include <igl/cumsum.h>
+#include <igl/doublearea.h>
+#include <random>
 
 void random_points_on_mesh(
   const int n,
@@ -8,6 +11,40 @@ 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());
+
+  // Compute areas
+  Eigen::MatrixXd A(V.rows(), 1);
+  Eigen::MatrixXd cum_A(V.rows(), 1);
+  igl::doublearea(V,F, A);
+  igl::cumsum(A, 1, cum_A);
+  cum_A = cum_A / A.sum();
+
+  // Generate random numbers;
+  std::default_random_engine generator;
+  std::uniform_real_distribution<double> distribution(0.0,1.0);
+
+  for(int i = 0;i<X.rows();i++) {
+	double alpha = distribution(generator);
+        double beta = distribution(generator); 
+        double gamma = distribution(generator);
+        if (alpha + beta > 1) {
+	    alpha = 1 - alpha;
+	    beta = 1 - beta;
+	}
+
+ 	// Linear search on cum_A to find the index of face;
+	int idx = 0;
+	while (cum_A(idx) < gamma) {
+            idx++;
+        }
+
+        // Sample point from V(idx)
+        Eigen::Vector3i Tri = F.row(idx);
+        Eigen::Vector3d v = V.row(Tri(0));
+        Eigen::Vector3d w = V.row(Tri(1)) - V.row(Tri(0));
+        Eigen::Vector3d u = V.row(Tri(2)) - V.row(Tri(0));
+        Eigen::Vector3d point = v + alpha * u + beta * w;
+        X.row(i) = point;
+  }
 }