Finished the assignment.

include/util.h

@@ -0,0 +1,16 @@
+#pragma once
+#include <Eigen/Core>
+
+/**
+* Had some trouble getting cross-product to compile properly or something,
+* so I rolled my own.
+*/
+inline Eigen::Vector3d cross(Eigen::Vector3d a, Eigen::Vector3d b) {
+
+	double s1 = a(1) * b(2) - a(2) * b(1);
+	double s2 = a(2) * b(0) - a(0) * b(2);
+	double s3 = a(0) * b(1) - a(1) * b(0);
+
+	Eigen::Vector3d result(s1, s2, s3);
+	return result;
+}

src/closest_rotation.cpp

@@ -1,9 +1,23 @@
 #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();
+		const Eigen::Matrix3d& M,
+		Eigen::Matrix3d& R) {
+
+	// Compute U and V for the decomposition.
+	JacobiSVD<MatrixXd> svd(M, ComputeThinU | ComputeThinV);
+	const Matrix3d& u = svd.matrixU();
+	const Matrix3d& v = svd.matrixV();
+	Matrix3d vT = svd.matrixV();
+	vT.transposeInPlace();
+
+	Eigen::Matrix3d omega = Matrix3d::Identity();
+	double det = (u * vT).determinant();
+	omega(2, 2) = det;
+
+	R = u * omega * vT;
 }

src/hausdorff_lower_bound.cpp

@@ -1,12 +1,31 @@
 #include "hausdorff_lower_bound.h"
 
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+
+using namespace Eigen;
+
 double hausdorff_lower_bound(
-  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;
+		const Eigen::MatrixXd & VX,
+		const Eigen::MatrixXi & FX,
+		const Eigen::MatrixXd & VY,
+		const Eigen::MatrixXi & FY,
+		const int n) {
+
+	// Sample points on the first mesh.
+	MatrixXd xPoints;
+	random_points_on_mesh(n, VX, FX, xPoints);
+
+	// Compute distances.
+	VectorXd distances;
+	MatrixXd points;
+	MatrixXd normals;
+	point_mesh_distance(xPoints, VY, FY, distances, points, normals);
+
+	double distance = 0;
+	for (int i = 0; i < n; i++) {
+		distance = std::max(distance, distances(i));
+	}
+
+	return distance;
 }

src/icp_single_iteration.cpp

@@ -1,16 +1,32 @@
 #include "icp_single_iteration.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+
+using namespace Eigen;
 
 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)
-{
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+		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) {
+
+	MatrixXd X;
+	random_points_on_mesh(num_samples, VX, FX, X);
+
+	VectorXd distances;
+	MatrixXd points;
+	MatrixXd normals;
+	point_mesh_distance(X, VY, FY, distances, points, normals);
+
+	if (method == ICPMethod::ICP_METHOD_POINT_TO_PLANE) {
+		point_to_plane_rigid_matching(X, points, normals, R, t);
+	} else {
+		point_to_point_rigid_matching(X, points, R, t);
+	}
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,9 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <Eigen/LU>
+#include "igl/per_face_normals.h"
+
+using namespace Eigen;
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +13,46 @@ 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 pointCount = X.rows();
+	int vertexCount = VY.rows();
+	int triangleCount = FY.rows();
+
+	P.resizeLike(X);
+
+	Eigen::MatrixXd FN(triangleCount, 3);
+	igl::per_face_normals(VY, FY, FN);
+
+	D.resize(X.rows());
+	N = Eigen::MatrixXd::Zero(X.rows(), X.cols());
+	for (int i = 0; i < pointCount; i++) {
+
+		Vector3d x = X.row(i);
+
+		double closestD = 9999999999999.0;
+		RowVector3d closestP;
+		RowVector3d closestN;
+		for (int j = 0; j < triangleCount; j++) {
+
+			int aIndex = FY(j, 0);
+			int bIndex = FY(j, 1);
+			int cIndex = FY(j, 2);
+			Vector3d a = VY.row(aIndex);
+			Vector3d b = VY.row(bIndex);
+			Vector3d c = VY.row(cIndex);
+
+			RowVector3d p;
+			double d;
+			point_triangle_distance(x, a, b, c, d, p);
+
+			if (d < closestD) {
+				closestP = p;
+				closestD = d;
+				closestN = FN.row(j);
+			}
+		}
+
+		D(i) = closestD;
+		P.row(i) = closestP;
+		N.row(i) = closestN;
+	}
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,8 @@
 #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 +11,57 @@ 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);
+	A.setZero(3 * k, 6);
+	VectorXd b(3 * k);
+
+	// Set up the matrix A.
+	for (int i = 0; i < k; i++) {
+		A(i * 3, 1) = X(i, 2);
+		A(i * 3, 2) = -X(i, 1);
+		A(i * 3, 3) = 1;
+
+		A(i * 3 + 1, 0) = -X(i, 2);
+		A(i * 3 + 1, 2) = X(i, 0);
+		A(i * 3 + 1, 4) = 1;
+
+		A(i * 3 + 2, 0) = X(i, 1);
+		A(i * 3 + 2, 1) = -X(i, 0);
+		A(i * 3 + 2, 5) = 1;
+
+		b(i * 3) = X(i, 0) - P(i, 0);
+		b(i * 3 + 1) = X(i, 1) - P(i, 1);
+		b(i * 3 + 2) = X(i, 2) - P(i, 2);
+	}
+
+	MatrixXd diagN1 = N.col(0).asDiagonal();
+	MatrixXd diagN2 = N.col(1).asDiagonal();
+	MatrixXd diagN3 = N.col(2).asDiagonal();
+	MatrixXd diagonals(diagN1.rows(), diagN1.cols() * 3);
+	diagonals << diagN1, diagN2, diagN3;
+
+	// The algorithm is like point -> point, but with the diagonals matrix, 
+	// so I just factor it in, in advance.
+	A = diagonals * A;
+	b = diagonals * b;
+
+	VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * b);
+	double alpha = u(0);
+	double beta = u(1);
+	double gamma = u(2);
+	t = Eigen::RowVector3d(u(3), u(4), u(5));
+
+	Matrix3d M = Eigen::Matrix3d::Identity();
+	M(0, 1) = -gamma;
+	M(0, 2) = beta;
+	M(1, 0) = gamma;
+	M(1, 2) = -alpha;
+	M(2, 0) = -beta;
+	M(2, 1) = alpha;
+
+	M.transposeInPlace();
+
+	closest_rotation(M, R);
 }
+

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,58 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include <Eigen/LU>
+
+#include "closest_rotation.h"
+
+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();
+		const Eigen::MatrixXd & X,
+		const Eigen::MatrixXd & P,
+		Eigen::Matrix3d & R,
+		Eigen::RowVector3d & t) {
+
+	int k = X.rows();
+	MatrixXd A(3 * k, 6);
+	A.setZero(3 * k, 6);
+	VectorXd b(3 * k);
+
+	// Set up the matrix A.
+	for (int i = 0; i < k; i++) {
+		A(i * 3, 1) = X(i, 2);
+		A(i * 3, 2) = -X(i, 1);
+		A(i * 3, 3) = 1;
+
+		A(i * 3 + 1, 0) = -X(i, 2);
+		A(i * 3 + 1, 2) = X(i, 0);
+		A(i * 3 + 1, 4) = 1;
+
+		A(i * 3 + 2, 0) = X(i, 1);
+		A(i * 3 + 2, 1) = -X(i, 0);
+		A(i * 3 + 2, 5) = 1;
+
+		b(i * 3) = X(i, 0) - P(i, 0);
+		b(i * 3 + 1) = X(i, 1) - P(i, 1);
+		b(i * 3 + 2) = X(i, 2) - P(i, 2);
+	}
+
+	VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * b);
+	double alpha = u(0);
+	double beta = u(1);
+	double gamma = u(2);
+	t = Eigen::RowVector3d(u(3), u(4), u(5));
+
+	Matrix3d M = Eigen::Matrix3d::Identity();
+	M(0, 1) = -gamma;
+	M(0, 2) = beta;
+	M(1, 0) = gamma;
+	M(1, 2) = -alpha;
+	M(2, 0) = -beta;
+	M(2, 1) = alpha;
+
+	// Tried this and for some reason it worked??? Don't think it said to 
+	// do this in the readme.
+	M.transposeInPlace();
+
+	closest_rotation(M, R);
 }