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

include/closest_rotation.h

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

main.cpp

@@ -85,7 +85,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,14 @@
 #include "closest_rotation.h"
 
 void closest_rotation(
-  const Eigen::Matrix3d & M,
-  Eigen::Matrix3d & R)
+    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 V = svd.matrixV().transpose();
+    Eigen::MatrixXd UV = U * V;
+    Eigen::Vector3d omega(1, 1, UV.determinant());
+    Eigen::Matrix3d Omega = omega.asDiagonal();
+    R = U * Omega * V;
 }

src/hausdorff_lower_bound.cpp

@@ -1,12 +1,17 @@
 #include "hausdorff_lower_bound.h"
+#include "point_mesh_distance.h"
+#include "random_points_on_mesh.h"
 
 double hausdorff_lower_bound(
-  const Eigen::MatrixXd & VX,
-  const Eigen::MatrixXi & FX,
-  const Eigen::MatrixXd & VY,
-  const Eigen::MatrixXi & FY,
-  const int n)
+    const Eigen::MatrixXd & VX,
+    const Eigen::MatrixXi & FX,
+    const Eigen::MatrixXd & VY,
+    const Eigen::MatrixXi & FY,
+    const int n)
 {
-  // Replace with your code
-  return 0;
+    Eigen::VectorXd D;
+    Eigen::MatrixXd X, P, N;
+    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,30 @@
 #include "icp_single_iteration.h"
+#include "point_mesh_distance.h"
+#include "point_to_plane_rigid_matching.h"
+#include "point_to_point_rigid_matching.h"
+#include "random_points_on_mesh.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,101 @@
 #include "point_mesh_distance.h"
+#include "point_triangle_distance.h"
+#include <unsupported/Eigen/BVH>
+#include <igl/per_face_normals.h> 
+#include <vector>
+
+struct Triangle 
+{
+    // each row is one vertex on this triangle
+    Eigen::Matrix3d vertices;
+
+    // row in FY that defines this triangle
+    int frow;
+};
+
+Eigen::AlignedBox3d bounding_box(Triangle t) 
+{
+        Eigen::Vector3d corner1 = t.vertices.colwise().minCoeff();
+        Eigen::Vector3d corner2 = t.vertices.colwise().maxCoeff();
+        return Eigen::AlignedBox3d(corner1, corner2);
+}
+
+struct PointTriangleMinimizer
+{
+    typedef double Scalar;
+    Eigen::RowVector3d query_point;
+
+    double shortest_dist = -1;
+    Eigen::RowVector3d closest_point;
+
+    // the row in FY that defines the triangle on which closest_point lies
+    int frow;
+
+    PointTriangleMinimizer(Eigen::RowVector3d x) 
+    {
+        query_point = x;
+    }
+
+    // Shortest distance from query_point to bounding box b
+    double minimumOnVolume(const Eigen::AlignedBox3d & b) 
+    {
+        Eigen::Matrix3d D;
+        D << b.corner(b.BottomLeftFloor) - query_point.transpose(),
+            Eigen::Vector3d::Zero(),
+            query_point.transpose() - b.corner(b.TopRightCeil);
+        return D.rowwise().maxCoeff().norm();
+    }
+
+    // Shortest distance from query_point to triangle t
+    double minimumOnObject(const Triangle & t) 
+    {
+        double d;
+        Eigen::RowVector3d p;
+        point_triangle_distance(query_point, t.vertices.row(0), t.vertices.row(1), t.vertices.row(2), d, p);
+
+        if (d < shortest_dist || shortest_dist == -1) 
+        {
+            shortest_dist = d;
+            closest_point = p;
+            frow = t.frow;
+        }
+
+        return d;
+    }
+};
+
 
 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();
+    Eigen::MatrixXd all_normals;
+    igl::per_face_normals(VY, FY, all_normals);
+
+    D.resize(X.rows());
+    P.resize(X.rows(), 3);
+    N.resize(X.rows(), 3);
+
+    std::vector<Triangle> triangles;
+    for (int j = 0; j < FY.rows(); j++) {
+        Eigen::RowVector3i fy = FY.row(j);
+        Triangle t;
+        t.vertices << VY.row(fy[0]), VY.row(fy[1]), VY.row(fy[2]);
+        t.frow = j;
+        triangles.push_back(t);
+    }
+
+    Eigen::KdBVH<double, 3, Triangle> triangle_tree(triangles.begin(), triangles.end());
+
+    for (int i = 0; i < X.rows(); i++) 
+    {
+        PointTriangleMinimizer minimizer(X.row(i));
+        D[i] = Eigen::BVMinimize(triangle_tree, minimizer);
+        P.row(i) = minimizer.closest_point;
+        N.row(i) = all_normals.row(minimizer.frow);
+    }
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,13 +1,42 @@
+#include "closest_rotation.h"
 #include "point_to_plane_rigid_matching.h"
+#include <Eigen/SVD>
 
 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::MatrixXd XN(X.rows(), 3);
+    for (int i = 0; i < X.rows(); i++) 
+    {
+        Eigen::RowVector3d x = X.row(i);
+        Eigen::RowVector3d n = N.row(i);
+        XN.row(i) = x.cross(n);
+    }
+
+    Eigen::MatrixXd A(P.rows(), 6);
+    A << XN, N;
+
+    Eigen::VectorXd b(X.rows());
+    for (int i = 0; i < X.rows(); i++) {
+        b[i] = -(X.row(i) - P.row(i)).dot(N.row(i));
+    }
+
+    Eigen::VectorXd u = A.colPivHouseholderQr().solve(b);
+    Eigen::Matrix3d U;
+
+    // first three elements of u define the angles of rotation
+    // about the x, y, and z axis, in that order
+    U << Eigen::RowVector3d(0, -u[2], u[1]),
+        Eigen::RowVector3d(u[2], 0, -u[0]),
+        Eigen::RowVector3d(-u[1], u[0], 0);
+
+    Eigen::MatrixXd M = Eigen::Matrix3d::Identity() + U;
+    closest_rotation(M, R);
+
+    // last three elements of u define the translation
+    t << u[3], u[4], u[5];
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,20 @@
+#include "closest_rotation.h"
 #include "point_to_point_rigid_matching.h"
 #include <igl/polar_svd.h>
 
 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::Vector3d x_centroid = X.colwise().mean();
+    Eigen::Vector3d p_centroid = P.colwise().mean();
+
+    Eigen::MatrixXd X_bar = (X.rowwise() - x_centroid.transpose()).eval();
+    Eigen::MatrixXd P_bar = (P.rowwise() - p_centroid.transpose()).eval();
+
+    closest_rotation(P_bar.transpose() * X_bar, R);
+    t = p_centroid - R * x_centroid;
 }