Yanjun Jiang A2 Submission

src/closest_rotation.cpp

@@ -1,9 +1,25 @@
 #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();
+	using namespace Eigen;
+
+	// Calculate UV
+	JacobiSVD<MatrixXd> svd(M, ComputeThinU | ComputeThinV);
+	Matrix3d U, V, omega;
+	U = svd.matrixU();
+	V = svd.matrixV();
+
+	// Calculate omega
+	omega = Matrix3d::Identity();
+	omega(2, 2) = (U * V.transpose()).determinant();
+
+	// Calculate R
+	R.resize(3, 3);
+	R = U * omega * 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,
@@ -8,5 +10,16 @@ double hausdorff_lower_bound(
   const int n)
 {
   // Replace with your code
-  return 0;
+	// Random sample points on mesh VX
+	Eigen::MatrixXd X;  // Points randomly sampled on X mesh
+	random_points_on_mesh(n, VX, FX, X);
+
+	// Compute the shortest point to mesh distance for all sampled points
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P;
+	Eigen::MatrixXd N;
+	point_mesh_distance(X, VY, FY, D, P, N);
+
+	// Get the hausdorff lower bound
+	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,18 @@ void icp_single_iteration(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	// Sample source mesh X
+	Eigen::MatrixXd X;
+	random_points_on_mesh(num_samples, VX, FX, X);
+
+	// Porject all X onto target mesh Y
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P, N;
+	point_mesh_distance(X, VY, FY, D, P, N);
+
+	// Update rigid transform to best match X and P
+	if (method == ICP_METHOD_POINT_TO_POINT)
+		point_to_point_rigid_matching(X, P, R, t);
+	else // method == ICP_METHOD_POINT_TO_PLANE
+		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 <igl/per_face_normals.h>
+#include "point_triangle_distance.h"
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -10,7 +12,32 @@ void point_mesh_distance(
 {
   // 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());
+
+  // Compute normal for all input triangles
+  Eigen::MatrixXd NY;
+  igl::per_face_normals(VY, FY, Eigen::Vector3d(1,1,1).normalized(), NY);
+
+  // Compute shortest distance for each query point
+  for (int i = 0; i < X.rows(); i++) {
+	  double shortest = -1;
+	  double dis;
+	  int triangle_idx;
+	  Eigen::RowVector3d curPoint;
+	  Eigen::RowVector3d shortPoint;
+	  // Find the triangle with the shortest distance
+	  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)), dis, curPoint);
+		  if (dis < shortest || shortest == -1) {
+			  shortest = dis;
+			  shortPoint = curPoint;
+			  triangle_idx = j;
+		  }
+	  }
+	  D(i) = shortest;
+	  P.row(i) = shortPoint;
+	  N.row(i) = NY.row(triangle_idx);
+  }
 }

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,
@@ -8,6 +10,59 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	int k = X.rows();
+	// Approximate point-to-plane minimizer
+	// Construct diag_N
+	Eigen::MatrixXd diag_N;
+	diag_N.resize(k, 3*k);
+	diag_N.block(0, 0, k, k) = N.col(0).asDiagonal();
+	diag_N.block(0, k, k, k) = N.col(1).asDiagonal();
+	diag_N.block(0, 2*k, k, k) = N.col(2).asDiagonal();
+
+	// Construct A
+	Eigen::MatrixXd A;
+	A = Eigen::MatrixXd::Zero(k*3, 6);
+	A.block(0, 1, k, 1) = X.col(2);
+	A.block(0, 2, k, 1) = -X.col(1);
+	A.block(0, 3, k, 1) = Eigen::VectorXd::Ones(k);
+	A.block(k, 0, k, 1) = -X.col(2);
+	A.block(k, 2, k, 1) = X.col(0);
+	A.block(k, 4, k, 1) = Eigen::VectorXd::Ones(k);
+	A.block(2*k, 0, k, 1) = X.col(1);
+	A.block(2*k, 1, k, 1) = -X.col(0);
+	A.block(2*k, 5, k, 1) = Eigen::VectorXd::Ones(k);
+
+	// Construct XP = [X1 - P1][X2 - P2] [X3 - P3]
+	Eigen::VectorXd XP, XP1, XP2, XP3;
+	XP1 = X.col(0) - P.col(0);
+	XP2 = X.col(1) - P.col(1);
+	XP3 = X.col(2) - P.col(2);
+	XP.resize(XP1.rows() + XP2.rows() + XP3.rows());
+	XP << XP1, XP2, XP3;
+
+	// Solve Wu = Z to get the optimal u
+	// Z = -N*XP, W = N*A
+	Eigen::VectorXd u(6);
+	Eigen::MatrixXd W;
+	Eigen::VectorXd Z;
+	Eigen::MatrixXd NA;
+	NA = diag_N * A;
+	W = NA.transpose()*NA;
+	Z = NA.transpose()*(diag_N*XP);
+	u = W.colPivHouseholderQr().solve(-Z);
+
+	// Compute optimal R
+	Eigen::MatrixXd M = Eigen::MatrixXd::Identity(3, 3);
+	M(0, 1) = -u(2);
+	M(0, 2) = u(1);
+	M(1, 0) = u(2);
+	M(1, 2) = -u(0);
+	M(2, 0) = -u(1);
+	M(2, 1) = u(0);
+	closest_rotation(M.transpose(), R);
+
+	// Compute optimal t
+	t(0) = u(3);
+	t(1) = u(4);
+	t(2) = u(5);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,4 +1,5 @@
 #include "point_to_point_rigid_matching.h"
+#include "closest_rotation.h"
 #include <igl/polar_svd.h>
 
 void point_to_point_rigid_matching(
@@ -8,7 +9,23 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	// Closed-form point-to-point minimizer
+	Eigen::RowVector3d X_centroid, P_centroid;
+	Eigen::MatrixXd X_bar, P_bar;
+
+	// Compute centroids
+	X_centroid = X.colwise().mean();
+	P_centroid = P.colwise().mean();
+
+	// Construct P_bar and X_bar
+	X_bar = X.rowwise() - X_centroid;
+	P_bar = P.rowwise() - P_centroid;
+	Eigen::MatrixXd M = X_bar.transpose()*P_bar;
+
+	// Find optimal R
+	closest_rotation(M, R);
+
+	// Find optimal t
+	t = P_centroid.transpose() - R*(X_centroid.transpose());
 }
 

src/point_triangle_distance.cpp

@@ -9,6 +9,85 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+
+	Eigen::RowVector3d v0 = x - a;
+	Eigen::RowVector3d v1 = b - a;
+	Eigen::RowVector3d v2 = c - a;
+	Eigen::RowVector3d n = v1.cross(v2);
+	n.normalize();
+	double dis = abs(v0.dot(n));
+	Eigen::RowVector3d x0 = x - dis * n; // Point projected to plane of the triangle
+	Eigen::RowVector3d v3 = x0 - a;
+	double v11 = v1.dot(v1);
+	double v12 = v1.dot(v2);
+	double v22 = v2.dot(v2);
+	double v31 = v3.dot(v1);
+	double v32 = v3.dot(v2);
+	double invden = 1.0/(v22*v11 - v12*v12);
+	double alpha = (v22*v31 - v12 * v32)*invden;
+	double beta = (v11*v32 - v12 * v31)*invden;
+	if (alpha >= 0 && beta >= 0 && alpha + beta <= 1) {
+		// Inside target triangle
+		p = a + alpha*v1 + beta*v2;
+		d = dis;
+	}
+	else if (alpha >= 0 && beta >= 0 && alpha + beta > 1) {
+		// Porject x0 to line bc
+		Eigen::RowVector3d v_x0c = c - x0;
+		Eigen::RowVector3d v_x0b = b - x0;
+		Eigen::RowVector3d v_bc = c - b;
+		Eigen::RowVector3d dir = (v_x0c.cross(v_x0b)).cross(v_bc);
+		dir.normalize();
+		double dis_x0 = abs(v_x0b.dot(dir));
+		Eigen::RowVector3d x0_p = x0 + dis_x0 * dir;
+		double t = (x0_p - b).dot(v_bc) / v_bc.dot(v_bc);
+		if (t > 1)
+			p = c;
+		else if (t < 0)
+			p = b;
+		else // 0 <= t <= 1
+			p = x0_p;
+		d = (x - p).norm();
+	}
+	else if (alpha >= 0 && beta < 0) {
+		// Project x0 to line ab
+		Eigen::RowVector3d v_x0b = b - x0;
+		Eigen::RowVector3d v_x0a = a - x0;
+		Eigen::RowVector3d v_ab = b - a;
+		Eigen::RowVector3d dir = (v_x0b.cross(v_x0a)).cross(v_ab);
+		dir.normalize();
+		double dis_x0 = abs(v_x0a.dot(dir));
+		Eigen::RowVector3d x0_p = x0 + dis_x0 * dir;
+		double t = (x0_p - a).dot(v_ab) / v_ab.dot(v_ab);
+		if (t > 1)
+			p = b;
+		else if (t < 0)
+			p = a;
+		else // 0 <= t <= 1
+			p = x0_p;
+		d = (x - p).norm();
+	}
+	else if (alpha < 0 && beta >= 0) {
+		// Project x0 to line ac
+		Eigen::RowVector3d v_x0a = a - x0;
+		Eigen::RowVector3d v_x0c = c - x0;
+		Eigen::RowVector3d v_ca = a - c;
+		Eigen::RowVector3d dir = (v_x0a.cross(v_x0c)).cross(v_ca);
+		dir.normalize();
+		double dis_x0 = abs(v_x0c.dot(dir));
+		Eigen::RowVector3d x0_p = x0 + dis_x0 * dir;
+		double t = (x0_p - c).dot(v_ca) / v_ca.dot(v_ca);
+		if (t > 1)
+			p = a;
+		else if (t < 0)
+			p = c;
+		else // 0 <= t <= 1
+			p = x0_p;
+		d = (x - p).norm();
+	}
+	else {// (alpha < 0 && beta < 0)
+		// Closest point is a
+		p = a;
+		d = (x - p).norm();
+	}
 }

src/random_points_on_mesh.cpp

@@ -1,4 +1,23 @@
 #include "random_points_on_mesh.h"
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <random>
+
+int firstGreater(Eigen::VectorXd &list, int first, int last, double tarVal) {
+	int mid = first + (last - first) / 2;
+	if (last == 0) {
+		return 0;
+	}
+	else if (list(mid) > tarVal) {
+		if (list(mid - 1) <= tarVal)
+			return mid;
+		else
+			firstGreater(list, first, mid - 1, tarVal);
+	}
+	else {
+		firstGreater(list, mid + 1, last, tarVal);
+	}
+}
 
 void random_points_on_mesh(
   const int n,
@@ -7,7 +26,36 @@ void random_points_on_mesh(
   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 cumulative sum C of relative areas
+	Eigen::VectorXd A(F.rows());	// Vector containing area for each triangle
+	Eigen::VectorXd C(F.rows());  // Vector containing cumulative area	
+	double gamma, alpha, beta;  // Random variables
+	int ttidx;  //target triangle index - triangle to sample point from
+	// Continuous uniform random sampling engine
+	std::random_device rd;
+	std::default_random_engine generator(rd());
+	std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+	// Compute cumulative sum of triangle area
+	igl::doublearea(V, F, A);
+	igl::cumsum(A/2.0, 1, C);
+	C /= C(C.rows() - 1);  // Normalize cumulative sum
+
+	X.resize(n, 3);
+	for (int i = 0; i < X.rows(); i++) {
+		// Area-weighted random sampling of triangles
+		gamma = distribution(generator);
+		ttidx = firstGreater(C, 0, C.size() - 1, gamma);
+
+		// Uniform random point sampling
+		alpha = distribution(generator);
+		beta = distribution(generator);
+		int v1 = F(ttidx, 0), v2 = F(ttidx, 1), v3 = F(ttidx, 2);
+		if (alpha + beta > 1) {
+			alpha = 1 - alpha;
+			beta = 1 - beta;
+		}
+		X.row(i) = V.row(v1) + alpha*(V.row(v2) - V.row(v1)) + beta*(V.row(v3) - V.row(v1));
+	}
 }