Minhua Zhu HW 2

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

src/closest_rotation.cpp

@@ -1,9 +1,17 @@
 #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();
+  //R = Eigen::Matrix3d::Identity();
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::Matrix3d O =  Eigen::Matrix3d::Zero();
+  O(0,0) = 1;
+  O(1,1) = 1;
+  O(2,2) = (svd.matrixU() * (svd.matrixV().transpose())).determinant();
+  R = svd.matrixU() * O * (svd.matrixV().transpose());
 }

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;
+  //return 0;
+  Eigen::MatrixXd X,P,N;
+  Eigen::VectorXd D;
+  random_points_on_mesh(n,VX,FX,X);
+  point_mesh_distance(X,VY,FY,D,P,N);
+  double lowerbound = D.maxCoeff();
+  return lowerbound;
 }

src/icp_single_iteration.cpp

@@ -1,4 +1,8 @@
 #include "icp_single_iteration.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -11,6 +15,20 @@ void icp_single_iteration(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+
+  Eigen::MatrixXd X;
+  random_points_on_mesh(num_samples,VX,FX,X);
+
+  Eigen::MatrixXd P,N;
+  Eigen::VectorXd D;
+  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 { // ICP_METHOD_POINT_TO_PLANE
+    point_to_plane_rigid_matching(X,P,N,R,t);
+  }
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,8 @@
 #include "point_mesh_distance.h"
+#include <igl/per_face_normals.h>
+#include "point_triangle_distance.h"
+#include <limits>
+#include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -10,7 +14,35 @@ void point_mesh_distance(
 {
   // Replace with your code
   P.resizeLike(X);
+  //N.resize(X.rows(),3);
   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();
+  //for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
+  //D = (X-P).rowwise().norm();
+  //D.resize(X.rows(), 1);
+  Eigen::MatrixXi triangle_F;
+  triangle_F.resize(X.rows(),3);
+
+  for (int i=0; i<X.rows(); i++) {
+    double idist = std::numeric_limits<double>::max();
+    Eigen::RowVector3d x = X.row(i);
+    Eigen::RowVector3d ip;
+
+    for (int j=0; j<FY.rows(); j++) {
+      double cur_dist;
+      Eigen::RowVector3d cur_p,a,b,c;
+      a = VY.row(FY(j,0));
+      b = VY.row(FY(j,1));
+      c = VY.row(FY(j,2));
+      point_triangle_distance(x,a,b,c,cur_dist,cur_p);
+      if (cur_dist < idist) {
+        idist = cur_dist;
+        ip = cur_p;
+        triangle_F.row(i) = FY.row(j);
+      }
+    }
+
+    P.row(i) = ip;
+    D(i) = idist;
+  }
+  igl::per_face_normals(VY,triangle_F,N);
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,6 +10,52 @@ void point_to_plane_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+  // nT
+  Eigen::MatrixXd nT;
+  nT.resize(X.rows(),3*X.rows());
+  nT << Eigen::MatrixXd (N.col(0).asDiagonal()),
+        Eigen::MatrixXd (N.col(1).asDiagonal()),
+        Eigen::MatrixXd (N.col(2).asDiagonal());
+
+  // A
+  Eigen::MatrixXd A;
+  A.resize(3*X.rows(),6);
+  Eigen::VectorXd diag_one = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(X.rows());
+  A << zero, X.col(2), -X.col(1), diag_one, zero, zero,
+       -X.col(2), zero, X.col(0), zero, diag_one, zero,
+       X.col(1), -X.col(0), zero, zero, zero, diag_one;
+  A = nT * A;
+
+  // X - P
+  Eigen::VectorXd X_minus_P;
+  X_minus_P.resize(3*X.rows());
+  X_minus_P << X.col(0) - P.col(0),
+               X.col(1) - P.col(1),
+               X.col(2) - P.col(2);
+  X_minus_P = nT * X_minus_P;
+
+  // u
+  Eigen::Matrix<double,6,1> u = ((A.transpose()*A).inverse()) * (-A.transpose()*X_minus_P);
+
+  // a,b,r
+  double a, b, r;
+  a = u(0,0);
+  b = u(1,0);
+  r = u(2,0);
+
+  // t
+  t << u(3,0),u(4,0),u(5,0);
+
+  // M
+  Eigen::Matrix3d M;
+  M << 1, -r, b,
+       r, 1, -a,
+       -b, a, 1;
+
+  // R
+  closest_rotation(M, R);
+
 }

src/point_to_point_rigid_matching.cpp

@@ -1,5 +1,7 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/Core>
 
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -8,7 +10,45 @@ void point_to_point_rigid_matching(
   Eigen::RowVector3d & t)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  // R = Eigen::Matrix3d::Identity();
+  // t = Eigen::RowVector3d::Zero();
+  // A
+  Eigen::MatrixXd A;
+  A.resize(3*X.rows(),6);
+  Eigen::VectorXd diag_one = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zero = Eigen::VectorXd::Zero(X.rows());
+  A << zero, X.col(2), -X.col(1), diag_one, zero, zero,
+       -X.col(2), zero, X.col(0), zero, diag_one, zero,
+       X.col(1), -X.col(0), zero, zero, zero, diag_one;
+
+  // X - P
+  Eigen::MatrixXd X_minus_P;
+  X_minus_P.resize(3*X.rows(),1);
+  X_minus_P << X.col(0) - P.col(0),
+               X.col(1) - P.col(1),
+               X.col(2) - P.col(2);
+
+  // u
+  Eigen::Matrix<double,6,1> u = (A.transpose()*A).inverse() * (-A.transpose()*X_minus_P);
+
+  // a,b,r
+  double a, b, r;
+  a = u(0,0);
+  b = u(1,0);
+  r = u(2,0);
+
+  // t
+  t << u(3,0),u(4,0),u(5,0);
+
+  // M
+  Eigen::Matrix3d M;
+  M << 1, -r, b,
+       r, 1, -a,
+       -b, a, 1;
+
+  // R
+  closest_rotation(M, R);
+
+
 }
 

src/point_triangle_distance.cpp

@@ -1,4 +1,57 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
+
+bool point_in_triangle(
+  const Eigen::RowVector3d & a,
+  const Eigen::RowVector3d & b,
+  const Eigen::RowVector3d & c,
+  const Eigen::RowVector3d & p)
+{
+  Eigen::RowVector3d pa,pb,pc;
+  pa = (p - a).normalized();
+  pb = (p - b).normalized();
+  pc = (p - c).normalized();
+  double cos_theta = pa.dot(pb) + pa.dot(pc) + pb.dot(pc);
+  return (cos_theta == 1.0);
+}
+
+void point_line_distance (
+  const Eigen::RowVector3d & x,
+  const Eigen::RowVector3d & a,
+  const Eigen::RowVector3d & b,
+  double & d,
+  Eigen::RowVector3d & p
+) 
+{
+  Eigen::RowVector3d ab,xa,n;
+  ab = a - b;
+  xa = x - a;
+  n = ab.cross(xa);
+  if ((n.norm() == 0) && (xa.norm() <= ab.norm())) { // x on ab
+    p = x;
+    d = 0;
+    return;
+  }
+
+  double dist = xa.dot(n);
+  Eigen::RowVector3d p_pontential = x - dist * n;
+
+  if ((p_pontential - a).norm() > ab.norm()) { // p is outside ab
+    p = b;
+    d = (x - b).norm();
+  }
+
+  else if ((p_pontential - b).norm() > ab.norm())
+  {
+    p = a;
+    d = (x - a).norm();
+  }
+  else {
+    p = p_pontential;
+    d = dist;
+  }
+
+}
 
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
@@ -9,6 +62,30 @@ void point_triangle_distance(
   Eigen::RowVector3d & p)
 {
   // Replace with your code
-  d = 0;
-  p = a;
+  // d = 0;
+  // p = a;
+  Eigen::RowVector3d n = ((a-b).cross(a-c)).normalized();
+  p = x - n.dot(x)*n;
+  // p in triangle
+  if (point_in_triangle(a,b,c,p)) {
+    d = (x - p).norm();
+  }
+  else // p outside triangle
+  {
+    double ad,bd,cd;
+    Eigen::RowVector3d ap,bp,cp;
+    point_line_distance(x,a,b,ad,ap);
+    point_line_distance(x,a,c,cd,cp);
+    point_line_distance(x,b,c,bd,bp);
+    d = ad;
+    p = ap;
+    if (cd > d) {
+      d = cd;
+      p = cp;
+    }
+    if (bd > d) {
+      d = bd;
+      p = bp;
+    }
+  }
 }