submit

src/closest_rotation.cpp

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

src/hausdorff_lower_bound.cpp

@@ -1,12 +1,22 @@
 #include "hausdorff_lower_bound.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
 
 double hausdorff_lower_bound(
-  const Eigen::MatrixXd & VX,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int n)
+	const Eigen::MatrixXd & VX,
+	const Eigen::MatrixXi & FX,
+	const Eigen::MatrixXd & VY,
+	const Eigen::MatrixXi & FY,
+	const int n)
 {
-  // Replace with your code
-  return 0;
+	// random choose points
+	Eigen::MatrixXd X;
+	random_points_on_mesh(n, VX, FX, X);
+
+	// compute point to mesh distance
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P, N;
+	point_mesh_distance(X, VY, FY, D, P, N);
+
+	return D.maxCoeff();
 }

src/icp_single_iteration.cpp

@@ -1,16 +1,29 @@
 #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,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int num_samples,
-  const ICPMethod method,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+	const Eigen::MatrixXd & VX,
+	const Eigen::MatrixXi & FX,
+	const Eigen::MatrixXd & VY,
+	const Eigen::MatrixXi & FY,
+	const int num_samples,
+	const ICPMethod method,
+	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::VectorXd D;
+	Eigen::MatrixXd P, N;
+	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 if (method == ICP_METHOD_POINT_TO_PLANE) {
+		point_to_plane_rigid_matching(X, P, N, R, t);
+	}
 }

src/point_mesh_distance.cpp

@@ -1,16 +1,43 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
 
 void point_mesh_distance(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  Eigen::VectorXd & D,
-  Eigen::MatrixXd & P,
-  Eigen::MatrixXd & N)
+	const Eigen::MatrixXd & X,
+	const Eigen::MatrixXd & VY,
+	const Eigen::MatrixXi & FY,
+	Eigen::VectorXd & D,
+	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();
+	D.resize(X.rows());
+	P.resizeLike(X);
+	N.resizeLike(X);
+
+	Eigen::MatrixXi F(X.rows(), 3);
+	for (int i = 0; i < X.rows(); i++) {
+		Eigen::RowVector3d x = X.row(1);
+		Eigen::RowVector3d point;
+		Eigen::RowVector3d normal;
+		double distance = INT_MAX;
+		int face_idx;
+		// find closest point
+		for (int j = 0; j < FY.rows(); j++) {
+			double d;
+			Eigen::RowVector3d p;
+			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);
+			if (d < distance) {
+				distance = d;
+				point = p;
+				face_idx = j;
+			}
+		}
+		D(i) = distance;
+		P.row(i) = point;
+		F.row(i) = FY.row(face_idx);
+	}
+	igl::per_face_normals(VY, F, N);
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,13 +1,40 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  const Eigen::MatrixXd & N,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+	const Eigen::MatrixXd & X,
+	const Eigen::MatrixXd & P,
+	const Eigen::MatrixXd & N,
+	Eigen::Matrix3d & R,
+	Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+	// compute [diag(N0), diag(N1), diag(N2)]
+	Eigen::MatrixXd normal(X.rows(), 3*X.rows());
+	Eigen::MatrixXd N0 = N.col(0).asDiagonal();
+	Eigen::MatrixXd N1 = N.col(1).asDiagonal();
+	Eigen::MatrixXd N2 = N.col(2).asDiagonal();
+	normal << N0, N1, N2;
+	// compute A
+	Eigen::RowVectorXd zero = Eigen::RowVectorXd::Zero(X.rows());
+	Eigen::RowVectorXd ones = Eigen::RowVectorXd::Ones(X.rows());
+	Eigen::MatrixXd A(3*X.rows(), 6);
+	A << zero, X.col(2), -X.col(1), ones, zero, zero,
+		-X.col(2), 0, X.col(0), zero, ones, zero,
+		X.col(1), -X.col(0), zero, zero, zero, ones;
+	// compute X - P
+	Eigen::MatrixXd xp(3*X.rows(), 3);
+	xp << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2);
+	// compute u
+	A = normal * A;
+	xp = normal * xp;
+	Eigen::RowVectorXd u;
+	// u = [alpha, beta, gamma, t0, t1, t2]
+	u = (A.transpose() * A).inverse() * -A.transpose() * xp;
+	// get t
+	t << u(3), u(4), u(5);
+	// compute M and R
+	Eigen::Matrix3d M;
+	M << 1, -u(2), u(1), u(2), 1, -u(0), -u(1), u(0), 1;
+	closest_rotation(M, R);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,22 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include "closest_rotation.h"
 
 void point_to_point_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+	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();
+	// closed form
+	Eigen::RowVector3d x = X.colwise().mean();
+	Eigen::RowVector3d p = P.colwise().mean();
+	Eigen::MatrixXd x_bar = X.rowwise() - x;
+	Eigen::MatrixXd p_bar = P.rowwise() - p;
+
+	Eigen::Matrix3d M = p_bar.transpose() * x_bar;
+	closest_rotation(M, R);
+
+	// p - (R * x^T)^T = p - x * R^T
+	t = p - x * R.transpose();
 }
 

src/point_triangle_distance.cpp

@@ -1,14 +1,49 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Dense>
 
 void point_triangle_distance(
-  const Eigen::RowVector3d & x,
-  const Eigen::RowVector3d & a,
-  const Eigen::RowVector3d & b,
-  const Eigen::RowVector3d & c,
-  double & d,
-  Eigen::RowVector3d & p)
+	const Eigen::RowVector3d & x,
+	const Eigen::RowVector3d & a,
+	const Eigen::RowVector3d & b,
+	const Eigen::RowVector3d & c,
+	double & d,
+	Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+	// calculate normal vector
+	Eigen::RowVector3d normal = (a - b).cross(a - c);
+	normal.normalize();
+
+	// project x on to triangle plane
+	Eigen::RowVector3d px = (x - a).dot(normal) * normal + x;
+
+	/* 
+		1 |6 / 3
+		--A-C---
+		4 |/ 5
+		  B
+		 /|
+		/2|      */
+	// if following value is positive, then px is in the triangle side of edge
+	double in_ab = (a - px).cross(a - b).dot(normal);
+	double in_bc = (b - px).cross(b - c).dot(normal);
+	double in_ca = (c - px).cross(c - a).dot(normal);
+
+	// find p
+	if (in_ab >= 0 && in_bc >= 0 && in_ca >= 0) {
+		p = px; // inside
+	} else if (in_ab < 0 && !in_ca < 0) {
+		p = a; // case 1
+	} else if (in_ab < 0 && in_bc < 0) {
+		p = b; // case 2
+	} else if (in_bc < 0 && in_ca < 0) {
+		p = c; // case 3
+	} else if (in_ab < 0) {
+		p = (a - px).dot(a - b) * (a - b) + a; // case 4
+	} else if (in_bc < 0) {
+		p = (b - px).dot(b - c) * (b - c) + b; // case 5
+	} else if (in_ca < 0) {
+		p = (c - px).dot(c - a) * (c - a) + c; // case 6
+	}
+
+	d = (x - p).norm();
 }

src/random_points_on_mesh.cpp

@@ -1,13 +1,58 @@
 #include "random_points_on_mesh.h"
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+
+
+int binary_find(Eigen::VectorXd lst, double val) {
+	int idx = 0;
+	if (lst.rows() <= 1) return idx;
+
+	int mid = lst.rows()/2;
+	if (lst(mid) < val) {
+		idx = mid + binary_find(lst.segment(mid, lst.rows()), val);
+	} else {
+		idx = binary_find(lst.head(mid), val);
+	}
+	return idx;
+}
+
 
 void random_points_on_mesh(
-  const int n,
-  const Eigen::MatrixXd & V,
-  const Eigen::MatrixXi & F,
-  Eigen::MatrixXd & X)
+	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());
-}
+	// calculate sumulative sum
+	Eigen::VectorXd dblx, csum;
+	igl::doublearea(V, F, dblx);
+	igl::cumsum(dblx, 1, csum);
+	double Ax = csum(csum.rows() - 1);
+	csum /= Ax;
 
+	// uniform distribution generator
+	std::random_device rd;
+	std::mt19937 gen(rd());
+	std::uniform_real_distribution<> dis(0.0, 1.0);
+
+	X.resize(n, 3);
+	for (int i = 0; i < X.rows(); i++) {
+		// find random index idx
+		double gamma = dis(gen);
+		int idx = binary_find(csum, gamma);
+
+		// generate and configurate alpha, beta
+	    double alpha = dis(gen);
+	    double beta = dis(gen);
+	    if (alpha + beta > 1) {
+	    	alpha = 1 - alpha;
+	    	beta = 1 - beta;
+	    }
+
+	    Eigen::VectorXd v1 = V.row(F(idx, 0));
+	    Eigen::VectorXd v2 = V.row(F(idx, 1));
+	    Eigen::VectorXd v3 = V.row(F(idx, 2));
+
+		X.row(i) = v1 + alpha * (v2 - v1) + beta * (v3 - v1);
+	}
+}