diff --git a/README.html b/README.html index 6c13281..c163d77 100644 --- a/README.html +++ b/README.html @@ -343,7 +343,7 @@

Constant function approximation

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\):

diff --git a/README.md b/README.md index bce5dc2..ce58753 100644 --- a/README.md +++ b/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$: diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index 54403c1..41c59dc 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -1,9 +1,29 @@ #include "closest_rotation.h" +#include +#include +#include void closest_rotation( const Eigen::Matrix3d & M, Eigen::Matrix3d & R) { // Replace with your code - R = Eigen::Matrix3d::Identity(); + // R = Eigen::Matrix3d::Identity(); + + // Compute the SVD of the given matrix M + + // exact solver - slow + Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); + + + Eigen::Matrix3d U = svd.matrixU(); + Eigen::Matrix3d V = svd.matrixV(); + + double det_uv = (U * V.transpose()).determinant(); + + Eigen::Matrix3d Omega = Eigen::Matrix3d::Identity(); + Omega(2,2) = det_uv; + + R = U * Omega * V.transpose(); + } diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..0612e51 100644 --- a/src/hausdorff_lower_bound.cpp +++ b/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,26 @@ double hausdorff_lower_bound( const int n) { // Replace with your code - return 0; + + // Sample points from X + Eigen::MatrixXd sampled_X(n, 3); + + random_points_on_mesh( + n, + VX, + FX, + sampled_X); + + Eigen::VectorXd D; + Eigen::MatrixXd P, N; + + point_mesh_distance( + sampled_X, + VY, + FY, + D, + P, + N); + + return D.maxCoeff(); } diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp index 1e8fda9..96c5239 100644 --- a/src/icp_single_iteration.cpp +++ b/src/icp_single_iteration.cpp @@ -1,4 +1,9 @@ #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" +#include void icp_single_iteration( const Eigen::MatrixXd & VX, @@ -13,4 +18,44 @@ void icp_single_iteration( // Replace with your code R = Eigen::Matrix3d::Identity(); t = Eigen::RowVector3d::Zero(); + + // Sample points from X + Eigen::MatrixXd sampled_X(num_samples, 3); + + random_points_on_mesh( + num_samples, + VX, + FX, + sampled_X); + + Eigen::VectorXd D; + Eigen::MatrixXd P, N; + + point_mesh_distance( + sampled_X, + VY, + FY, + D, + P, + N); + + + if (method == ICP_METHOD_POINT_TO_POINT) { + + point_to_point_rigid_matching( + sampled_X, + P, + R, + t); + + } else if (method == ICP_METHOD_POINT_TO_PLANE) { + + point_to_plane_rigid_matching( + sampled_X, + P, + N, + R, + t); + + } } diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..6dc29ba 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,4 +1,6 @@ #include "point_mesh_distance.h" +#include "point_triangle_distance.h" +#include void point_mesh_distance( const Eigen::MatrixXd & X, @@ -9,8 +11,68 @@ void point_mesh_distance( Eigen::MatrixXd & N) { // Replace with your code + // P.resizeLike(X); + // N = Eigen::MatrixXd::Zero(X.rows(),X.cols()); + // for(int i = 0;i +#include +#include "closest_rotation.h" void point_to_plane_rigid_matching( const Eigen::MatrixXd & X, @@ -10,4 +13,67 @@ void point_to_plane_rigid_matching( // Replace with your code R = Eigen::Matrix3d::Identity(); t = Eigen::RowVector3d::Zero(); + + + Eigen::MatrixXd N_x_diag(N.rows(), N.rows()); + Eigen::MatrixXd N_y_diag(N.rows(), N.rows()); + Eigen::MatrixXd N_z_diag(N.rows(), N.rows()); + N_x_diag = Eigen::MatrixXd(N.col(0).asDiagonal()); + N_y_diag = Eigen::MatrixXd(N.col(1).asDiagonal()); + N_z_diag = Eigen::MatrixXd(N.col(2).asDiagonal()); + + + Eigen::MatrixXd N_diag_concat(N.rows(), 3*N.rows()); + N_diag_concat << N_x_diag, N_y_diag, N_z_diag; + + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*N.rows(), 6); + + Eigen::VectorXd X_minus_P(3*N.rows()); + + // iterate over all x + for (int i=0; i +#include "closest_rotation.h" +// closed form solution void point_to_point_rigid_matching( const Eigen::MatrixXd & X, const Eigen::MatrixXd & P, @@ -10,5 +12,29 @@ void point_to_point_rigid_matching( // Replace with your code R = Eigen::Matrix3d::Identity(); t = Eigen::RowVector3d::Zero(); + + Eigen::Matrix3d M; + + // First find the average point in P and X + Eigen::Vector3d P_avg = P.colwise().mean(); + Eigen::Vector3d X_avg = X.colwise().mean(); + + + + + Eigen::MatrixXd X_bar = X.rowwise() - X_avg.transpose(); + Eigen::MatrixXd P_bar = P.rowwise() - P_avg.transpose(); + + + + M = P_bar.transpose() * X_bar; + + + closest_rotation( + M, + R); + + + t = (P_avg - R * X_avg).transpose(); } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 6405100..7e21269 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -1,4 +1,35 @@ #include "point_triangle_distance.h" +#include +#include + +/* +First project the given point on the plane of the triangle. +If the projected point lies inside the triangle, that that is the closest point. +If it lies outside, find the 2D projection of the projected point over one of the triangle edges based on +which region it falls into. + +If it lies outside, project the prev plane-projected point to all the three sides of the triangle. +For the three points that are projected, eliminate points that lie outside the triangle by the same check as before. +Now we have one point on some edge. Find the distance, and compare with the distances to the remaining vertices. +Report the least distance. +*/ + +// This function checks if a given point x_0 *in the plane of the triangle*, exists inside or not. +bool check_if_on_triangle(const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & c, + const Eigen::RowVector3d & x_0); + +// Projecting a plane-projected point over a line given by two coordinate points a and b +void project_on_line(const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & x_0, Eigen::RowVector3d & x_0_proj); + +// Given a point on the edge and the vertices of the triangle, this function populates the nearest point and distance +// for the cases where the plane-projected point lies outside of the triangle +void compare_distance_edgepoint_vertices (const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & c, + const Eigen::RowVector3d & x_0_ab, const Eigen::RowVector3d & x, + Eigen::RowVector3d & p, double & d); + +// populates minp to be one of p1 or p2 based on which is closest to x +void return_minimum_distant_point_to_x (const Eigen::RowVector3d & p1, const Eigen::RowVector3d & p2, + const Eigen::RowVector3d & x, Eigen::RowVector3d & minp); void point_triangle_distance( const Eigen::RowVector3d & x, @@ -9,6 +40,138 @@ void point_triangle_distance( Eigen::RowVector3d & p) { // Replace with your code - d = 0; - p = a; + // d = 0; + // p = a; + + Eigen::RowVector3d unit_normal = (b-a).cross(c-b).normalized(); + + // compute the difference vector of X with any one of the points on the triangle + Eigen::RowVector3d diff_vector = x - c; + + // Find the dot product with the unit normal + double proj_dot_product = unit_normal.dot(diff_vector); + + // Find the projection point x_0 of the input point x on the plan spanned by the vertices of the triangle + Eigen::RowVector3d x_0 = x - proj_dot_product * unit_normal; + + // If x_0 lies within the triangle, then abs(proj_dot_product) is the closest distance and x_0 is the + // closest point + + if (check_if_on_triangle(a, b, c, x_0)) { + // the projected point is within the triangle! + p(0) = x_0(0); p(1) = x_0(1); p(2) = x_0(2); + d = abs(proj_dot_product); + return; + + } else { + // the projected point is outside the triangular region + // in this case the closest point will be either a point on the edge or one of the vertex points + + // first find all the projections of x_0 on each of the edge, and then check if those lie on the triangle or not + Eigen::RowVector3d x_0_ab, x_0_bc, x_0_ca; + project_on_line(a, b, x_0, x_0_ab); + project_on_line(b, c, x_0, x_0_bc); + project_on_line(c, a, x_0, x_0_ca); + + // check if the points projected on the edge exist inside the triangle. if yes, consider them. + // then finally, compare dstance against vertices to pick the closest + + Eigen::RowVector3d p_closest_tmp; + p_closest_tmp(0) = a(0); p_closest_tmp(1) = a(1); p_closest_tmp(2) = a(2); + + if (check_if_on_triangle(a, b, c, x_0_ab)) { + compare_distance_edgepoint_vertices(p_closest_tmp, b, c, x_0_ab, x, p_closest_tmp, d); + + } + + if (check_if_on_triangle(a, b, c, x_0_bc)) { + compare_distance_edgepoint_vertices(p_closest_tmp, b, c, x_0_bc, x, p_closest_tmp, d); + + } + + if (check_if_on_triangle(a, b, c, x_0_ca)) { + compare_distance_edgepoint_vertices(p_closest_tmp, b, c, x_0_ca, x, p_closest_tmp, d); + + } + + // also check the vertices finally + compare_distance_edgepoint_vertices(p_closest_tmp, b, c, a, x, p, d); + return; + + + } + + +} + +// This function checks if a given point x_0 *in the plane of the triangle*, exists inside or not. +bool check_if_on_triangle(const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & c, + const Eigen::RowVector3d & x_0) { + + // Check if x_0 is within the triangle region + + // Any region within the triangle can be expressed as + // b + alpha * (a-b) + beta * (c-b) + + // Decompose the projected point x_0 based on alpha and beta by solving two linear equations obtained by + // (x_0 - b) dot (a-b), and + // (x_0 - b) dot (c-b) + + // Equations worked on the board to solve for alpha and beta as follows: + + double beta_numerator = ((a-b).dot(c-b) * (x_0-b).dot(a-b)) - ((x_0-b).dot(c-b) * (a-b).dot(a-b)); + double beta_denominator = std::pow((c-b).dot(a-b), 2.0) - ((a-b).dot(a-b) * (c-b).dot(c-b)); + + double beta = beta_numerator/beta_denominator; + + double alpha = ((x_0-b).dot(a-b) - beta * (c-b).dot(a-b))/(a-b).dot(a-b); + + if ((beta >= 0.0) && (alpha >= 0.0) && (alpha + beta <= 1.0)) { + // the projected point is within the triangle! + return true; + } + + return false; +} + +void project_on_line(const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & x_0, + Eigen::RowVector3d & x_0_proj) { + + // find the normal dir that points from the line to the point OUTWARDS + // normal is parallel to AB X (AP X AB) - Note that cross product is not associative! Where P = P(x_0) + + Eigen::RowVector3d AP_cross_AB = (x_0 - a).cross(b - a); + + Eigen::RowVector3d unitnorm_direction = (b-a).cross(AP_cross_AB).normalized(); + + x_0_proj = x_0 - ((x_0 - a).dot(unitnorm_direction)) * unitnorm_direction; + +} + +void compare_distance_edgepoint_vertices (const Eigen::RowVector3d & a, const Eigen::RowVector3d & b, const Eigen::RowVector3d & c, + const Eigen::RowVector3d & x_0_ab, const Eigen::RowVector3d & x, + Eigen::RowVector3d & p, double & d) { + + +Eigen::RowVector3d p_tmp1, p_tmp2, p_tmp3; + +return_minimum_distant_point_to_x(a, b, x, p_tmp1); +return_minimum_distant_point_to_x(p_tmp1, c, x, p_tmp2); +return_minimum_distant_point_to_x(p_tmp2, x_0_ab, x, p_tmp3); + +p(0) = p_tmp3(0); p(1) = p_tmp3(1); p(2) = p_tmp3(2); +d = (x - p).norm(); + +} + + +void return_minimum_distant_point_to_x (const Eigen::RowVector3d & p1, const Eigen::RowVector3d & p2, + const Eigen::RowVector3d & x, Eigen::RowVector3d & minp) { + +if ((p1-x).dot(p1-x) >= (p2-x).dot(p2-x)) { + minp(0) = p2(0); minp(1) = p2(1); minp(2) = p2(2); +} else { + minp(0) = p1(0); minp(1) = p1(1); minp(2) = p1(2); +} + } diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 6e85d75..9b55687 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -1,4 +1,12 @@ #include "random_points_on_mesh.h" +#include +#include +#include +#include + +// custom binary search function that returns the index of the first number greater than the input double find +int binary_search_first_greater_element(const Eigen::VectorXd & a, const double & find, int min_index, int max_index, + int ans_index = 0); void random_points_on_mesh( const int n, @@ -7,7 +15,88 @@ void random_points_on_mesh( Eigen::MatrixXd & X) { // REPLACE WITH YOUR CODE: + // X.resize(n,3); + // for(int i = 0;i dis(0.0, 1.0); + X.resize(n,3); - for(int i = 0;i 1.0) { + alpha = 1 - alpha; + beta = 1 - beta; + } + + // set the random point sampled + X.row(rand_num_count) = v1 + alpha * (v2 - v1) + beta * (v3 - v1); + } + + +} + + +// custom binary search function +int binary_search_first_greater_element(const Eigen::VectorXd & a, const double & find, int min_index, int max_index, + int ans_index) { + /* ans_index variable is to carry a possible answer index into the next recursion cycle so that the + first greatest number is only picked */ + + int sz = max_index - min_index + 1; + + if (sz == 1) { + if ((a(ans_index) > a(min_index)) && (a(min_index) > find)) { + return min_index; + } else { + return ans_index; + } + } + + if (a(min_index + floor(sz/2)) > find) { + // bookmark the index as the possible answer and move to search more + ans_index = min_index + floor(sz/2); + + return binary_search_first_greater_element(a, find, min_index, min_index + floor(sz/2) - 1, ans_index); + + + } else { + return binary_search_first_greater_element(a, find, min_index + floor(sz/2), max_index, ans_index); + } + + return ans_index; }