From 1bfdbe0bc5c829247df90cb0c71e9cadeca1cc3e Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Mon, 5 Feb 2018 16:42:45 -0500 Subject: [PATCH 01/10] finished src/random_points_on_mesh.cpp --- include/random_points_on_mesh.h | 5 +++++ src/random_points_on_mesh.cpp | 36 ++++++++++++++++++++++++++++++++- 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/include/random_points_on_mesh.h b/include/random_points_on_mesh.h index 4fe2497..21e8d60 100644 --- a/include/random_points_on_mesh.h +++ b/include/random_points_on_mesh.h @@ -1,6 +1,11 @@ #ifndef RANDOM_POINTS_ON_MESH_H #define RANDOM_POINTS_ON_MESH_H #include +#include +#include +#include +#include + // RANDOM_POINTS_ON_MESH Randomly sample a mesh (V,F) n times. // // Inputs: diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 6e85d75..5e4ec99 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -8,6 +8,40 @@ void random_points_on_mesh( { // REPLACE WITH YOUR CODE: X.resize(n,3); - for(int i = 0;i randx) { + if (randx > cumsumA(idx - 1)) { + idx += gap; + } + else { + idx -= gap; + } + gap /= 2; + } + + double alpha = ((double)rand()/(double)RAND_MAX); + double beta = ((double)rand()/(double)RAND_MAX); + + if (alpha + beta > 1) { + alpha = 1 - alpha; + beta = 1 - beta; + } + + X.row(i) = V.row(F(idx, 0)) + + alpha * V.row(F(idx, 1)) + + beta * V.row(F(idx, 2)); + } } From 30b4589886be235ee8a4a4e55a43c276695c11cc Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Mon, 5 Feb 2018 20:36:06 -0500 Subject: [PATCH 02/10] finished distance functions --- include/point_mesh_distance.h | 2 ++ src/point_mesh_distance.cpp | 20 +++++++++++++--- src/point_triangle_distance.cpp | 41 ++++++++++++++++++++++++++++++--- 3 files changed, 57 insertions(+), 6 deletions(-) diff --git a/include/point_mesh_distance.h b/include/point_mesh_distance.h index f542c54..2c14999 100644 --- a/include/point_mesh_distance.h +++ b/include/point_mesh_distance.h @@ -1,6 +1,8 @@ #ifndef POINT_MESH_DISTANCE_H #define POINT_MESH_DISTANCE_H #include +#include +#include "point_triangle_distance.h" // Compute the distances `D` between a set of given points `X` and their // closest points `P` on a given mesh with vertex positions `VY` and face // indices `FY`. diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 2e6a070..4256b16 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,5 +1,5 @@ #include "point_mesh_distance.h" - +#include void point_mesh_distance( const Eigen::MatrixXd & X, const Eigen::MatrixXd & VY, @@ -11,6 +11,20 @@ void point_mesh_distance( // Replace with your code P.resizeLike(X); N = Eigen::MatrixXd::Zero(X.rows(),X.cols()); - for(int i = 0;i::max(); + for(int j = 0;j d) { + D(i) = d; + P.row(i) = p; + N.row(i) = pfn.row(j); + } + } + } } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 6405100..faeec1b 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -1,4 +1,5 @@ #include "point_triangle_distance.h" +#include void point_triangle_distance( const Eigen::RowVector3d & x, @@ -8,7 +9,41 @@ void point_triangle_distance( double & d, Eigen::RowVector3d & p) { - // Replace with your code - d = 0; - p = a; + // following algorithm from below link + // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf + Eigen::RowVector3d ba = b-a; + Eigen::RowVector3d cb = c-b; + Eigen::RowVector3d ax = a-x; + Eigen::RowVector3d n = ba.cross(cb); + double area = n.norm(); + double angle = ax.dot(n) / n.norm(); + Eigen::RowVector3d projectx = x - angle * n / n.norm(); + + Eigen::RowVector3d pb = projectx - b; + Eigen::RowVector3d pc = projectx - c; + Eigen::RowVector3d pa = projectx - a; + double alpha = (pb.cross(pc)).norm() / area; + double beta = (pc.cross(pa)).norm() / area; + double gamma = 1 - alpha - beta; + + if(0 <= alpha && alpha <= 1 && + 0 <= beta && beta <= 1 && + 0 <= gamma && gamma <= 1) { + p = projectx; + d = (x - p).norm(); + return ; + } + d = std::numeric_limits::max(); + if (d > (x - a).norm()) { + d = (x - a).norm(); + p = a; + } + if (d > (x - b).norm()) { + d = (x - b).norm(); + p = b; + } + if (d > (x - c).norm()) { + d = (x - c).norm(); + p = c; + } } From 581d3e4e84b02cbf4a4ee3b826965bba2ea7547b Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Mon, 5 Feb 2018 21:00:33 -0500 Subject: [PATCH 03/10] finished src/hausdorff_lower_bound.cpp --- src/hausdorff_lower_bound.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp index 8608964..c532994 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,11 @@ double hausdorff_lower_bound( const int n) { // Replace with your code - return 0; + Eigen::MatrixXd sample; + random_points_on_mesh(n, VX, FX, sample); + Eigen::VectorXd D; + Eigen::MatrixXd P; + Eigen::MatrixXd N; + point_mesh_distance(sample, VY, FY, D, P, N); + return D.maxCoeff(); } From 5b09f418f984954ef655430b92050533e16dabe8 Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Mon, 5 Feb 2018 21:24:13 -0500 Subject: [PATCH 04/10] add closest_rotation with template bug --- src/closest_rotation.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index 54403c1..c9041ef 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -6,4 +6,10 @@ void closest_rotation( { // Replace with your code R = Eigen::Matrix3d::Identity(); + Eigen::JacobiSVD svd(M, ComputeThinU | ComputeThinV); + Eigen::Matrix3d omega; + omega << 1, 0, 0, + 0, 1, 0, + 0, 0, (svd.matrixU() * svd.matrixV().transpose()).determinant(); + R = svd.matrixU() * omega * svd.matrixV().transpose(); } From 7d835b975b22f898145f0719b983591462503ad9 Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Mon, 5 Feb 2018 22:08:32 -0500 Subject: [PATCH 05/10] added p2po p2pl --- src/closest_rotation.cpp | 4 ++- src/point_to_plane_rigid_matching.cpp | 38 +++++++++++++++++++++++++-- src/point_to_point_rigid_matching.cpp | 27 +++++++++++++++++++ 3 files changed, 66 insertions(+), 3 deletions(-) diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index c9041ef..1a31b19 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -1,4 +1,6 @@ #include "closest_rotation.h" +#include +#include void closest_rotation( const Eigen::Matrix3d & M, @@ -6,7 +8,7 @@ void closest_rotation( { // Replace with your code R = Eigen::Matrix3d::Identity(); - Eigen::JacobiSVD svd(M, ComputeThinU | ComputeThinV); + Eigen::JacobiSVD svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV); Eigen::Matrix3d omega; omega << 1, 0, 0, 0, 1, 0, diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index 1978510..e2dc5b0 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -1,4 +1,6 @@ #include "point_to_plane_rigid_matching.h" +#include "closest_rotation.h" +#include void point_to_plane_rigid_matching( const Eigen::MatrixXd & X, @@ -8,6 +10,38 @@ void point_to_plane_rigid_matching( Eigen::RowVector3d & t) { // Replace with your code - R = Eigen::Matrix3d::Identity(); - t = Eigen::RowVector3d::Zero(); + Eigen::Matrix3d dig = Eigen::Matrix3d::Zero(X.rows() * 3, 3); + dig.block(0,0,X.rows(),X.rows()) = N.col(0).asDiagonal(); + dig.block(0,X.rows(),X.rows(),X.rows()) = N.col(1).asDiagonal(); + dig.block(0,X.rows()*2,X.rows(),X.rows()) = N.col(2).asDiagonal(); + + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6); + for (int i = 0; i +#include "closest_rotation.h" +#include void point_to_point_rigid_matching( const Eigen::MatrixXd & X, @@ -10,5 +12,30 @@ void point_to_point_rigid_matching( // Replace with your code R = Eigen::Matrix3d::Identity(); t = Eigen::RowVector3d::Zero(); + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6); + Eigen::VectorXd B = Eigen::VectorXd::Zero(X.rows() * 3); + Eigen::MatrixXd XP = X-P; + for (int i = 0; i Date: Tue, 6 Feb 2018 00:07:57 -0500 Subject: [PATCH 06/10] backup --- include/icp_single_iteration.h | 1 - include/point_triangle_distance.h | 7 +++++ main.cpp | 3 +- src/icp_single_iteration.cpp | 20 +++++++++++++ src/point_mesh_distance.cpp | 12 ++++++-- src/point_to_plane_rigid_matching.cpp | 12 +++++++- src/point_triangle_distance.cpp | 42 +++++++++++++++++++++------ 7 files changed, 82 insertions(+), 15 deletions(-) diff --git a/include/icp_single_iteration.h b/include/icp_single_iteration.h index 03e4040..4ab74ba 100644 --- a/include/icp_single_iteration.h +++ b/include/icp_single_iteration.h @@ -37,5 +37,4 @@ void icp_single_iteration( const ICPMethod method, Eigen::Matrix3d & R, Eigen::RowVector3d & t); - #endif diff --git a/include/point_triangle_distance.h b/include/point_triangle_distance.h index ecdd14e..27a1400 100644 --- a/include/point_triangle_distance.h +++ b/include/point_triangle_distance.h @@ -19,4 +19,11 @@ void point_triangle_distance( const Eigen::RowVector3d & c, double & d, Eigen::RowVector3d & p); + +void dist_helper(const Eigen::RowVector3d & x, + const Eigen::RowVector3d & px, + const Eigen::RowVector3d & a, + const Eigen::RowVector3d & b, + double & d, + Eigen::RowVector3d & p); #endif diff --git a/main.cpp b/main.cpp index 3115cc9..b65c7f6 100644 --- a/main.cpp +++ b/main.cpp @@ -20,7 +20,8 @@ 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_POINT; + ICPMethod method = ICP_METHOD_POINT_TO_PLANE; igl::viewer::Viewer viewer; std::cout< void icp_single_iteration( const Eigen::MatrixXd & VX, @@ -11,6 +16,21 @@ void icp_single_iteration( Eigen::RowVector3d & t) { // Replace with your code + std::cout << "test1" << std::endl; R = Eigen::Matrix3d::Identity(); t = Eigen::RowVector3d::Zero(); + Eigen::MatrixXd X; + random_points_on_mesh(num_samples, VX, FX, X); + std::cout << "test2" << std::endl; + Eigen::VectorXd D; + Eigen::MatrixXd P; + Eigen::MatrixXd N; + point_mesh_distance(X, VY, FY, D, P, N); + + std::cout << "test3" << std::endl; + switch(method) + { + case ICP_METHOD_POINT_TO_POINT: point_to_point_rigid_matching(X, P, R, t); break; + case ICP_METHOD_POINT_TO_PLANE: point_to_plane_rigid_matching(X, P, N, R, t); break; + } } diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 4256b16..20130f0 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -1,5 +1,6 @@ #include "point_mesh_distance.h" #include +#include void point_mesh_distance( const Eigen::MatrixXd & X, const Eigen::MatrixXd & VY, @@ -9,22 +10,27 @@ void point_mesh_distance( Eigen::MatrixXd & N) { // Replace with your code - P.resizeLike(X); + D = Eigen::VectorXd::Zero(X.rows()); + P = Eigen::MatrixXd::Zero(X.rows(), 3); + N = Eigen::MatrixXd::Zero(X.rows(), 3); + N = Eigen::MatrixXd::Zero(X.rows(),X.cols()); double d; Eigen::RowVector3d p; Eigen::MatrixXd pfn; igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),pfn); + for(int i = 0;i::max(); for(int j = 0;j d) { D(i) = d; P.row(i) = p; N.row(i) = pfn.row(j); } } + // std::cout << "test2.4" << std::endl; } + } diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index e2dc5b0..d84cc84 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -1,6 +1,7 @@ #include "point_to_plane_rigid_matching.h" #include "closest_rotation.h" #include +#include void point_to_plane_rigid_matching( const Eigen::MatrixXd & X, @@ -10,10 +11,14 @@ void point_to_plane_rigid_matching( Eigen::RowVector3d & t) { // Replace with your code - Eigen::Matrix3d dig = Eigen::Matrix3d::Zero(X.rows() * 3, 3); + std::cout << "test3.1" << std::endl; + Eigen::MatrixXd dig = Eigen::MatrixXd::Zero(X.rows(), X.rows() *3); dig.block(0,0,X.rows(),X.rows()) = N.col(0).asDiagonal(); + std::cout << "test3.11" << std::endl; dig.block(0,X.rows(),X.rows(),X.rows()) = N.col(1).asDiagonal(); + std::cout << "test3.12" << std::endl; dig.block(0,X.rows()*2,X.rows(),X.rows()) = N.col(2).asDiagonal(); + std::cout << "test3.15" << std::endl; Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6); for (int i = 0; i +#include +#include void point_triangle_distance( const Eigen::RowVector3d & x, @@ -11,7 +13,11 @@ void point_triangle_distance( { // following algorithm from below link // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf + Eigen::RowVector3d ab = a-b; Eigen::RowVector3d ba = b-a; + Eigen::RowVector3d ac = a-c; + Eigen::RowVector3d ca = c-a; + Eigen::RowVector3d bc = b-c; Eigen::RowVector3d cb = c-b; Eigen::RowVector3d ax = a-x; Eigen::RowVector3d n = ba.cross(cb); @@ -22,6 +28,7 @@ void point_triangle_distance( Eigen::RowVector3d pb = projectx - b; Eigen::RowVector3d pc = projectx - c; Eigen::RowVector3d pa = projectx - a; + double alpha = (pb.cross(pc)).norm() / area; double beta = (pc.cross(pa)).norm() / area; double gamma = 1 - alpha - beta; @@ -34,16 +41,33 @@ void point_triangle_distance( return ; } d = std::numeric_limits::max(); - if (d > (x - a).norm()) { - d = (x - a).norm(); - p = a; + dist_helper(x, projectx, a, b, d, p); + dist_helper(x, projectx, b, c, d, p); + dist_helper(x, projectx, c, a, d, p); +} + +void dist_helper(const Eigen::RowVector3d & x, + const Eigen::RowVector3d & px, + const Eigen::RowVector3d & a, + const Eigen::RowVector3d & b, + double & d, + Eigen::RowVector3d & p) { + Eigen::RowVector3d pa = a + ((px-a).dot(b-a)) / ((b-a).dot(b-a)) * (b-a); + double distance; + if (abs((pa-a).norm() + (a-b).norm() - (pa-b).norm()) < 1e-10) { + distance = (x-a).norm(); + p = a; } - if (d > (x - b).norm()) { - d = (x - b).norm(); - p = b; + else if (abs((pa-b).norm() + (b-a).norm() - (pa-a).norm()) <1e-10) { + distance = (x-b).norm(); + p = b; } - if (d > (x - c).norm()) { - d = (x - c).norm(); - p = c; + else { + distance = (x-pa).norm(); + p = pa; + } + + if (d > distance) { + d = distance; } } From 245a372e1a4bf357420b22ba2e017c9bff2f21c0 Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Tue, 6 Feb 2018 01:01:42 -0500 Subject: [PATCH 07/10] fix random sample --- main.cpp | 4 +-- src/icp_single_iteration.cpp | 4 --- src/point_triangle_distance.cpp | 9 ++++++ src/random_points_on_mesh.cpp | 52 +++++++++++++++++++++++++++------ 4 files changed, 54 insertions(+), 15 deletions(-) diff --git a/main.cpp b/main.cpp index b65c7f6..4fde073 100644 --- a/main.cpp +++ b/main.cpp @@ -20,8 +20,8 @@ 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; + ICPMethod method = ICP_METHOD_POINT_TO_POINT; + // ICPMethod method = ICP_METHOD_POINT_TO_PLANE; igl::viewer::Viewer viewer; std::cout<::max(); @@ -57,14 +63,17 @@ void dist_helper(const Eigen::RowVector3d & x, if (abs((pa-a).norm() + (a-b).norm() - (pa-b).norm()) < 1e-10) { distance = (x-a).norm(); p = a; + std::cout << "test2" << std::endl; } else if (abs((pa-b).norm() + (b-a).norm() - (pa-a).norm()) <1e-10) { distance = (x-b).norm(); p = b; + std::cout << "test3" << std::endl; } else { distance = (x-pa).norm(); p = pa; + std::cout << "test4" << std::endl; } if (d > distance) { diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 5e4ec99..2f6c75b 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -1,4 +1,6 @@ #include "random_points_on_mesh.h" +#include +#include void random_points_on_mesh( const int n, @@ -15,33 +17,65 @@ void random_points_on_mesh( double ttl = cumsumA(cumsumA.rows() - 1, 0); for(int i=0;i distribution(0.,ttl); + double randx = distribution(generator); + int idx = cumsumA.rows() / 2; int gap = cumsumA.rows() / 4; //go over boundary? - while(idx != 0 && idx != cumsumA.rows() - 1 && - cumsumA(idx - 1) < randx && - cumsumA(idx) > randx) { - if (randx > cumsumA(idx - 1)) { + while(idx >= 1 && idx <= cumsumA.rows() - 1 && gap != 0) { + if (cumsumA(idx - 1, 0) < randx && cumsumA(idx, 0) > randx) { + // std::cout << "hihiiiihi" << std::endl; + break; + } + if (randx > cumsumA(idx)) { idx += gap; } else { idx -= gap; } - gap /= 2; - } + gap = gap / 2; + if (gap == 0) { + gap = 1; + } + // std::cout << "==============" << std::endl; + // std::cout << cumsumA(idx - 1) << std::endl; + // std::cout << randx << std::endl; + // std::cout << cumsumA(idx) << std::endl; + // std::cout << gap << std::endl; + // std::cout << idx << std::endl; + // std::string mystr; + // getline (std::cin, mystr); - double alpha = ((double)rand()/(double)RAND_MAX); - double beta = ((double)rand()/(double)RAND_MAX); + } + // std::cout << idx << std::endl; + std::uniform_real_distribution distribution_alpha(0.,1.); + std::uniform_real_distribution distribution_beta(0.,1.); + double alpha = distribution_alpha(generator); + double beta = distribution_beta(generator); if (alpha + beta > 1) { alpha = 1 - alpha; beta = 1 - beta; } + if (idx < 0) { + idx = 0; + } + if (idx > cumsumA.rows() - 1) { + idx = cumsumA.rows() - 1; + } + X.row(i) = V.row(F(idx, 0)) + alpha * V.row(F(idx, 1)) + beta * V.row(F(idx, 2)); + // std::cout << "==============" << std::endl; + // std::cout << idx << std::endl; + // std::cout << randx << std::endl; + // std::cout << alpha << std::endl; + // std::cout << beta << std::endl; } } From 74699ef44f79671b7909f03fcac0a7ce43901dde Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Tue, 6 Feb 2018 01:15:14 -0500 Subject: [PATCH 08/10] finally working, reverse UV --- src/closest_rotation.cpp | 2 +- src/point_triangle_distance.cpp | 14 +++++--------- src/random_points_on_mesh.cpp | 4 ++-- 3 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/closest_rotation.cpp b/src/closest_rotation.cpp index 1a31b19..6ba04fe 100644 --- a/src/closest_rotation.cpp +++ b/src/closest_rotation.cpp @@ -13,5 +13,5 @@ void closest_rotation( omega << 1, 0, 0, 0, 1, 0, 0, 0, (svd.matrixU() * svd.matrixV().transpose()).determinant(); - R = svd.matrixU() * omega * svd.matrixV().transpose(); + R = svd.matrixV() * omega * svd.matrixU().transpose(); } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index fc431f8..02ae391 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -13,11 +13,11 @@ void point_triangle_distance( { // following algorithm from below link // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf - if (true) { - d = (x-a).norm(); - p = a; - return ; - } + // if (true) { + // d = (x-a).norm(); + // p = a; + // return ; + // } Eigen::RowVector3d ab = a-b; Eigen::RowVector3d ba = b-a; Eigen::RowVector3d ac = a-c; @@ -43,7 +43,6 @@ void point_triangle_distance( 0 <= gamma && gamma <= 1) { p = projectx; d = (x - p).norm(); - std::cout << "test1" << std::endl; return ; } d = std::numeric_limits::max(); @@ -63,17 +62,14 @@ void dist_helper(const Eigen::RowVector3d & x, if (abs((pa-a).norm() + (a-b).norm() - (pa-b).norm()) < 1e-10) { distance = (x-a).norm(); p = a; - std::cout << "test2" << std::endl; } else if (abs((pa-b).norm() + (b-a).norm() - (pa-a).norm()) <1e-10) { distance = (x-b).norm(); p = b; - std::cout << "test3" << std::endl; } else { distance = (x-pa).norm(); p = pa; - std::cout << "test4" << std::endl; } if (d > distance) { diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 2f6c75b..711abff 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -69,8 +69,8 @@ void random_points_on_mesh( } X.row(i) = V.row(F(idx, 0)) + - alpha * V.row(F(idx, 1)) + - beta * V.row(F(idx, 2)); + alpha * (V.row(F(idx, 1)) - V.row(F(idx, 0))) + + beta * (V.row(F(idx, 2)) - V.row(F(idx, 0))); // std::cout << "==============" << std::endl; // std::cout << idx << std::endl; // std::cout << randx << std::endl; From 9d6a7ab98a9997a0731d7a04b89c21e11801b3fe Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Tue, 6 Feb 2018 01:16:14 -0500 Subject: [PATCH 09/10] cleanup debug code --- src/point_mesh_distance.cpp | 1 - src/point_triangle_distance.cpp | 5 ----- src/random_points_on_mesh.cpp | 17 ----------------- 3 files changed, 23 deletions(-) diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp index 20130f0..d8ac445 100644 --- a/src/point_mesh_distance.cpp +++ b/src/point_mesh_distance.cpp @@ -30,7 +30,6 @@ void point_mesh_distance( N.row(i) = pfn.row(j); } } - // std::cout << "test2.4" << std::endl; } } diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp index 02ae391..9c39db3 100644 --- a/src/point_triangle_distance.cpp +++ b/src/point_triangle_distance.cpp @@ -13,11 +13,6 @@ void point_triangle_distance( { // following algorithm from below link // http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf - // if (true) { - // d = (x-a).norm(); - // p = a; - // return ; - // } Eigen::RowVector3d ab = a-b; Eigen::RowVector3d ba = b-a; Eigen::RowVector3d ac = a-c; diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp index 711abff..6ac074c 100644 --- a/src/random_points_on_mesh.cpp +++ b/src/random_points_on_mesh.cpp @@ -24,10 +24,8 @@ void random_points_on_mesh( int idx = cumsumA.rows() / 2; int gap = cumsumA.rows() / 4; - //go over boundary? while(idx >= 1 && idx <= cumsumA.rows() - 1 && gap != 0) { if (cumsumA(idx - 1, 0) < randx && cumsumA(idx, 0) > randx) { - // std::cout << "hihiiiihi" << std::endl; break; } if (randx > cumsumA(idx)) { @@ -40,17 +38,7 @@ void random_points_on_mesh( if (gap == 0) { gap = 1; } - // std::cout << "==============" << std::endl; - // std::cout << cumsumA(idx - 1) << std::endl; - // std::cout << randx << std::endl; - // std::cout << cumsumA(idx) << std::endl; - // std::cout << gap << std::endl; - // std::cout << idx << std::endl; - // std::string mystr; - // getline (std::cin, mystr); - } - // std::cout << idx << std::endl; std::uniform_real_distribution distribution_alpha(0.,1.); std::uniform_real_distribution distribution_beta(0.,1.); double alpha = distribution_alpha(generator); @@ -71,11 +59,6 @@ void random_points_on_mesh( X.row(i) = V.row(F(idx, 0)) + alpha * (V.row(F(idx, 1)) - V.row(F(idx, 0))) + beta * (V.row(F(idx, 2)) - V.row(F(idx, 0))); - // std::cout << "==============" << std::endl; - // std::cout << idx << std::endl; - // std::cout << randx << std::endl; - // std::cout << alpha << std::endl; - // std::cout << beta << std::endl; } } From bb3eeb9fabb5d6a303e126458ac2576edf73684a Mon Sep 17 00:00:00 2001 From: Ziheng Liang Date: Tue, 6 Feb 2018 01:26:37 -0500 Subject: [PATCH 10/10] cleanup v2 --- src/point_to_plane_rigid_matching.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/point_to_plane_rigid_matching.cpp b/src/point_to_plane_rigid_matching.cpp index d84cc84..eb5d039 100644 --- a/src/point_to_plane_rigid_matching.cpp +++ b/src/point_to_plane_rigid_matching.cpp @@ -11,14 +11,10 @@ void point_to_plane_rigid_matching( Eigen::RowVector3d & t) { // Replace with your code - std::cout << "test3.1" << std::endl; Eigen::MatrixXd dig = Eigen::MatrixXd::Zero(X.rows(), X.rows() *3); dig.block(0,0,X.rows(),X.rows()) = N.col(0).asDiagonal(); - std::cout << "test3.11" << std::endl; dig.block(0,X.rows(),X.rows(),X.rows()) = N.col(1).asDiagonal(); - std::cout << "test3.12" << std::endl; dig.block(0,X.rows()*2,X.rows(),X.rows()) = N.col(2).asDiagonal(); - std::cout << "test3.15" << std::endl; Eigen::MatrixXd A = Eigen::MatrixXd::Zero(X.rows() * 3, 6); for (int i = 0; i