From 24d6637a4fef9c6576b77d7225a878056193658f Mon Sep 17 00:00:00 2001 From: Alec Jacobson Date: Sat, 7 Jul 2018 16:34:02 +0200 Subject: [PATCH 1/8] 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/8] 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 20e92a425493324cff01a2910564971070ea22bd Mon Sep 17 00:00:00 2001 From: Randy Hickey Date: Mon, 8 Oct 2018 23:45:04 -0400 Subject: [PATCH 3/8] done --- src/closest_rotation.cpp | 10 ++++- src/hausdorff_lower_bound.cpp | 10 ++++- src/icp_single_iteration.cpp | 17 ++++++-- src/point_mesh_distance.cpp | 29 +++++++++++--- src/point_to_plane_rigid_matching.cpp | 21 ++++++++-- src/point_to_point_rigid_matching.cpp | 11 +++-- src/point_triangle_distance.cpp | 58 ++++++++++++++++++++++----- src/random_points_on_mesh.cpp | 27 +++++++++++-- 8 files changed, 152 insertions(+), 31 deletions(-) diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index 54403c1..1cb5488 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -1,9 +1,15 @@ #include "closest_rotation.h" +#include +#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 U = svd.matrixU(); + Eigen::Matrix3d Vt = (svd.matrixV()).transpose(); + Eigen::Matrix3d O = Eigen::Matrix3d::Identity(); + O(2,2) = (U * Vt).determinant(); + R = U * O * Vt; } diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..25bcf16 100644 --- a/src/hausdorff_lower_bound.cpp +++ b/src/hausdorff_lower_bound.cpp @@ -1,4 +1,6 @@ #include "hausdorff_lower_bound.h" +#include "point_mesh_distance.h" +#include "random_points_on_mesh.h" double hausdorff_lower_bound( const Eigen::MatrixXd & VX, @@ -7,6 +9,10 @@ double hausdorff_lower_bound( const Eigen::MatrixXi & FY, const int n) { - // Replace with your code - return 0; + Eigen::MatrixXd X; + Eigen::VectorXd D; + Eigen::MatrixXd P, N; + random_points_on_mesh(n, VX, FX, X); + point_mesh_distance(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..7681f88 100644 --- a/src/icp_single_iteration.cpp +++ b/src/icp_single_iteration.cpp @@ -1,4 +1,8 @@ #include "icp_single_iteration.h" +#include "random_points_on_mesh.h" +#include "point_to_plane_rigid_matching.h" +#include "point_to_point_rigid_matching.h" +#include "point_mesh_distance.h" void icp_single_iteration( const Eigen::MatrixXd & VX, @@ -10,7 +14,14 @@ void icp_single_iteration( Eigen::Matrix3d & R, Eigen::RowVector3d & t) { - // Replace with your code - R = Eigen::Matrix3d::Identity(); - t = Eigen::RowVector3d::Zero(); + Eigen::MatrixXd X, P, N; + Eigen::VectorXd D; + random_points_on_mesh(num_samples,VX,FX,X); + 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 (ICP_METHOD_POINT_TO_POINT) { + point_to_plane_rigid_matching(X,P,N,R,t); + } } diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..3112062 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, @@ -8,9 +10,26 @@ 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 void point_to_plane_rigid_matching( const Eigen::MatrixXd & X, @@ -7,7 +9,20 @@ 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 r = X.rows(); + Eigen::MatrixXd A; + A << Eigen::VectorXd::Zero(r), X.col(2), -X.col(1), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), -X.col(2), Eigen::VectorXd::Zero(r), X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), X.col(1), -X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r); + Eigen::VectorXd B; + B << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2); + Eigen::MatrixXd Nd; + Nd.block(0, 0, r, r) = N.col(0).asDiagonal(); + Nd.block(0, r, r, r) = N.col(1).asDiagonal(); + Nd.block(0, 2 * r, r, r) = N.col(2).asDiagonal(); + A = Nd * A; + B = Nd * B; + Eigen::VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * B); + Eigen::Matrix3d M; + M << 1, u(2), -u(1), -u(2), 1, u(0), u(1), -u(0), 1; + closest_rotation(M,R); + t = Eigen::Vector3d(u(3),u(4),u(5)); } diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp index 079151f..f4d7581 100644 --- a/src/point_to_point_rigid_matching.cpp +++ b/src/point_to_point_rigid_matching.cpp @@ -1,5 +1,6 @@ #include "point_to_point_rigid_matching.h" #include +#include "closest_rotation.h" void point_to_point_rigid_matching( const Eigen::MatrixXd & X, @@ -7,8 +8,10 @@ void point_to_point_rigid_matching( Eigen::Matrix3d & R, Eigen::RowVector3d & t) { - // Replace with your code - R = Eigen::Matrix3d::Identity(); - t = Eigen::RowVector3d::Zero(); + Eigen::MatrixXd xbar, pbar; + xbar = X.rowwise() - X.colwise().mean(); + pbar = P.rowwise() - P.colwise().mean(); + Eigen::Matrix3d M = (pbar.transpose() * xbar).transpose(); + closest_rotation(M, R); + t = (P.colwise().mean()).transpose() - R*((X.colwise().mean()).transpose()); } - diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 6405100..c3e791f 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -1,14 +1,54 @@ #include "point_triangle_distance.h" +#include +#include +#include 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; + Eigen::Hyperplane plane = Eigen::Hyperplane::Through(a, b, c); + Eigen::RowVector3d proj = plane.projection(x); + if (((b-a).cross(c-a)).dot((b-a).cross(x-a)) >= 0 && ((a-c).cross(b-c)).dot((a-c).cross(x-c)) >= 0 && ((c-b).cross(a-b)).dot((c-b).cross(x-b)) >= 0) { + d = (x - proj).norm(); + p = proj; + } + else { + d = 100000000; + double z = (x - a).dot(b - a) / (b - a).squaredNorm(); + if (z < 0) z = 0; + else if (z > 1) z = 1; + Eigen::RowVector3d projection = a + z * (b - a); + Eigen::RowVector3d abP = projection; + double abD = (x - projection).norm(); + if (abD < d) { + d = abD; + p = abP; + } + z = (x - b).dot(c - b) / (c - b).squaredNorm(); + if (z < 0) z = 0; + else if (z > 1) z = 1; + projection = b + z * (c - b); + Eigen::RowVector3d bcP = projection; + double bcD = (x - projection).norm(); + if (bcD < d) { + d = bcD; + p = bcP; + } + z = (x - c).dot(a - c) / (a - c).squaredNorm(); + if (z < 0) z = 0; + else if (z > 1) z = 1; + projection = c + z * (a - c); + Eigen::RowVector3d caP = projection; + double caD = (x - projection).norm(); + if (caD < d) { + d = caD; + p = caP; + } + } + } diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 6e85d75..bee09dd 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -1,4 +1,7 @@ #include "random_points_on_mesh.h" +#include +#include +#include void random_points_on_mesh( const int n, @@ -6,8 +9,26 @@ 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 distribution(0.0, 1.0); + for(int i = 0;i 1){ + alpha = 1 - beta; + beta = 1 - alpha; + } + X.row(i) = alpha * (v[1] - v[0]) + beta * (v[2] - v[0]) + v[0]; + } } From 222783a4e1c5d926b1f6ab6460b08b4cf40bb937 Mon Sep 17 00:00:00 2001 From: rgh000 <35755112+rgh000@users.noreply.github.com> Date: Mon, 8 Oct 2018 23:54:45 -0400 Subject: [PATCH 4/8] Update point_triangle_distance.cpp --- src/point_triangle_distance.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index c3e791f..b1dea7b 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -4,12 +4,12 @@ #include 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) { Eigen::Hyperplane plane = Eigen::Hyperplane::Through(a, b, c); Eigen::RowVector3d proj = plane.projection(x); From 2e5448e049d58bf8436864148e2b210cfeb08d59 Mon Sep 17 00:00:00 2001 From: rgh000 <35755112+rgh000@users.noreply.github.com> Date: Mon, 8 Oct 2018 23:57:25 -0400 Subject: [PATCH 5/8] Update random_points_on_mesh.cpp --- src/random_points_on_mesh.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index bee09dd..c4523e9 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -15,16 +15,16 @@ void random_points_on_mesh( igl::cumsum(area * 0.5, 1, cumarea); std::random_device gen; std::uniform_real_distribution<> distribution(0.0, 1.0); - for(int i = 0;i 1){ + if(alpha + beta > 1) { alpha = 1 - beta; beta = 1 - alpha; } From 5c90e58b038ee3dd124613a13d35503ec1a2a4af Mon Sep 17 00:00:00 2001 From: rgh000 <35755112+rgh000@users.noreply.github.com> Date: Mon, 8 Oct 2018 23:58:41 -0400 Subject: [PATCH 6/8] Update point_to_plane_rigid_matching.cpp --- src/point_to_plane_rigid_matching.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index 6a48fff..4606a76 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -24,5 +24,5 @@ void point_to_plane_rigid_matching( Eigen::Matrix3d M; M << 1, u(2), -u(1), -u(2), 1, u(0), u(1), -u(0), 1; closest_rotation(M,R); - t = Eigen::Vector3d(u(3),u(4),u(5)); + t = Eigen::Vector3d(u(3), u(4), u(5)); } From 4c2066eb3ec173e9163c529efbe52615e5240ac7 Mon Sep 17 00:00:00 2001 From: rgh000 <35755112+rgh000@users.noreply.github.com> Date: Tue, 9 Oct 2018 00:16:53 -0400 Subject: [PATCH 7/8] fixed typo in code. please consider new version --- src/icp_single_iteration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp index 7681f88..f34aa36 100644 --- a/src/icp_single_iteration.cpp +++ b/src/icp_single_iteration.cpp @@ -21,7 +21,7 @@ void icp_single_iteration( if (method == ICP_METHOD_POINT_TO_POINT) { point_to_point_rigid_matching(X,P,R,t); } - else if (ICP_METHOD_POINT_TO_POINT) { + else { point_to_plane_rigid_matching(X,P,N,R,t); } } From b20fa23e96fec7ae41c3b4522db69137d9d986fe Mon Sep 17 00:00:00 2001 From: rgh000 <35755112+rgh000@users.noreply.github.com> Date: Tue, 9 Oct 2018 00:53:28 -0400 Subject: [PATCH 8/8] fixed memory error. please consider new version --- src/point_to_plane_rigid_matching.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index 4606a76..d4fb828 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -10,18 +10,18 @@ void point_to_plane_rigid_matching( Eigen::RowVector3d & t) { int r = X.rows(); - Eigen::MatrixXd A; + Eigen::MatrixXd A(r * 3, 6); A << Eigen::VectorXd::Zero(r), X.col(2), -X.col(1), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), -X.col(2), Eigen::VectorXd::Zero(r), X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r), Eigen::VectorXd::Zero(r), X.col(1), -X.col(0), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Zero(r), Eigen::VectorXd::Ones(r); - Eigen::VectorXd B; + Eigen::VectorXd B(r * 3); B << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2); - Eigen::MatrixXd Nd; - Nd.block(0, 0, r, r) = N.col(0).asDiagonal(); - Nd.block(0, r, r, r) = N.col(1).asDiagonal(); - Nd.block(0, 2 * r, r, r) = N.col(2).asDiagonal(); + Eigen::MatrixXd Nd(r, 3 * r); + Eigen::MatrixXd Nd0[] = {N.col(0).asDiagonal(), N.col(1).asDiagonal(), N.col(2).asDiagonal()}; + Nd << Nd0[0], Nd0[1], Nd0[2]; A = Nd * A; B = Nd * B; - Eigen::VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * B); - Eigen::Matrix3d M; + Eigen::VectorXd u; + u = (A.transpose() * A).inverse() * (-A.transpose() * B); + Eigen::Matrix3d M(3, 3); M << 1, u(2), -u(1), -u(2), 1, u(0), u(1), -u(0), 1; closest_rotation(M,R); t = Eigen::Vector3d(u(3), u(4), u(5));