HW02 Submit - Vineeth Bhaskara

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,29 @@
 #include "closest_rotation.h"
+#include <Eigen/SVD>
+#include <Eigen/LU>
+#include <iostream>
 
 void closest_rotation(
   const Eigen::Matrix3d & M,
   Eigen::Matrix3d & R)
 {
   // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+  // R = Eigen::Matrix3d::Identity();
+
+  // Compute the SVD of the given matrix M
+
+  // exact solver - slow
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+
+  Eigen::Matrix3d U = svd.matrixU();
+  Eigen::Matrix3d V = svd.matrixV();
+
+  double det_uv = (U * V.transpose()).determinant();
+
+  Eigen::Matrix3d Omega = Eigen::Matrix3d::Identity();
+  Omega(2,2) = det_uv;
+
+  R = U * Omega * 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,
@@ -8,5 +10,26 @@ double hausdorff_lower_bound(
   const int n)
 {
   // Replace with your code
-  return 0;
+
+  // Sample points from X
+  Eigen::MatrixXd sampled_X(n, 3);
+
+  random_points_on_mesh(
+  n,
+  VX,
+  FX,
+  sampled_X);
+
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P, N;
+
+  point_mesh_distance(
+  sampled_X,
+  VY,
+  FY,
+  D,
+  P,
+  N);
+
+  return D.maxCoeff();
 }

src/icp_single_iteration.cpp

@@ -1,4 +1,9 @@
 #include "icp_single_iteration.h"
+#include "point_to_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+#include <iostream>
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -13,4 +18,44 @@ void icp_single_iteration(
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+
+  // Sample points from X
+  Eigen::MatrixXd sampled_X(num_samples, 3);
+
+  random_points_on_mesh(
+  num_samples,
+  VX,
+  FX,
+  sampled_X);
+
+  Eigen::VectorXd D;
+  Eigen::MatrixXd P, N;
+
+  point_mesh_distance(
+  sampled_X,
+  VY,
+  FY,
+  D,
+  P,
+  N);
+
+
+  if (method == ICP_METHOD_POINT_TO_POINT) {
+
+    point_to_point_rigid_matching(
+    sampled_X,
+    P,
+    R,
+    t);
+
+  } else if (method == ICP_METHOD_POINT_TO_PLANE) {
+
+    point_to_plane_rigid_matching(
+    sampled_X,
+    P,
+    N,
+    R,
+    t);
+
+  }
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,6 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <igl/per_face_normals.h>
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -9,8 +11,68 @@ void point_mesh_distance(
   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();
+
   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();
+  D.resize(X.rows());
+  N.resizeLike(X);
+
+  for (int i=0; i<X.rows(); i++) {
+
+    double least_d = 0;
+    Eigen::RowVector3d least_p;
+    int least_faceidx = 0;
+
+    for (int j=0; j<FY.rows(); j++) {
+
+      double d_tmp;
+      Eigen::RowVector3d p_tmp;
+
+      point_triangle_distance(X.row(i).transpose(), 
+                              VY.row(FY(j, 0)).transpose(),
+                              VY.row(FY(j, 1)).transpose(),
+                              VY.row(FY(j, 2)).transpose(),
+                              d_tmp,
+                              p_tmp);
+
+      if (j==0) {
+        least_d = d_tmp;
+        least_p(0) = p_tmp(0); least_p(1) = p_tmp(1); least_p(2) = p_tmp(2); 
+        least_faceidx = j;
+      } else {
+        if (d_tmp < least_d) {
+          least_d = d_tmp;
+          least_p(0) = p_tmp(0); least_p(1) = p_tmp(1); least_p(2) = p_tmp(2); 
+          least_faceidx = j;
+        }
+      }
+
+
+    }
+
+    D(i) = least_d;
+    P(i, 0) = least_p(0); P(i, 1) = least_p(1); P(i, 2) = least_p(2);
+
+    Eigen::Vector3d LeastTriangleNormal;
+    // Build V and F based on given coordinates
+    Eigen::MatrixXd V_dummy(3,3);
+    V_dummy.row(0) = VY.row(FY(least_faceidx, 0));
+    V_dummy.row(1) = VY.row(FY(least_faceidx, 1));
+    V_dummy.row(2) = VY.row(FY(least_faceidx, 2));
+
+    Eigen::MatrixXd F_dummy(1,3);
+    F_dummy << 0, 1, 2;
+
+    Eigen::MatrixXd N_dummy(1,3);
+    igl::per_face_normals(V_dummy, F_dummy, N_dummy);
+
+    LeastTriangleNormal = N_dummy.row(0).normalized();
+
+    N(i, 0) = LeastTriangleNormal(0); N(i, 1) = LeastTriangleNormal(1); N(i, 2) = LeastTriangleNormal(2);
+
+  }
+
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
+#include <Eigen/LU>
+#include <Eigen/Dense>
+#include "closest_rotation.h"
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -10,4 +13,67 @@ void point_to_plane_rigid_matching(
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+
+
+  Eigen::MatrixXd N_x_diag(N.rows(), N.rows());
+  Eigen::MatrixXd N_y_diag(N.rows(), N.rows());
+  Eigen::MatrixXd N_z_diag(N.rows(), N.rows());
+  N_x_diag = Eigen::MatrixXd(N.col(0).asDiagonal());
+  N_y_diag = Eigen::MatrixXd(N.col(1).asDiagonal());
+  N_z_diag = Eigen::MatrixXd(N.col(2).asDiagonal());
+
+
+  Eigen::MatrixXd N_diag_concat(N.rows(), 3*N.rows());
+  N_diag_concat << N_x_diag, N_y_diag, N_z_diag;
+
+
+  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3*N.rows(), 6);
+
+  Eigen::VectorXd X_minus_P(3*N.rows());
+
+  // iterate over all x
+  for (int i=0; i<X.rows(); i++) {
+
+    X_minus_P(i) = X(i, 0) - P(i, 0);
+    X_minus_P(X.rows()+i) = X(i, 1) - P(i, 1);
+    X_minus_P(2*X.rows() + i) = X(i, 2) - P(i, 2);
+
+    A(X.rows() + i, 0) = -1.0 * X(i, 2);
+    A(2*X.rows() + i, 0) = X(i, 1);
+    A(i, 1) = X(i, 2);
+    A(2*X.rows() + i, 1) = -1.0 * X(i, 0);
+    A(i, 2) = -1.0 * X(i, 1);
+    A(X.rows() + i, 2) = X(i, 0);
+
+    A(i, 3) = 1;
+    A(X.rows() + i, 4) = 1;
+    A(2*X.rows() + i, 5) = 1;
+  }
+
+  Eigen::MatrixXd A_new =  N_diag_concat * A;
+  Eigen::VectorXd X_minus_P_new = N_diag_concat * X_minus_P;
+
+  Eigen::VectorXd U(6);
+
+
+  U = (A_new.transpose()*A_new).colPivHouseholderQr().solve(-1.0*A_new.transpose()*X_minus_P_new);
+
+
+  // assign answer for t
+  t(0) = U(3); t(1) = U(4); t(2) = U(5);
+
+  // Create a matrix M
+  Eigen::Matrix3d M = Eigen::Matrix3d::Identity();
+  M(0, 1) = -1.0 * U(2);
+  M(0, 2) = U(1);
+  M(1, 0) = U(2);
+  M(1, 2) = -1.0 * U(0);
+  M(2, 0) = -1.0 * U(1);
+  M(2, 1) = U(0);
+
+  // now find the closest rotation
+  closest_rotation(
+  M,
+  R);
+
 }

src/point_to_point_rigid_matching.cpp

@@ -1,6 +1,8 @@
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
+#include "closest_rotation.h"
 
+// closed form solution
 void point_to_point_rigid_matching(
   const Eigen::MatrixXd & X,
   const Eigen::MatrixXd & P,
@@ -10,5 +12,29 @@ void point_to_point_rigid_matching(
   // Replace with your code
   R = Eigen::Matrix3d::Identity();
   t = Eigen::RowVector3d::Zero();
+
+  Eigen::Matrix3d M;
+
+  // First find the average point in P and X
+  Eigen::Vector3d P_avg = P.colwise().mean();
+  Eigen::Vector3d X_avg = X.colwise().mean();
+
+
+
+
+  Eigen::MatrixXd X_bar = X.rowwise() - X_avg.transpose();
+  Eigen::MatrixXd P_bar = P.rowwise() - P_avg.transpose();
+
+
+
+  M = P_bar.transpose() * X_bar;
+
+
+  closest_rotation(
+  M,
+  R);
+
+
+  t = (P_avg - R * X_avg).transpose();
 }