Peiqi Wang HW2

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"(
@@ -33,9 +33,12 @@ int main(int argc, char *argv[])
   s        halve number of samples
 )";
 
+
   // predefined colors
   const Eigen::RowVector3d orange(1.0,0.7,0.2);
   const Eigen::RowVector3d blue(0.2,0.3,0.8);
+
+  // sets X, Y surface, i.e. V = [VX, VY], F = [FX, FY], and C
   const auto & set_meshes = [&]()
   {
     // Concatenate meshes into one big mesh
@@ -51,6 +54,9 @@ int main(int argc, char *argv[])
     C.bottomLeftCorner(FY.rows(),3).rowwise() = blue;
     viewer.data.set_colors(C);
   };
+  // sets X, P where
+  //  X are randomly sampled points
+  //  P are closest point from X to Y
   const auto & set_points = [&]()
   {
     Eigen::MatrixXd X,P;
@@ -85,7 +91,7 @@ int main(int argc, char *argv[])
       Eigen::RowVector3d t;
       icp_single_iteration(VX,FX,VY,FY,num_samples,method,R,t);
       // Apply transformation to source mesh
-      VX = ((VX*R).rowwise() + t).eval();
+      VX = ((VX*R.transpose()).rowwise() + t).eval();
       set_meshes();
       if(show_samples)
       {

src/closest_rotation.cpp

@@ -1,9 +1,19 @@
 #include "closest_rotation.h"
 
+#include <Eigen/SVD>
+#include <Eigen/LU>
+
 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::ComputeFullU | Eigen::ComputeFullV);
+
+    Eigen::MatrixXd U = svd.matrixU();
+    Eigen::MatrixXd Vt = svd.matrixV().transpose();
+
+    R << 1, 0, 0,
+         0, 1, 0,
+         0, 0, (U * Vt).determinant();
+    R = (U * R) * Vt;
 }

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,17 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+
+    // Sample `n` points from `X`
+    Eigen::MatrixXd X;
+    random_points_on_mesh(n, VX, FX, X);
+
+    // Directed Hausdorff's lower bound is supremum of 
+    //      point-to-mesh distances for all sampled X
+    //      i.e. worst of bests
+    Eigen::VectorXd D;
+    Eigen::MatrixXd P, N;
+    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_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +14,24 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+
+    auto transform = [&R, &t](Eigen::MatrixXd& X) {
+        return (X * R.transpose()).rowwise() + t;
+    };
+
+    // Sample X, project X to mesh Y after transformation
+    //      P = P_Y(T(X)) \subset Y
+    Eigen::VectorXd D;
+    Eigen::MatrixXd X, P, N;
+    random_points_on_mesh(num_samples,VX,FX,X);
+    point_mesh_distance(X, VY, FY, D, P, N);
+
+    // Update rigid transformation to match X and P
+    R = Eigen::Matrix3d::Identity();
+    t = Eigen::RowVector3d::Zero();
+    if (method == ICP_METHOD_POINT_TO_POINT) {
+        point_to_point_rigid_matching(X, P, R, t);
+    } else if (method == ICP_METHOD_POINT_TO_PLANE) {
+        point_to_plane_rigid_matching(X, P, N, R, t);
+    }
 }

src/point_mesh_distance.cpp

@@ -1,4 +1,11 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+
+#include <igl/per_face_normals.h>
+#include <limits>
+#include <vector>
+#include <algorithm>
+
 
 void point_mesh_distance(
   const Eigen::MatrixXd & X,
@@ -8,9 +15,40 @@ 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();
+    D.resize(X.rows());
+    P.resizeLike(X);
+    N.resizeLike(X);
+
+    auto ny = FY.rows();
+
+    Eigen::MatrixXd NY;
+    igl::per_face_normals(VY, FY, Eigen::Vector3d(1,1,1).normalized(), NY);
+
+    int min_idx;
+    double d, min_d;
+    Eigen::RowVector3d p, closest_p;
+
+    for (int i = 0; i < X.rows(); ++i) {
+        auto x = X.row(i);
+        min_idx = 0;
+        min_d = std::numeric_limits<int>::max();
+
+        // Computes closest distance & points from `x` to every triangle `t`
+        // Find triangle that minimizes point-to-mesh distance
+        for (int j = 0; j < ny; ++j) {
+            auto t = FY.row(j);
+            point_triangle_distance(x, VY.row(t(0)), VY.row(t(1)), VY.row(t(2)), d, p);
+            if (d < min_d) {
+                min_d = d;
+                min_idx = j;
+                closest_p = p;
+            }
+        }
+
+        // Saves closest distance, closest point, and normal
+        D(i) = min_d;
+        P.row(i) = closest_p;
+        N.row(i) = NY.row(min_idx);
+    }
 }
+

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,7 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+
+#include <Eigen/Dense>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +10,39 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    // Construct matrix A
+    int k = X.rows();
+    Eigen::VectorXd u(6);
+    Eigen::MatrixXd A(3*k, 6);
+    Eigen::MatrixXd b(3*k, 1);
+    Eigen::MatrixXd dN(k, 3*k);
+
+    Eigen::VectorXd zeros = Eigen::VectorXd::Zero(k);
+    Eigen::VectorXd ones  = Eigen::VectorXd::Ones(k);
+
+    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;
+
+    b << X.col(0) - P.col(0),
+         X.col(1) - P.col(1),
+         X.col(2) - P.col(2);
+
+    dN << Eigen::MatrixXd(N.col(0).asDiagonal()), 
+          Eigen::MatrixXd(N.col(1).asDiagonal()),
+          Eigen::MatrixXd(N.col(2).asDiagonal());
+
+    A = dN * A;
+    b = dN * b;
+
+    // solve for u
+    u = (A.transpose()*A).inverse() * (-A.transpose()*b);
+
+    // fill R and t
+    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.tail(3);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,40 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include "closest_rotation.h"
+
+#include <Eigen/Dense>
 
 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();
+    // Construct matrix A
+    int k = X.rows();
+    Eigen::VectorXd u(6);
+    Eigen::MatrixXd A(3*k, 6);
+    Eigen::MatrixXd b(3*k, 1);
+
+    Eigen::VectorXd zeros = Eigen::VectorXd::Zero(k);
+    Eigen::VectorXd ones  = Eigen::VectorXd::Ones(k);
+
+    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;
+
+    b << X.col(0) - P.col(0),
+         X.col(1) - P.col(1),
+         X.col(2) - P.col(2);
+
+    // solve for u
+    u = (A.transpose()*A).inverse() * (-A.transpose()*b);
+
+    // fill R and t
+    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.tail(3);
 }
 

src/point_triangle_distance.cpp

@@ -1,5 +1,7 @@
 #include "point_triangle_distance.h"
 
+#include <Eigen/Geometry> 
+
 void point_triangle_distance(
   const Eigen::RowVector3d & x,
   const Eigen::RowVector3d & a,
@@ -8,7 +10,67 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+    // Reference 
+    //  http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.479.8237&rep=rep1&type=pdf
+
+
+    // Find point `q` on plane defined by a point `p` and normal `n` {z: (z-p) * n = 0}
+    //      such that `q` is the (orthogonally) projected point of `x`
+    auto project = [](
+        const Eigen::RowVector3d& p,
+        const Eigen::RowVector3d& n,
+        const Eigen::RowVector3d& x) {
+            return x - ((x - p).dot(n) / (n.squaredNorm())) * n;
+        };
+
+    auto n = (b-a).cross(c-a);
+    p = project(a, n, x);
+
+    // Find line connecting center of triangle and the vertices
+    auto va = (a-c).normalized() + (a-b).normalized();
+    auto vb = (b-c).normalized() + (b-a).normalized();
+    auto vc = (c-b).normalized() + (c-a).normalized();
+
+    // Find location of projected point `p`
+    //  In particular, `p` is closest to edge represented by `uv`
+    auto fa = (va.cross(p-a)).dot(n);
+    auto fb = (vb.cross(p-b)).dot(n);
+    auto fc = (vc.cross(p-c)).dot(n);
+
+    int ui, vi;
+    if (fa > 0 && fb < 0) {
+        ui = 0; vi = 1;
+    } else if (fb > 0 && fc < 0) {
+        ui = 1; vi = 2;
+    } else if (fc > 0 && fa < 0) {
+        ui = 2; vi = 0;
+    }
+
+    auto get = [&](int i){ return (i == 0) ? a : ((i == 1) ? b : c); };
+    auto u = get(ui);
+    auto v = get(vi);
+
+    if ((v-p).cross(u-p).dot(n) > 0) {
+        // project `p` to the closest edge `uv` if `p` outside triangle
+        //      the closest point is either a projected point `q` on edge `uv`
+        //      or the vertices `u` or `v`
+        auto n2 = ((v-p).cross(u-p)).cross(u-v);
+        auto q = project(u, n2, p);
+        auto t = (q-u).dot(v-u) / (v-u).dot(v-u);
+
+        if (t >= 0 && t <= 1) {
+            d = sqrt((p-q).squaredNorm() + (x-p).squaredNorm());
+            p = q;
+        } else if (t < 0) {
+            d = sqrt((p-u).squaredNorm() + (x-p).squaredNorm());
+            p = u;
+        } else {
+            d = sqrt((p-v).squaredNorm() + (x-p).squaredNorm());
+            p = v;
+        }
+    } else {
+        // `p` is the closest point on the triangle if `p` inside triangle
+        d = (x-p).norm();
+    }
+
 }