Amlan Kar

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

include/closest_rotation.h

@@ -1,6 +1,9 @@
 #ifndef CLOSEST_ROTATION_H
 #define CLOSEST_ROTATION_H
 #include <Eigen/Core>
+#include <Eigen/SVD>
+#include <Eigen/Dense>
+
 // Given a 3×3 matrix `M`, find the closest rotation matrix `R`.
 //
 // Inputs:

include/hausdorff_lower_bound.h

@@ -1,6 +1,9 @@
 #ifndef HAUSDORFF_LOWER_BOUND_H
 #define HAUSDORFF_LOWER_BOUND_H
 #include <Eigen/Core>
+#include <point_mesh_distance.h>
+#include <random_points_on_mesh.h>
+
 // Compute a lower bound on the _directed_ Hausdorff distance from a given mesh
 // (VX,FX) to another mesh (VY,FY).
 //

include/icp_single_iteration.h

@@ -1,6 +1,10 @@
 #ifndef ICP_SINGLE_ITERATION_H
 #define ICP_SINGLE_ITERATION_H
 #include <Eigen/Core>
+#include <random_points_on_mesh.h>
+#include <point_mesh_distance.h>
+#include <point_to_plane_rigid_matching.h>
+#include <point_to_point_rigid_matching.h>
 
 enum ICPMethod
 {

include/point_mesh_distance.h

@@ -1,6 +1,10 @@
 #ifndef POINT_MESH_DISTANCE_H
 #define POINT_MESH_DISTANCE_H
 #include <Eigen/Core>
+#include <limits>
+#include "point_triangle_distance.h"
+#include "igl/per_face_normals.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`.

include/point_to_plane_rigid_matching.h

@@ -1,6 +1,8 @@
 #ifndef POINT_TO_PLANE_RIGID_MATCHING_H
 #define POINT_TO_PLANE_RIGID_MATCHING_H
 #include <Eigen/Core>
+#include <closest_rotation.h>
+
 // Given a set of source points `X` and corresponding target points `P` and
 // normals `N`, find the optimal rigid transformation (`R`,`t`) that aligns `X`
 // to `P`, minimizing the matching energy:

include/point_to_point_rigid_matching.h

@@ -1,6 +1,8 @@
 #ifndef POINT_TO_POINT_RIGID_MATCHING_H
 #define POINT_TO_POINT_RIGID_MATCHING_H
 #include <Eigen/Core>
+#include <closest_rotation.h>
+
 // Given a set of source points X and corresponding target points P, find the
 // optimal rigid transformation (R,t) that aligns X to P, minimizing the
 // matching energy:
@@ -19,5 +21,4 @@ void point_to_point_rigid_matching(
   const Eigen::MatrixXd & P,
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t);
-#endif
-
+#endif

include/point_triangle_distance.h

@@ -1,6 +1,8 @@
 #ifndef POINT_TRIANGLE_DISTANCE_H
 #define POINT_TRIANGLE_DISTANCE_H
 #include <Eigen/Core>
+#include <limits>
+
 // Compute the distance `d` between a given point `x` and the closest point `p` on
 // a given triangle with corners `a`, `b`, and `c`.
 //

include/random_points_on_mesh.h

@@ -1,6 +1,9 @@
 #ifndef RANDOM_POINTS_ON_MESH_H
 #define RANDOM_POINTS_ON_MESH_H
 #include <Eigen/Core>
+#include <igl/doublearea.h>
+#include <igl/cumsum.h>
+#include <cstdlib>
 // RANDOM_POINTS_ON_MESH Randomly sample a mesh (V,F) n times.
 //
 // Inputs:

src/closest_rotation.cpp

@@ -4,6 +4,15 @@ 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_star;
+  double dt = (U*V.transpose()).determinant();
+
+  omega_star << 1, 0, 0,
+                0, 1, 0,
+                0, 0, dt;
+
+  R = U*omega_star*V.transpose();
+}

src/hausdorff_lower_bound.cpp

@@ -7,6 +7,11 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
-}
+  Eigen::MatrixXd pointsX, P, N;
+  Eigen::VectorXd D;
+
+  random_points_on_mesh(n, VX, FX, pointsX);
+  point_mesh_distance(pointsX, VY, FY, D, P, N);
+
+  return D.maxCoeff();
+}

src/icp_single_iteration.cpp

@@ -10,7 +10,19 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   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 pointsX, P, N;
+  Eigen::VectorXd D;
+
+  random_points_on_mesh(num_samples, VX, FX, pointsX);
+  point_mesh_distance(pointsX, VY, FY, D, P, N);
+
+  if(method == ICP_METHOD_POINT_TO_POINT){
+    point_to_point_rigid_matching(pointsX, P, R, t);
+  }
+  else{
+    point_to_plane_rigid_matching(pointsX, P, N, R, t);
+  }
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,5 @@
 #include "point_mesh_distance.h"
+// #include <iostream>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +9,43 @@ void point_mesh_distance(
   Eigen::MatrixXd & P,
   Eigen::MatrixXd & N)
 {
-  // Replace with your code
   P.resizeLike(X);
+  D = Eigen::VectorXd(X.rows());
   N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
-  for(int i = 0;i<X.rows();i++) P.row(i) = VY.row(i%VY.rows());
+
+  // Compute face normals
+  Eigen::MatrixXd FN;
+  igl::per_face_normals(VY,FY,Eigen::Vector3d(1,1,1).normalized(),FN);  
+
+  for(int i = 0;i<X.rows();i++){
+    double d = std::numeric_limits<double>::max(), tmp_d;
+    Eigen::RowVector3d closest_point, tmp_point, b;
+    int min_index = 0;
+
+    for(int j = 0;j<FY.rows();j++){
+      point_triangle_distance(
+        X.row(i), 
+        VY.row(FY(j, 0)),
+        VY.row(FY(j, 1)),
+        VY.row(FY(j, 2)),
+        tmp_d, tmp_point
+      );
+
+      // std::cout << i << "," << j << "," << tmp_d << "," << d << "\n";
+
+      if (tmp_d < d){
+        d = tmp_d;
+        closest_point = tmp_point;
+        min_index = j;
+      }
+    }
+
+    P.row(i) = closest_point;
+    D(i) = d;
+    N.row(i) = FN.row(min_index);
+
+    // std::cout << min_index << "," << d << "\n";
+  }
+
   D = (X-P).rowwise().norm();
-}
+}

src/point_to_plane_rigid_matching.cpp

@@ -7,7 +7,52 @@ 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 k = X.rows();
+
+  // Set A to zeros
+  Eigen::MatrixXd A;
+  A = Eigen::MatrixXd::Zero(3*k, 6);
+
+  // Build A
+  // Put all the equations corresponding to the rows
+  // together, making it easy to construct A
+  A.col(0).segment(k, k) = -X.col(2);
+  A.col(0).segment(2*k, k) = X.col(1);
+  A.col(1).segment(0, k) = X.col(2);
+  A.col(1).segment(2*k, k) = -X.col(0);
+  A.col(2).segment(0, k) = -X.col(1);
+  A.col(2).segment(k, k) = X.col(0);
+  A.col(3).segment(0, k) = Eigen::VectorXd::Ones(k);
+  A.col(4).segment(k, k) = Eigen::VectorXd::Ones(k);
+  A.col(5).segment(2*k, k) = Eigen::VectorXd::Ones(k);
+
+  // Build X-P
+  Eigen::VectorXd PX;
+  PX = Eigen::VectorXd::Zero(3*k, 1);
+  PX.segment(0, k) = X.col(0) - P.col(0);
+  PX.segment(k, k) = X.col(1) - P.col(1);
+  PX.segment(2*k, k) = X.col(2) - P.col(2);
+
+  // Build N_diag
+  Eigen::MatrixXd N_diag;
+  N_diag = Eigen::MatrixXd::Zero(k, 3*k);
+  N_diag.block(0,0,k,k) = N.col(0).asDiagonal();
+  N_diag.block(0,k,k,k) = N.col(1).asDiagonal();
+  N_diag.block(0,2*k,k,k) = N.col(2).asDiagonal();
+
+  A = N_diag * A;
+  PX = N_diag * PX;
+
+  // Least squares solution
+  Eigen::Matrix3d M, residual;
+  Eigen::VectorXd u;
+  u = (A.transpose() * A).inverse() * (-A.transpose() * PX);
+  t = Eigen::RowVector3d(u(3),u(4),u(5));
+
+  residual << 0, -u(2), u(1),
+       u(2), 0, -u(0),
+       -u(1), u(0), 0;
+
+  M = Eigen::Matrix3d::Identity() - residual;
+  closest_rotation(M, R);
 }

src/point_to_point_rigid_matching.cpp

@@ -7,8 +7,14 @@ void point_to_point_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
-}
+  Eigen::RowVector3d X_mean = X.colwise().mean();
+  Eigen::MatrixXd X_cor = X.rowwise() - X_mean;
 
+  Eigen::RowVector3d P_mean = P.colwise().mean();
+  Eigen::MatrixXd P_cor = P.rowwise() - P_mean;
+
+  Eigen::MatrixXd M = (P_cor.transpose() * X_cor).transpose();
+  closest_rotation(M, R);
+  t = (P_mean.transpose() - R*X_mean.transpose()).transpose();
+  // Formula is according to column vectors
+}

src/point_triangle_distance.cpp

@@ -8,7 +8,93 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
-}
+  // NOTE: Can be optimized if needed, doing lazily
+
+  // Seems like the proper algorithm using barycentric
+  // coordinates would need to project to two edges anyway
+  // (for obtuse triangles)
+
+  d = std::numeric_limits<double>::max();
+
+  Eigen::RowVector3d ba = b - a;
+  Eigen::RowVector3d ca = c - a;
+  Eigen::RowVector3d xa = x - a;
+  Eigen::RowVector3d n = ba.cross(ca);
+  n.normalize();
+
+  // find point on plane
+  double t = n.dot(xa);
+  Eigen::RowVector3d pop = x + t*n;
+
+  // find barycentric coordinates
+  double d00 = ba.dot(ba);
+  double d01 = ba.dot(ca);
+  double d11 = ca.dot(ca);
+  double d20 = xa.dot(ba);
+  double d21 = xa.dot(ca);
+
+  double denom = d00 * d11 - d01 * d01;
+
+  double beta = (d11 * d20 - d01 * d21) / denom;
+  double gamma = (d00 * d21 - d01 * d20) / denom;
+  double alpha = 1 - beta - gamma; 
+
+  if(alpha > 0 && alpha < 1 && beta > 0 && beta < 1 && gamma > 0 && gamma < 1){
+    p = alpha * a + beta * b + gamma * c;
+    d = (p - x).norm();
+    return;
+  }
+
+  // check three vertices
+  double tmp = (x-a).norm();
+  if (d > tmp){
+    d = tmp;
+    p = a;
+  }
+
+  tmp = (x-b).norm();
+  if (d > tmp){
+    d = tmp;
+    p = b;
+  }
+
+  tmp = (x-c).norm();
+  if (d > tmp){
+    d = tmp;
+    p = c;
+  }
+
+  // check three edges
+  // ab
+  t = (b - a).dot(x - a)/(b-a).dot(b-a);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = a + t*(b-a);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+
+  // bc
+  t = (c - b).dot(x - b)/(c-b).dot(c-b);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = b + t*(c-b);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+
+  // ca
+  t = (a - c).dot(x - c)/(a-c).dot(a-c);
+  if (t > 0 && t < 1){
+    Eigen::RowVector3d tmp_p = c + t*(a-c);
+    tmp = (tmp_p - x).norm();
+    if (d > tmp){
+      p = tmp_p;
+      d = tmp;
+    }
+  }
+}