From 24d6637a4fef9c6576b77d7225a878056193658f Mon Sep 17 00:00:00 2001 From: Alec Jacobson Date: Sat, 7 Jul 2018 16:34:02 +0200 Subject: [PATCH 1/3] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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$: From ac4489887a1401700525c58f5f8902af5c5c0313 Mon Sep 17 00:00:00 2001 From: Alec Jacobson Date: Sat, 7 Jul 2018 16:34:34 +0200 Subject: [PATCH 2/3] Update README.html --- README.html | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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\):

From e7a3e134a54a7e51f1c69fe241bee6c9296a087d Mon Sep 17 00:00:00 2001 From: Wenzheng Chen Date: Sun, 7 Oct 2018 18:39:06 -0400 Subject: [PATCH 3/3] hw2 --- main.cpp | 240 +++++++++++++------------- src/closest_rotation.cpp | 21 ++- src/hausdorff_lower_bound.cpp | 30 +++- src/icp_single_iteration.cpp | 40 +++-- src/point_mesh_distance.cpp | 54 ++++-- src/point_to_plane_rigid_matching.cpp | 63 +++++-- src/point_to_point_rigid_matching.cpp | 36 +++- src/point_triangle_distance.cpp | 117 +++++++++++-- src/random_points_on_mesh.cpp | 60 ++++++- 9 files changed, 461 insertions(+), 200 deletions(-) diff --git a/main.cpp b/main.cpp index 3115cc9..8498097 100644 --- a/main.cpp +++ b/main.cpp @@ -3,27 +3,27 @@ #include "random_points_on_mesh.h" #include "point_mesh_distance.h" #include -#include +#include #include #include #include -int main(int argc, char *argv[]) -{ - // Load input meshes - Eigen::MatrixXd OVX,VX,VY; - Eigen::MatrixXi FX,FY; - igl::read_triangle_mesh( - (argc>1?argv[1]:"../shared/data/max-registration-partial.obj"),OVX,FX); - igl::read_triangle_mesh( - (argc>2?argv[2]:"../shared/data/max-registration-complete.obj"),VY,FY); +int main(int argc, char *argv[]) { + // Load input meshes + Eigen::MatrixXd OVX, VX, VY; + Eigen::MatrixXi FX, FY; + igl::read_triangle_mesh( + (argc > 1 ? argv[1] : "./max-registration-partial.obj"), OVX, FX); + igl::read_triangle_mesh( + (argc > 2 ? argv[2] : "./max-registration-complete.obj"), VY, FY); - int num_samples = 100; - bool show_samples = true; - ICPMethod method = ICP_METHOD_POINT_TO_POINT; + int num_samples = 100; + bool show_samples = true; + ICPMethod method = ICP_METHOD_POINT_TO_POINT; - igl::viewer::Viewer viewer; - std::cout<bool - { - if(viewer.core.is_animating) - { - //////////////////////////////////////////////////////////////////////// - // Perform single iteration of ICP method - //////////////////////////////////////////////////////////////////////// - Eigen::Matrix3d R; - Eigen::RowVector3d t; - icp_single_iteration(VX,FX,VY,FY,num_samples,method,R,t); - // Apply transformation to source mesh - VX = ((VX*R).rowwise() + t).eval(); - set_meshes(); - if(show_samples) - { - set_points(); - } - } - return false; - }; - viewer.callback_key_pressed = - [&](igl::viewer::Viewer &,unsigned char key,int)->bool - { - switch(key) - { - case ' ': - viewer.core.is_animating ^= 1; - break; - case 'H': - case 'h': - std::cout<<"D_{H}(X -> Y) >= "<< - hausdorff_lower_bound(VX,FX,VY,FY,num_samples)<bool + { + if(viewer.core.is_animating) + { + //////////////////////////////////////////////////////////////////////// + // Perform single iteration of ICP method + //////////////////////////////////////////////////////////////////////// + Eigen::Matrix3d R; + Eigen::RowVector3d t; + icp_single_iteration(VX,FX,VY,FY,num_samples,method,R,t); + // Apply transformation to source mesh + VX = ((R*VX.transpose()).transpose().rowwise() + t);//.eval(); + set_meshes(); + if(show_samples) + { + set_points(); + } + } + return false; + }; + viewer.callback_key_pressed = + [&](igl::opengl::glfw::Viewer &,unsigned char key,int)->bool + { + switch(key) + { + case ' ': + viewer.core.is_animating ^= 1; + break; + case 'H': + case 'h': + std::cout<<"D_{H}(X -> Y) >= "<< + hausdorff_lower_bound(VX,FX,VY,FY,num_samples)< +#include + +void closest_rotation(const Eigen::Matrix3d & M, Eigen::Matrix3d & R) { + // Replace with your code + R = Eigen::Matrix3d::Identity(); + + Eigen::JacobiSVD svd(M, + Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::Matrix3d V = svd.matrixV(), U = svd.matrixU(); + // Matrix3d S = U.inverse() * M * V.transpose().inverse(); // S = U^-1 * A * VT * -1 + + R(2, 2) = (V * U.transpose()).determinant(); + R = V * R * U.transpose(); } + diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..b85af50 100644 --- a/src/hausdorff_lower_bound.cpp +++ b/src/hausdorff_lower_bound.cpp @@ -1,12 +1,24 @@ #include "hausdorff_lower_bound.h" -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; +#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) { + // Replace with your code + Eigen::MatrixXd Xsam; + random_points_on_mesh(n, VX, FX, Xsam); + + Eigen::VectorXd D; + Eigen::MatrixXd P, N; + point_mesh_distance(Xsam, VY, FY, D, P, N); + + double nre = 0; + for (int i = 0; i < n; i++) { + if (nre < D[i]) + nre = D[i]; + } + + return nre; } diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp index 1e8fda9..24cca87 100644 --- a/src/icp_single_iteration.cpp +++ b/src/icp_single_iteration.cpp @@ -1,16 +1,30 @@ #include "icp_single_iteration.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) -{ - // Replace with your code - R = Eigen::Matrix3d::Identity(); - t = Eigen::RowVector3d::Zero(); +#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) { + // Replace with your code + // R = Eigen::Matrix3d::Identity(); + // t = Eigen::RowVector3d::Zero(); + + // first sample + Eigen::MatrixXd Xsam; + random_points_on_mesh(num_samples, VX, FX, Xsam); + + // close correspondence + Eigen::VectorXd D; + Eigen::MatrixXd P, N; + point_mesh_distance(Xsam, VY, FY, D, P, N); + + // match + // point_to_point_rigid_matching(Xsam, P, R, t); + point_to_plane_rigid_matching(Xsam, P, N, R, t); } + diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..5ecd252 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,16 +1,44 @@ #include "point_mesh_distance.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) -{ - // Replace with your code - P.resizeLike(X); - N = Eigen::MatrixXd::Zero(X.rows(),X.cols()); - for(int i = 0;i -void point_to_plane_rigid_matching( - 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(); +#include "closest_rotation.h" + +void point_to_plane_rigid_matching(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(); + + int row = X.rows(); + Eigen::VectorXd Z0 = Eigen::VectorXd::Zero(row); + Eigen::VectorXd Z1 = Eigen::VectorXd::Ones(row); + + Eigen::VectorXd X0 = X.col(0); + Eigen::VectorXd X1 = X.col(1); + Eigen::VectorXd X2 = X.col(2); + Eigen::MatrixXd A(3 * row, 6); + A << Z0, X2, -X1, Z1, Z0, Z0, -X2, Z0, X0, Z0, Z1, Z0, X1, -X0, Z0, Z0, Z0, Z1; + + Eigen::MatrixXd b(3 * row, 1); + b << P.col(0) - X0, P.col(1) - X1, P.col(2) - X2; + + /* faster + Eigen::MatrixXd diag(row, 3 * row); + diag << Eigen::MatrixXd(N.col(0).asDiagonal()), Eigen::MatrixXd( + N.col(1).asDiagonal()), Eigen::MatrixXd(N.col(2).asDiagonal()); + + A = diag * A; + b = diag * b; + */ + + Eigen::MatrixXd ATA = A.transpose() * A; + Eigen::MatrixXd ATb = A.transpose() * b; + + Eigen::VectorXd u = ATA.inverse() * ATb; + Eigen::Matrix3d M = Eigen::Matrix3d::Identity(); + double alpha = u[0]; + double beta = u[1]; + double gamma = u[2]; + + M(1, 0) += -gamma; + M(2, 0) += beta; + M(0, 1) += gamma; + M(2, 1) += -alpha; + M(0, 2) += -beta; + M(1, 2) += alpha; + + closest_rotation(M, R); + + t[0] = u[3]; + t[1] = u[4]; + t[2] = u[5]; } diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp index 079151f..f2b2b13 100644 --- a/src/point_to_point_rigid_matching.cpp +++ b/src/point_to_point_rigid_matching.cpp @@ -1,14 +1,32 @@ #include "point_to_point_rigid_matching.h" #include -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(); +#include "closest_rotation.h" + +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(); + + // R * PL = PR + std::cout << X.colwise().mean() << std::endl; + Eigen::RowVector3d PLavg = X.colwise().mean(); + Eigen::RowVector3d PRavg = P.colwise().mean(); + + // subtranct mean + Eigen::MatrixXd PL, PR; + PL.resizeLike(X); + PR.resizeLike(X); + for (int i = 0; i < X.rows(); i++) { + PL.row(i) = X.row(i) - PLavg; + PR.row(i) = P.row(i) - PRavg; + } + + Eigen::Matrix3d tmp = PL.transpose() * PR; + closest_rotation(tmp, R); + + t = (PRavg.transpose() - R * PLavg.transpose()).transpose(); } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 6405100..8f828aa 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -1,14 +1,109 @@ #include "point_triangle_distance.h" -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) -{ - // Replace with your code - d = 0; - p = a; +double clipdouble(double a, double amin, double amax) { + if (a > amax) + return amax; + + if (a < amin) + return amin; + + return a; } + +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) { + // Replace with your code + + // fast + Eigen::RowVector3d ax = x - a; + Eigen::RowVector3d bx = x - b; + Eigen::RowVector3d cx = x - c; + + double dax = ax.norm(); + double dbx = bx.norm(); + double dcx = cx.norm(); + + if (dax < dbx) { + if (dax < dcx) { + d = dax; + p = a; + } else { + d = dcx; + p = c; + } + } else { + if (dbx < dcx) { + d = dbx; + p = b; + } else { + d = dcx; + p = c; + } + } + + return; + + // calculate a point d in the plane of abc + // s.t xd is minimal + // the same as solve equation + Eigen::MatrixXd A(3, 2); + A.col(0) = (b - a).transpose(); + A.col(1) = (c - a).transpose(); + + Eigen::MatrixXd B(3, 1); + B.col(0) = (x - a).transpose(); + + Eigen::MatrixXd ATA = A.transpose() * A; + Eigen::MatrixXd ATB = A.transpose() * B; + Eigen::MatrixXd ATAINV; + ATAINV.resizeLike(ATA); + ATAINV(0, 0) = ATA(1, 1); + ATAINV(1, 0) = -ATA(1, 0); + ATAINV(0, 1) = -ATA(0, 1); + ATAINV(1, 1) = ATA(0, 0); + + double det = ATA(0, 0) * ATA(1, 1) - ATA(0, 1) * ATA(1, 0); + if (det != 0) { + ATAINV /= det; + Eigen::MatrixXd X = ATAINV * ATB; + + double a1 = X(0, 0); + double a2 = X(1, 0); + double a3 = 1 - a1 - a2; + + a1 = clipdouble(a1, 0, 1); + a2 = clipdouble(a2, 0, 1); + a3 = clipdouble(a3, 0, 1); + + p = a1 * a + a2 * b + a3 * c; + } else { + + Eigen::RowVector3d ax = x - a; + Eigen::RowVector3d bx = x - b; + Eigen::RowVector3d cx = x - c; + + double dax = ax.norm(); + double dbx = bx.norm(); + double dcx = cx.norm(); + + if (dax < dbx) { + if (dax < dcx) { + d = dax; + p = a; + } else { + d = dcx; + p = c; + } + } else { + if (dbx < dcx) { + d = dbx; + p = b; + } else { + d = dcx; + p = c; + } + } + } +} + diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 6e85d75..affd7fb 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -1,13 +1,55 @@ #include "random_points_on_mesh.h" -void random_points_on_mesh( - 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(rand()) / static_cast(RAND_MAX); + return r; +} + +void random_points_on_mesh(const int n, const Eigen::MatrixXd & V, + const Eigen::MatrixXi & F, Eigen::MatrixXd & X) { + // REPLACE WITH YOUR CODE: + X.resize(n, 3); + + // faster + int vnum = V.rows(); + for (int i = 0; i < n; i++) { + // find where it is + int idx = int(randomfloat() * vnum); + X.row(i) = V.row(idx); + } + return; + + // first, calculate the area of each triangle + Eigen::MatrixXd areas; + igl::doublearea(V, F, areas); + + // second, calculate the cumsum of the points + Eigen::MatrixXd consumareas; + igl::cumsum(areas, 1, consumareas); + + // for each random + int fnum = F.rows(); + for (int i = 0; i < n; i++) { + float tmp = randomfloat() * consumareas(fnum - 1, 0); + + // find where it is + int idx = 0; + for (int j = 0; j < fnum; j++) { + if (tmp <= consumareas(j, 0)) { + idx = j; + break; + } + } + + double a1 = randomfloat(); + double a2 = (1 - a1) * randomfloat(); + double a3 = 1 - a1 - a2; + + X.row(i) = a1 * V.row(F(idx, 0)) + a2 * V.row(F(idx, 1)) + + a3 * V.row(F(idx, 2)); + } }