diff --git a/include/util.h b/include/util.h new file mode 100644 index 0000000..fcd09a8 --- /dev/null +++ b/include/util.h @@ -0,0 +1,16 @@ +#pragma once +#include + +/** +* 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; +} \ No newline at end of file diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index 54403c1..1fa0f08 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -1,9 +1,23 @@ #include "closest_rotation.h" +#include +#include + +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 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; } diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..8c3cc68 100644 --- a/src/hausdorff_lower_bound.cpp +++ b/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; } diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp index 1e8fda9..b497c79 100644 --- a/src/icp_single_iteration.cpp +++ b/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); + } } diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..6d1d100 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,4 +1,9 @@ #include "point_mesh_distance.h" +#include "point_triangle_distance.h" +#include +#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 + +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); } + diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp index 079151f..91ce50a 100644 --- a/src/point_to_point_rigid_matching.cpp +++ b/src/point_to_point_rigid_matching.cpp @@ -1,14 +1,58 @@ #include "point_to_point_rigid_matching.h" -#include +#include + +#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); } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 6405100..a06af89 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -1,4 +1,53 @@ #include "point_triangle_distance.h" +#include "util.h" + +using namespace Eigen; + +struct Plane { + Eigen::Vector3d normal; + double d; + + bool inFront(Eigen::Vector3d point) { + return normal.dot(point) + d > 0; + } + + double getPointD(Eigen::Vector3d point) { + return normal.dot(point) + d; + } + + Eigen::Vector3d project(Eigen::Vector3d point) { + double pointD = normal.dot(point); + return point - normal * (d + pointD); + } +}; + +Plane makeTrianglePlane(Eigen::Vector3d a, Eigen::Vector3d b, + Eigen::Vector3d c) { + Eigen::Vector3d u = b - a; + Eigen::Vector3d v = c - a; + + Plane p; + p.normal = cross(u, v); + p.normal.normalize(); + + assert(abs(p.normal.dot(u)) < 0.0001 && abs(p.normal.dot(v)) < 0.0001); + + p.d = -a.dot(p.normal); + + return p; +} + +Plane makeEdgePlane(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c, + Eigen::Vector3d backPoint) { + Plane trianglePlane = makeTrianglePlane(a, b, c); + + if (trianglePlane.inFront(backPoint)) { + trianglePlane.normal = -trianglePlane.normal; + trianglePlane.d = -trianglePlane.d; + } + + return trianglePlane; +} void point_triangle_distance( const Eigen::RowVector3d & x, @@ -6,9 +55,77 @@ void point_triangle_distance( const Eigen::RowVector3d & b, const Eigen::RowVector3d & c, double & d, - Eigen::RowVector3d & p) -{ - // Replace with your code - d = 0; - p = a; + Eigen::RowVector3d & p) { + + Eigen::RowVector3d normal = cross((b - a), (c - a)); + normal.normalize(); + + Plane ab = makeEdgePlane(a, b, a + normal, c); + Plane bc = makeEdgePlane(b, c, b + normal, a); + Plane ca = makeEdgePlane(c, a, c + normal, b); + + // Check if the point is in the positive half space of each edge plane. + bool frontAB = ab.inFront(x); + bool frontBC = bc.inFront(x); + bool frontCA = ca.inFront(x); + + // Also check if the point projects onto a valid portion of each edge. + // We classify the point as being either before the first point of the + // edge (-1), between the endpoints (0), or after the second point on the + // edge (1). + Vector3d aToB = b - a; + Vector3d bToC = c - b; + Vector3d cToA = a - c; + assert(a.dot(aToB) < b.dot(aToB)); + assert(b.dot(bToC) < c.dot(bToC)); + assert(c.dot(cToA) < a.dot(cToA)); + int withinAB = -(a.dot(aToB) > x.dot(aToB)) + (x.dot(aToB) > b.dot(aToB)); + int withinBC = -(b.dot(bToC) > x.dot(bToC)) + (x.dot(bToC) > c.dot(bToC)); + int withinCA = -(c.dot(cToA) > x.dot(cToA)) + (x.dot(cToA) > a.dot(cToA)); + + // Compute projection onto the triangle plane. + Plane trianglePlane = makeTrianglePlane(a, b, c); + Eigen::RowVector3d projection = trianglePlane.project(x); + assert(abs(trianglePlane.getPointD(a)) < 0.001); + assert(abs(trianglePlane.getPointD(b)) < 0.001); + assert(abs(trianglePlane.getPointD(c)) < 0.001); + assert(abs(ab.normal.dot(trianglePlane.normal)) < 0.001); + + /* + * 1. If its behind all edge planes, the closest projection lies in the + * triangle. + * + * 2. If its in front of an edge plane and within the bounds of that edge, + * then the closest projection lies on that edge. + * + * 3. Otherwise, it must be at one of the corners. which one depends on + * which side of the edges it is on. + */ + if (!frontAB && !frontBC && !frontCA) { + + p = projection; + + } else if (frontAB && withinAB == 0) { + p = ab.project(projection); + } else if (frontBC && withinBC == 0) { + p = bc.project(projection); + } else if (frontCA && withinCA == 0) { + p = ca.project(projection); + } else if (withinAB < 0 && withinCA > 0) { + p = a; + } else if (withinBC < 0 && withinAB > 0) { + p = b; + } else if (withinCA < 0 && withinBC > 0) { + p = c; + } else { + + // Shouldn't happen. + assert(0 == 1); + } + + d = (x - p).norm(); + + assert(d - 0.0001 <= (x - a).norm()); + assert(d - 0.0001 <= (x - b).norm()); + assert(d - 0.0001 <= (x - c).norm()); } diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 6e85d75..c7b98d9 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -1,4 +1,13 @@ #include "random_points_on_mesh.h" +#include +#include +#include "igl/doublearea.h" +#include "igl/cumsum.h" + +using namespace Eigen; + +// Random number generator instance. +std::minstd_rand generator; void random_points_on_mesh( const int n, @@ -6,8 +15,57 @@ void random_points_on_mesh( const Eigen::MatrixXi & F, Eigen::MatrixXd & X) { - // REPLACE WITH YOUR CODE: - X.resize(n,3); - for(int i = 0;i cumulativeAreas; + for (int i = 0; i < cumSumAreas.rows(); i++) { + cumulativeAreas.push_back(cumSumAreas(i, 0)); + } + + // Generate the random points. + for (int i = 0; i < sampleCount; i++) { + std::uniform_real_distribution distribution(0, 1); + double sample1D = distribution(generator); + + // This employs binary search to convert the 1D sample into an actual + // triangle index. + auto comparator = [](double a, double b) -> bool { return a < b; }; + auto location = std::lower_bound(cumulativeAreas.begin(), + cumulativeAreas.end(), sample1D, comparator); + auto it = location - cumulativeAreas.begin(); + int triangleIndex = it; + + // Now we just have to sample within the triangle. We do the + // parallelogram thing to create good sample parameters. + double alpha = distribution(generator); + double beta = distribution(generator); + if (alpha + beta > 1) { + alpha = 1 - alpha; + beta = 1 - beta; + } + + // Now we just interpolate. The sample parameters are restricted + // already to form a triangle. + Vector3d p1 = V.row(F(triangleIndex, 0)); + Vector3d p2 = V.row(F(triangleIndex, 1)); + Vector3d p3 = V.row(F(triangleIndex, 2)); + Vector3d position = p1 + (p2 - p1) * alpha + (p3 - p1) * beta; + X.row(i) = position; + } + }