From 24d6637a4fef9c6576b77d7225a878056193658f Mon Sep 17 00:00:00 2001 From: Alec Jacobson Date: Sat, 7 Jul 2018 16:34:02 +0200 Subject: [PATCH 1/4] 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/4] 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 5804b9d9abfdc71accaff0cdbe5b5f27dfd289cd Mon Sep 17 00:00:00 2001 From: Yihuan Mao Date: Thu, 27 Sep 2018 20:43:37 -0400 Subject: [PATCH 3/4] submission --- src/point_mesh_distance.cpp | 32 ++++++++++++++++++++---- src/point_triangle_distance.cpp | 43 ++++++++++++++++++++++++++++++--- src/random_points_on_mesh.cpp | 21 +++++++++++++++- 3 files changed, 87 insertions(+), 9 deletions(-) diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..8b72530 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,4 +1,5 @@ #include "point_mesh_distance.h" +#include "point_triangle_distance.h" void point_mesh_distance( const Eigen::MatrixXd & X, @@ -8,9 +9,30 @@ 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_triangle_distance( const Eigen::RowVector3d & x, @@ -8,7 +9,43 @@ void point_triangle_distance( double & d, Eigen::RowVector3d & p) { - // Replace with your code - d = 0; - p = a; + Eigen::RowVector3d n=b-a,tp=c-a,x0; + n=n.cross(tp).normalized(); + if (n.dot(x-a)<0) n=-n; + x0=x-n.dot(x-a)*n; + Eigen::RowVector3d ab=b-a,bc=c-b,ca=a-c; + ab=ab.normalized(); + bc=bc.normalized(); + ca=ca.normalized(); + Eigen::RowVector3d a2b=a+ab*ab.dot(x0-a),b2c=b+bc*bc.dot(x0-b),c2a=c+ca*ca.dot(x0-c); + double dd; + d=(x-a).norm(); + p=a; + dd=(x-b).norm(); + if (dd void random_points_on_mesh( const int n, @@ -8,6 +9,24 @@ void random_points_on_mesh( { // REPLACE WITH YOUR CODE: X.resize(n,3); - for(int i = 0;i1){ + a=1-a; + b=1-b; + } + X.row(i)=V.row(F(j,0))+a*(V.row(F(j,1))-V.row(F(j,0)))+b*(V.row(F(j,2))-V.row(F(j,0))); + } } From a4bfba0e40fc3749b0ae35d804b58f6bdf22fdc0 Mon Sep 17 00:00:00 2001 From: Yihuan Mao Date: Sun, 7 Oct 2018 00:02:53 -0400 Subject: [PATCH 4/4] Yihuan Mao submit --- main.cpp | 2 +- src/closest_rotation.cpp | 11 ++++- src/hausdorff_lower_bound.cpp | 12 ++++- src/icp_single_iteration.cpp | 22 ++++++++-- src/point_to_plane_rigid_matching.cpp | 49 +++++++++++++++++++-- src/point_to_point_rigid_matching.cpp | 63 +++++++++++++++++++++++++-- src/point_triangle_distance.cpp | 1 - 7 files changed, 144 insertions(+), 16 deletions(-) diff --git a/main.cpp b/main.cpp index 3115cc9..65a5ce7 100644 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,7 @@ int main(int argc, char *argv[]) int num_samples = 100; bool show_samples = true; - ICPMethod method = ICP_METHOD_POINT_TO_POINT; + ICPMethod method = ICP_METHOD_POINT_TO_PLANE; igl::viewer::Viewer viewer; std::cout< +#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 V=svd.matrixV(); + Eigen::Matrix3d omega=Eigen::Matrix3d::Identity(); + R=U*V.transpose(); + if (R.determinant()<0) omega(2,2)=-1; + R=(U*omega*V.transpose()).transpose(); } diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..743c46c 100644 --- a/src/hausdorff_lower_bound.cpp +++ b/src/hausdorff_lower_bound.cpp @@ -1,4 +1,7 @@ #include "hausdorff_lower_bound.h" +#include "random_points_on_mesh.h" +#include "point_mesh_distance.h" +#include double hausdorff_lower_bound( const Eigen::MatrixXd & VX, @@ -7,6 +10,11 @@ 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; + Eigen::MatrixXd 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..dbafbab 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_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, @@ -10,7 +14,19 @@ 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; + Eigen::VectorXd D; + Eigen::MatrixXd P; + Eigen::MatrixXd N; + printf("------1------"); + random_points_on_mesh(num_samples,VX,FX,X); + printf("------2------"); + point_mesh_distance(X,VY,FY,D,P,N); + printf("------3------"); + if (method == ICP_METHOD_POINT_TO_POINT){ + point_to_point_rigid_matching(X,P,R,t); + } + else { + point_to_plane_rigid_matching(X,P,N,R,t); + } } diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index 1978510..5ef4490 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -1,4 +1,7 @@ #include "point_to_plane_rigid_matching.h" +#include +#include "closest_rotation.h" +#include void point_to_plane_rigid_matching( const Eigen::MatrixXd & X, @@ -7,7 +10,47 @@ 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 n=X.rows(); + Eigen::MatrixXd N1=N.col(0).asDiagonal(),N2=N.col(1).asDiagonal(),N3=N.col(2).asDiagonal(),NN; + NN.resize(n,3*n); + NN << N1,N2,N3; + Eigen::MatrixXd X1=X.col(0),X2=X.col(1),X3=X.col(2); + Eigen::MatrixXd P1=P.col(0),P2=P.col(1),P3=P.col(2); + Eigen::MatrixXd one=Eigen::MatrixXd::Ones(n,1); + Eigen::MatrixXd zero=Eigen::MatrixXd::Zero(n,1); + Eigen::MatrixXd row1,row2,row3; + + row1.resize(n,6); + row2.resize(n,6); + row3.resize(n,6); + row1 << zero,X3,-X2,one,zero,zero; + row2 << -X3,zero,X1,zero,one,zero; + row3 << X2,-X1,zero,zero,zero,one; + Eigen::MatrixXd A,u,AA,b; + A.resize(3*n,6); + A << row1, + row2, + row3; + P1=P1-X1; + P2=P2-X2; + P3=P3-X3; + b.resize(3*n,1); + b << P1, + P2, + P3; + b=NN*b; + A=NN*A; + AA=A.transpose()*A; + u=AA.inverse()*A.transpose()*b; + //u = AA.lu().solve(A.transpose()*b); + Eigen::Matrix3d M=Eigen::Matrix3d::Identity(); + double alpha=u(0,0),beta=u(1,0),gamma=u(2,0); + M(0,1)=-gamma; + M(0,2)=beta; + M(1,0)=gamma; + M(1,2)=-alpha; + M(2,0)=-beta; + M(2,1)=alpha; + closest_rotation(M,R); + t = Eigen::RowVector3d(u(3,0),u(4,0),u(5,0)); } diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp index 079151f..9640210 100644 --- a/src/point_to_point_rigid_matching.cpp +++ b/src/point_to_point_rigid_matching.cpp @@ -1,5 +1,8 @@ #include "point_to_point_rigid_matching.h" -#include +#include +#include "closest_rotation.h" +#include +#include void point_to_point_rigid_matching( const Eigen::MatrixXd & X, @@ -7,8 +10,60 @@ 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::VectorXd Xavg=X.colwise().sum()/n; + Eigen::VectorXd Pavg=P.colwise().sum()/n; + Eigen::MatrixXd ones=Eigen::MatrixXd::Ones(n,3),X2,X1=X,P1=P; + printf("%u %u ",(ones*Xavg.asDiagonal()).rows(),(ones*Xavg.asDiagonal()).cols()); + X1=X1-ones*Xavg.asDiagonal(); + P1=P1-ones*Pavg.asDiagonal(); + //X2=X1.transpose(); + printf("aaa"); + Eigen::MatrixXd W=Eigen::MatrixXd::Zero(3,3); + W=P1.transpose()*X1; + printf("bbb"); + Eigen::JacobiSVD svd(W, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::Matrix3d U=svd.matrixU(); + Eigen::Matrix3d V=svd.matrixV(); + R=U*V.transpose(); + closest_rotation(W,R); + printf("ccc"); + t=-Xavg+R*Pavg;*/ + int n=X.rows(); + Eigen::MatrixXd X1=X.col(0),X2=X.col(1),X3=X.col(2); + Eigen::MatrixXd P1=P.col(0),P2=P.col(1),P3=P.col(2); + Eigen::MatrixXd one=Eigen::MatrixXd::Ones(n,1); + Eigen::MatrixXd zero=Eigen::MatrixXd::Zero(n,1); + Eigen::MatrixXd row1,row2,row3; + row1.resize(n,6); + row2.resize(n,6); + row3.resize(n,6); + row1 << zero,X3,-X2,one,zero,zero; + row2 << -X3,zero,X1,zero,one,zero; + row3 << X2,-X1,zero,zero,zero,one; + Eigen::MatrixXd A,u,AA,b; + A.resize(3*n,6); + A << row1, + row2, + row3; + P1=P1-X1; + P2=P2-X2; + P3=P3-X3; + b.resize(3*n,1); + b << P1, + P2, + P3; + AA=A.transpose()*A; + u=AA.inverse()*A.transpose()*b; + Eigen::Matrix3d M=Eigen::Matrix3d::Identity(); + double alpha=u(0,0),beta=u(1,0),gamma=u(2,0); + M(0,1)=-gamma; + M(0,2)=beta; + M(1,0)=gamma; + M(1,2)=-alpha; + M(2,0)=-beta; + M(2,1)=alpha; + closest_rotation(M,R); + t = Eigen::RowVector3d(u(3,0),u(4,0),u(5,0)); } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 209e238..72a992c 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -46,6 +46,5 @@ void point_triangle_distance( d=dd; p=c2a; } - d=0; }