fix random sample

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<<R"(

src/icp_single_iteration.cpp

@@ -16,18 +16,14 @@ 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;

src/point_triangle_distance.cpp

@@ -13,6 +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 ;
+  }
   Eigen::RowVector3d ab = a-b;
   Eigen::RowVector3d ba = b-a;
   Eigen::RowVector3d ac = a-c;
@@ -38,6 +43,7 @@ void point_triangle_distance(
   	0 <= gamma && gamma <= 1) {
   	p = projectx;
   	d = (x - p).norm();
+    std::cout << "test1" << std::endl;
   	return ;
   }
   d = std::numeric_limits<double>::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) {

src/random_points_on_mesh.cpp

@@ -1,4 +1,6 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
+#include <random>
 
 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<n;i++) {
-  	double randx = ((double)rand()/(double)RAND_MAX) * ttl;
+
+    static std::default_random_engine generator;
+    std::uniform_real_distribution<double> 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<double> distribution_alpha(0.,1.);
+    std::uniform_real_distribution<double> 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;
   }
 }