Liupei Lu HW02

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,18 @@
 #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::MatrixXd U = svd.matrixU();
+  Eigen::MatrixXd V = svd.matrixV();
+  Eigen::Matrix3d sigma;
+  sigma << 1, 0 , 0,
+           0, 1 , 0,
+           0, 0, (U * V.transpose()).determinant();
+  R = U * sigma * V.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,
@@ -7,6 +9,11 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  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);
+
+  return D.maxCoeff();
 }

src/icp_single_iteration.cpp

@@ -1,16 +1,31 @@
 #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,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int num_samples,
-  const ICPMethod method,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    const Eigen::MatrixXd &VX,
+    const Eigen::MatrixXi &FX,
+    const Eigen::MatrixXd &VY,
+    const Eigen::MatrixXi &FY,
+    const int num_samples,
+    const ICPMethod method,
+    Eigen::Matrix3d &R,
+    Eigen::RowVector3d &t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+  Eigen::MatrixXd X, P, N;
+  Eigen::VectorXd D;
+
+  random_points_on_mesh(num_samples, VX, FX, X);
+  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
+  {
+    point_to_plane_rigid_matching(X, P, N, R, t);
+  }
 }

src/point_mesh_distance.cpp

@@ -1,16 +1,39 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include "igl/per_face_normals.h"
 
 void point_mesh_distance(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  Eigen::VectorXd & D,
-  Eigen::MatrixXd & P,
-  Eigen::MatrixXd & N)
+    const Eigen::MatrixXd &X,
+    const Eigen::MatrixXd &VY,
+    const Eigen::MatrixXi &FY,
+    Eigen::VectorXd &D,
+    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();
+  N = Eigen::MatrixXd::Zero(X.rows(), X.cols());
+  D.resize(X.rows());
+
+  for (int i = 0; i < X.rows(); i++)
+  {
+
+    double minD = std::numeric_limits<double>::max();
+    Eigen::RowVector3d minP;
+    for (int j = 0; j < FY.rows(); j++)
+    {
+      double d;
+      Eigen::RowVector3d p;
+      point_triangle_distance(X.row(i),VY.row(FY(j,0)), VY.row(FY(j,1)), VY.row(FY(j,2)), d, p);
+      if (minD < d)
+      {
+        minD = d;
+        minP = p;
+      }
+    }
+
+    D(i) = minD;
+    P.row(i) = minP;
+  }
+
+  igl::per_face_normals(VY,FY,N);
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,13 +1,42 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  const Eigen::MatrixXd & N,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    const Eigen::MatrixXd &X,
+    const Eigen::MatrixXd &P,
+    const Eigen::MatrixXd &N,
+    Eigen::Matrix3d &R,
+    Eigen::RowVector3d &t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+
+  Eigen::VectorXd ones = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zeros = Eigen::VectorXd::Zero(X.rows());
+
+  Eigen::VectorXd B;
+  B << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2);
+
+  Eigen::MatrixXd A(3 * X.rows(), 6);
+  A << zeros, X.col(2), -X.col(1), ones, zeros, zeros,
+      -X.col(2), zeros, X.col(0), zeros, ones, zeros,
+      X.col(1), -X.col(0), zeros, zeros, zeros, ones;
+
+  Eigen::MatrixXd diagN(X.rows(),3*X.rows());
+  diagN << Eigen::MatrixXd (N.col(0).asDiagonal()),
+           Eigen::MatrixXd (N.col(1).asDiagonal()),
+           Eigen::MatrixXd (N.col(2).asDiagonal());
+
+  A = diagN * A;
+  B = diagN * B;
+  Eigen::VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * B);
+
+  Eigen::Matrix3d M;
+
+  M << 1, -u(2), u(1),
+      u(2), 1, -u(0),
+      -u(1), u(0), 1;
+
+  closest_rotation(M, R);
+
+  t << u(3), u(4), u(5);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,34 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/Dense>
+#include <iostream>
 
 void point_to_point_rigid_matching(
-  const Eigen::MatrixXd & X,
-  const Eigen::MatrixXd & P,
-  Eigen::Matrix3d & R,
-  Eigen::RowVector3d & t)
+    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 ones = Eigen::VectorXd::Ones(X.rows());
+  Eigen::VectorXd zeros = Eigen::VectorXd::Zero(X.rows());
+
+  Eigen::VectorXd B(X.rows() * 3);
+  B << X.col(0) - P.col(0), X.col(1) - P.col(1), X.col(2) - P.col(2);
+
+  Eigen::MatrixXd A(3 * X.rows(), 6);
+  A << zeros, X.col(2), -X.col(1), ones, zeros, zeros,
+      -X.col(2), zeros, X.col(0), zeros, ones, zeros,
+      X.col(1), -X.col(0), zeros, zeros, zeros, ones;
+
+  Eigen::VectorXd u = (A.transpose() * A).inverse() * (-A.transpose() * B);
 
+  Eigen::Matrix3d M;
+  M << 1, -u(2), u(1),
+      u(2), 1, -u(0),
+      -u(1), u(0), 1;
+
+  closest_rotation(M, R);
+
+  t << u(3), u(4), u(5);
+}

src/point_triangle_distance.cpp

@@ -1,14 +1,83 @@
 #include "point_triangle_distance.h"
+#include <Eigen/Geometry>
+
+void point_on_line(const Eigen::RowVector3d &x,
+                   const Eigen::RowVector3d &a,
+                   const Eigen::RowVector3d &b,
+                   const Eigen::RowVector3d &projectP,
+                   double &d,
+                   Eigen::RowVector3d &p)
+{
+  Eigen::RowVector3d nab = ((b - projectP).cross(a - projectP)).cross(b - a);
+  nab.normalize();
+  double d_ab = abs((a - projectP).dot(nab));
+  Eigen::RowVector3d pab = projectP + d_ab * nab;
+  double t = (pab - a).dot(b - a) / (b - a).dot(b - a);
+  if (t < 0)
+  {
+    p = a;
+    d = (x - p).norm();
+  }
+  if (0 < t < 1)
+  {
+    p = pab;
+    d = (x - p).norm();
+  }
+  if (t > 1)
+  {
+    p = b;
+    d = (x - p).norm();
+  }
+}
 
 void point_triangle_distance(
-  const Eigen::RowVector3d & x,
-  const Eigen::RowVector3d & a,
-  const Eigen::RowVector3d & b,
-  const Eigen::RowVector3d & c,
-  double & d,
-  Eigen::RowVector3d & p)
+    const Eigen::RowVector3d &x,
+    const Eigen::RowVector3d &a,
+    const Eigen::RowVector3d &b,
+    const Eigen::RowVector3d &c,
+    double &d,
+    Eigen::RowVector3d &p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  Eigen::RowVector3d v1 = b - a;
+  Eigen::RowVector3d v2 = -a + c;
+  Eigen::RowVector3d n = v1.cross(v2);
+  n.normalize();
+
+  double distance = abs((x - a).dot(n));
+  Eigen::RowVector3d projectP = x - distance * n;
+  Eigen::RowVector3d v3 = projectP - a;
+  Eigen::RowVector3d v4, v5, v6;
+
+  // based on https://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
+  double u = (v2.dot(v2) * v3.dot(v1) - v1.dot(v2) * v3.dot(v2)) * 1.0 /
+             (v2.dot(v2) * v1.dot(v1) - v1.dot(v2) * v1.dot(v2));
+
+  double v = (v1.dot(v1) * v3.dot(v2) - v1.dot(v2) * v3.dot(v1)) * 1.0 /
+             (v2.dot(v2) * v1.dot(v1) - v1.dot(v2) * v1.dot(v2));
+
+  if (u >= 0 && v >= 0 && (u + v <= 1))
+  { //inside
+    p = a + u * v1 + v * v2;
+    d = distance;
+  }
+  else if (u >= 0 and v < 0)
+  {
+    // On ab
+    point_on_line(x, a, b, projectP, d, p);
+  }
+  else if (u < 0 && v >= 0)
+  {
+    //On ac
+    point_on_line(x, a, c, projectP, d, p);
+  }
+  else if (u >= 0 && v >= 0 && u + v > 1)
+  {
+    //On bc
+    point_on_line(x, b, c, projectP, d, p);
+  }
+  else
+  { //Outside
+    p = a;
+    d = (x - p).norm();
+  }
 }

src/random_points_on_mesh.cpp

@@ -1,13 +1,47 @@
 #include "random_points_on_mesh.h"
+#include <iostream>
+#include <random>
+#include "igl/doublearea.h"
+#include "igl/cumsum.h"
 
-void random_points_on_mesh(
-  const int n,
-  const Eigen::MatrixXd & V,
-  const Eigen::MatrixXi & F,
-  Eigen::MatrixXd & X)
+int return_index(Eigen::VectorXd C, double r)
 {
-  // REPLACE WITH YOUR CODE:
-  X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+  for(int i = 0; i < C.rows(); i++)
+  {
+    if (C(i) / C(C.rows() - 1) >= r)
+        return i;
+  }
 }
 
+void random_points_on_mesh(
+    const int n,
+    const Eigen::MatrixXd &V,
+    const Eigen::MatrixXi &F,
+    Eigen::MatrixXd &X)
+{
+  X.resize(n, 3);
+
+  Eigen::MatrixXd list;
+  Eigen::VectorXd C;
+  igl::doublearea(V, F, list);
+  igl::cumsum(list, 1, C);
+
+  //uniformed distribution from http://www.cplusplus.com/reference/random/uniform_real_distribution/
+  std::default_random_engine generator;
+  std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+  for (int i = 0; i < X.rows(); i++)
+  {
+    double a = distribution(generator);
+    double b = distribution(generator);
+    double r = distribution(generator);
+
+    if (a + b > 1.0)
+    {
+      a = 1 - a;
+      b = 1 - b;
+    }
+    int idx = return_index(C, r);
+    X.row(i) = V.row(F(idx, 0)) + a * (V.row(F(idx, 1)) - V.row(F(idx, 0))) + b * (V.row(F(idx, 2)) - V.row(F(idx, 0)));
+  }
+}