Yihuan Mao submission

README.html

@@ -343,7 +343,7 @@ <h3 id="constantfunctionapproximation">Constant function approximation</h3>
 
 <h3 id="linearfunctionapproximation">Linear function approximation</h3>
 
-<p>If we make make a slightly more appropriate assuming that in the neighborhood
+<p>If we make make a slightly more appropriate assumption that in the neighborhood
 of the <span class="math">\(P_Y(\z₀)\)</span> the surface <span class="math">\(Y\)</span> is a plane, then we can improve this
 approximation while keeping <span class="math">\(\f\)</span> linear in <span class="math">\(\z\)</span>:</p>
 

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$:
 

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

src/closest_rotation.cpp

@@ -1,9 +1,16 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/Dense> 
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  Eigen::JacobiSVD<Eigen::MatrixXd> 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();
 }

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 <Eigen/Dense>
 
 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();
 }

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);
+  }
 }

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<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
-  D = (X-P).rowwise().norm();
+  int n=X.rows(),m=FY.rows(),jj;
+  D.resize(n);
+  P.resize(n,3);
+  N.resize(n,3);
+  double d,dd;
+  Eigen::RowVector3d p,pp,a,b,nn; 
+  for (int i=0; i<n; i++){
+  	d=99999999;
+  	for (int j=0; j<m; j++){
+  	  point_triangle_distance(X.row(i),VY.row(FY(j,0)),VY.row(FY(j,1)),VY.row(FY(j,2)),dd,pp);
+  	  if (dd<d){
+  	  	jj=j;
+  	  	d=dd;
+  	  	p=pp;
+	  }
+	}
+	D(i)=d;
+	P.row(i)=p;
+	a=VY.row(FY(jj,0))-VY.row(FY(jj,1));
+	b=VY.row(FY(jj,0))-VY.row(FY(jj,2));
+	nn=a.cross(b);
+	if (nn.dot(X.row(i)-p)<0) nn=-nn;
+	nn=nn.normalized();
+	N.row(i)=nn;
+  }
 }
+

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
+#include <Eigen/Dense>
+#include "closest_rotation.h"
+#include <fstream>
 
 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));
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,69 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include <Eigen/Dense>
+#include "closest_rotation.h"
+#include <fstream>
+#include <Eigen/SVD>
 
 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();
+/*
+  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<Eigen::MatrixXd> 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));
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_triangle_distance.h"
+#include <fstream>
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -8,7 +9,42 @@ 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<d){
+  	d=dd;
+  	p=b;
+  }
+  dd=(x-c).norm();
+  if (dd<d){
+  	d=dd;
+  	p=c;
+  }
+  dd=(x-a2b).norm();
+  if (((a2b-a).dot(a2b-b)<0)&&(dd<d)){
+  	d=dd;
+  	p=a2b;
+  }
+  dd=(x-b2c).norm();
+  if (((b2c-b).dot(b2c-c)<0)&&(dd<d)){
+  	d=dd;
+  	p=b2c;
+  }
+  dd=(x-c2a).norm();
+  if (((c2a-a).dot(c2a-c)<0)&&(dd<d)){
+  	d=dd;
+  	p=c2a;
+  }
 }
+

src/random_points_on_mesh.cpp

@@ -1,4 +1,5 @@
 #include "random_points_on_mesh.h"
+#include <igl/doublearea.h>
 
 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;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+  double a,b,Ssum,cho;
+  Eigen::MatrixXd S(F.rows(),1);
+  Eigen::Vector3d p,a1,b1,st;
+  int j;
+  for (int i=0; i<n; i++){
+  	igl::doublearea(V,F,S);
+  	Ssum=S.sum();
+  	cho=double(rand()%10000)/10001*Ssum;
+  	for (j=0; j<F.rows(); j++)
+  	  if (cho<S(j,0)) break;
+  	  else cho-=S(j,0);
+  	a=double(rand()%10000)/10001;
+  	b=double(rand()%10000)/10001;
+  	if (a+b>1){
+  		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))); 
+  }
 }