HW2

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::Matrix3d> svd(M, Eigen::DecompositionOptions::ComputeFullU | Eigen::DecompositionOptions::ComputeFullV);
+
+    Eigen::Matrix3d U = svd.matrixU();
+    Eigen::Matrix3d V = svd.matrixV();
+    Eigen::Matrix3d O;
+    O << 1, 0, 0,
+         0, 1, 0,
+         0, 0, (U * V.transpose()).determinant();
+
+    R = U * O * V.transpose();
 }

src/hausdorff_lower_bound.cpp

@@ -1,4 +1,7 @@
 #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 +10,15 @@ double hausdorff_lower_bound(
   const Eigen::MatrixXi & FY,
   const int n)
 {
-  // Replace with your code
-  return 0;
+    Eigen::MatrixXd X;
+    random_points_on_mesh(n,VX,FX,X);
+
+    // Point to mesh distances from point cloud X to Y
+    Eigen::VectorXd D;
+    Eigen::MatrixXd P;
+    Eigen::MatrixXd N;
+    point_mesh_distance(X, VY, FY, D, P, N);
+
+    // Return the largest distance
+    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_point_rigid_matching.h"
+#include "point_to_plane_rigid_matching.h"
 
 void icp_single_iteration(
   const Eigen::MatrixXd & VX,
@@ -10,7 +14,20 @@ void icp_single_iteration(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    // Sample source mesh
+    Eigen::MatrixXd X;
+    random_points_on_mesh(num_samples,VX,FX,X);
+
+    // Point to mesh distances from point cloud X to Y
+    Eigen::VectorXd D;
+    Eigen::MatrixXd P;
+    Eigen::MatrixXd N;
+    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,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,
@@ -8,9 +10,34 @@ 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();
+    P.resizeLike(X);
+    N = Eigen::MatrixXd::Zero(X.rows(), X.cols());
+
+    Eigen::MatrixXd N_all;
+    igl::per_face_normals(VY, FY, Eigen::Vector3d(1, 1, 1).normalized(), N_all);
+
+    for (int i = 0; i < X.rows(); i++) {
+
+        double d = std::numeric_limits<double>::max();
+
+        for (int j = 0; j < FY.rows(); j++) {
+            const Eigen::RowVector3d x = X.row(i);
+
+            const Eigen::Vector3d a = VY.row(FY(j, 0));
+            const Eigen::Vector3d b = VY.row(FY(j, 1));
+            const Eigen::Vector3d c = VY.row(FY(j, 2));
+
+            double d_temp;
+            Eigen::RowVector3d p_temp;
+            point_triangle_distance(x, a, b, c, d_temp, p_temp);
+
+            if (d_temp < d) {
+                d = d_temp;
+                P.row(i) = p_temp;
+                N.row(i) = N_all.row(j);
+            }
+        }
+    }
+
+    D = (X - P).rowwise().norm();
 }

src/point_to_plane_rigid_matching.cpp

@@ -1,4 +1,6 @@
 #include "point_to_plane_rigid_matching.h"
+#include "closest_rotation.h"
+#include <Eigen/LU>
 
 void point_to_plane_rigid_matching(
   const Eigen::MatrixXd & X,
@@ -7,7 +9,46 @@ void point_to_plane_rigid_matching(
   Eigen::Matrix3d & R,
   Eigen::RowVector3d & t)
 {
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+    const int k = X.rows();
+
+    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * k, 6);
+
+    A.block(0, 1, k, 1) = X.col(2);
+    A.block(0, 2, k, 1) = -X.col(1);
+    A.block(0, 3, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+    A.block(k, 0, k, 1) = -X.col(2);
+    A.block(k, 2, k, 1) = X.col(0);
+    A.block(k, 4, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+    A.block(2 * k, 0, k, 1) = X.col(1);
+    A.block(2 * k, 1, k, 1) = -X.col(0);
+    A.block(2 * k, 5, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+
+    Eigen::VectorXd x_minus_p(3 * k);
+    x_minus_p.segment(0, k) = X.col(0) - P.col(0);
+    x_minus_p.segment(k, k) = X.col(1) - P.col(1);
+    x_minus_p.segment(2 * k, k) = X.col(2) - P.col(2);
+
+    Eigen::MatrixXd diagN(k, 3*k);
+    diagN << Eigen::MatrixXd(N.col(0).asDiagonal()),
+          Eigen::MatrixXd(N.col(1).asDiagonal()),
+          Eigen::MatrixXd(N.col(2).asDiagonal());
+
+    A = diagN * A;
+    x_minus_p = diagN * x_minus_p;
+
+    Eigen::VectorXd u(6);
+    u = (A.transpose() * A).inverse() * (-A.transpose() * x_minus_p);
+
+    const double alpha = u(0);
+    const double beta = u(1);
+    const double gamma = u(2);
+
+    // This is different from the README; see issue #9
+    Eigen::Matrix3d M;
+    M << 1, gamma, -beta,
+         -gamma, 1, alpha,
+         beta, -alpha, 1;
+
+    closest_rotation(M, R);
+    t << u(3), u(4), u(5);
 }

src/point_to_point_rigid_matching.cpp

@@ -1,14 +1,48 @@
 #include "point_to_point_rigid_matching.h"
-#include <igl/polar_svd.h>
+#include "closest_rotation.h"
+#include <Eigen/LU>
+
+#include <iostream>
 
 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();
+    const int k = X.rows();
+
+    Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3 * k, 6);
+
+    A.block(0, 1, k, 1) = X.col(2);
+    A.block(0, 2, k, 1) = -X.col(1);
+    A.block(0, 3, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+    A.block(k, 0, k, 1) = -X.col(2);
+    A.block(k, 2, k, 1) = X.col(0);
+    A.block(k, 4, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+    A.block(2 * k, 0, k, 1) = X.col(1);
+    A.block(2 * k, 1, k, 1) = -X.col(0);
+    A.block(2 * k, 5, k, 1) = Eigen::MatrixXd::Ones(k, 1);
+
+    Eigen::VectorXd x_minus_p(3 * k);
+    x_minus_p.segment(0, k) = X.col(0) - P.col(0);
+    x_minus_p.segment(k, k) = X.col(1) - P.col(1);
+    x_minus_p.segment(2 * k, k) = X.col(2) - P.col(2);
+
+    Eigen::VectorXd u(6);
+    u = (A.transpose() * A).inverse() * (-A.transpose() * x_minus_p);
+
+    const double alpha = u(0);
+    const double beta = u(1);
+    const double gamma = u(2);
+
+    // This is different from the README; see issue #9
+    Eigen::Matrix3d M;
+    M << 1, gamma, -beta,
+         -gamma, 1, alpha,
+         beta, -alpha, 1;
+
+    closest_rotation(M, R);
+    t << u(3), u(4), u(5);
 }
 

src/point_triangle_distance.cpp

@@ -8,7 +8,8 @@ void point_triangle_distance(
   double & d,
   Eigen::RowVector3d & p)
 {
-  // Replace with your code
-  d = 0;
-  p = a;
+  Eigen::RowVector3d center = (a + b + c) / 3;
+
+  d = (center - x).norm();
+  p = c;
 }

src/random_points_on_mesh.cpp

@@ -1,13 +1,49 @@
 #include "random_points_on_mesh.h"
+#include <igl/cumsum.h>
+#include <igl/doublearea.h>
+
+#include <random>
 
 void random_points_on_mesh(
   const int n,
   const Eigen::MatrixXd & V,
   const Eigen::MatrixXi & F,
   Eigen::MatrixXd & X)
 {
-  // REPLACE WITH YOUR CODE:
-  X.resize(n,3);
-  for(int i = 0;i<X.rows();i++) X.row(i) = V.row(i%V.rows());
+    Eigen::MatrixXd areas(F.rows(), 1);
+
+    igl::doublearea(V, F, areas);
+
+    Eigen::VectorXd C(F.rows());
+
+    igl::cumsum(areas, 1, C);
+
+    std::default_random_engine generator;
+    std::uniform_real_distribution<double> distribution(0.0, 1.0);
+
+    X.resize(n, 3);
+    for (int i = 0; i < X.rows(); i++) {
+
+        double gamma = distribution(generator);
+
+        int T=0;
+        while (C(T)/C(C.rows()-1) < gamma)
+            T++;
+
+        Eigen::Vector3d v1 = V.row(F(T, 0));
+        Eigen::Vector3d v2 = V.row(F(T, 1));
+        Eigen::Vector3d v3 = V.row(F(T, 2));
+
+        double alpha = distribution(generator);
+        double beta = distribution(generator);
+
+        if ((alpha + beta) > 1) {
+            alpha = 1 - alpha;
+            beta = 1 - beta;
+        }
+
+        Eigen::Vector3d x = v1 + alpha * (v2 - v1) + beta * (v3 - v1);
+        X.row(i) = x;
+    }
 }