wenzheng chen 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

@@ -3,27 +3,27 @@
 #include "random_points_on_mesh.h"
 #include "point_mesh_distance.h"
 #include <igl/read_triangle_mesh.h>
-#include <igl/viewer/Viewer.h>
+#include <igl/opengl/glfw/Viewer.h>
 #include <Eigen/Core>
 #include <string>
 #include <iostream>
 
-int main(int argc, char *argv[])
-{
-  // Load input meshes
-  Eigen::MatrixXd OVX,VX,VY;
-  Eigen::MatrixXi FX,FY;
-  igl::read_triangle_mesh(
-    (argc>1?argv[1]:"../shared/data/max-registration-partial.obj"),OVX,FX);
-  igl::read_triangle_mesh(
-    (argc>2?argv[2]:"../shared/data/max-registration-complete.obj"),VY,FY);
+int main(int argc, char *argv[]) {
+	// Load input meshes
+	Eigen::MatrixXd OVX, VX, VY;
+	Eigen::MatrixXi FX, FY;
+	igl::read_triangle_mesh(
+			(argc > 1 ? argv[1] : "./max-registration-partial.obj"), OVX, FX);
+	igl::read_triangle_mesh(
+			(argc > 2 ? argv[2] : "./max-registration-complete.obj"), VY, FY);
 
-  int num_samples = 100;
-  bool show_samples = true;
-  ICPMethod method = ICP_METHOD_POINT_TO_POINT;
+	int num_samples = 100;
+	bool show_samples = true;
+	ICPMethod method = ICP_METHOD_POINT_TO_POINT;
 
-  igl::viewer::Viewer viewer;
-  std::cout<<R"(
+	igl::opengl::glfw::Viewer viewer;
+	std::cout
+			<< R"(
   [space]  toggle animation
   H,h      print lower bound on directed Hausdorff distance from X to Y
   M,m      toggle between point-to-point and point-to-plane methods
@@ -33,111 +33,111 @@ 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);
-  const auto & set_meshes = [&]()
-  {
-    // Concatenate meshes into one big mesh
-    Eigen::MatrixXd V(VX.rows()+VY.rows(),VX.cols());
-    V << VX, VY;
-    Eigen::MatrixXi F(FX.rows()+FY.rows(),FX.cols());
-    F<<FX,FY.array()+VX.rows();
-    viewer.data.clear();
-    viewer.data.set_mesh(V,F);
-    // Assign orange and blue colors to each mesh's faces
-    Eigen::MatrixXd C(F.rows(),3);
-    C.topLeftCorner(FX.rows(),3).rowwise() = orange;
-    C.bottomLeftCorner(FY.rows(),3).rowwise() = blue;
-    viewer.data.set_colors(C);
-  };
-  const auto & set_points = [&]()
-  {
-    Eigen::MatrixXd X,P;
-    random_points_on_mesh(num_samples,VX,FX,X);
-    Eigen::VectorXd D;
-    Eigen::MatrixXd N;
-    point_mesh_distance(X,VY,FY,D,P,N);
-    Eigen::MatrixXd XP(X.rows()+P.rows(),3);
-    XP<<X,P;
-    Eigen::MatrixXd C(XP.rows(),3);
-    C.array().topRows(X.rows()).rowwise() = (1.-(1.-orange.array())*.8);
-    C.array().bottomRows(P.rows()).rowwise() = (1.-(1.-blue.array())*.4);
-    viewer.data.set_points(XP,C);
-    Eigen::MatrixXi E(X.rows(),2);
-    E.col(0) = Eigen::VectorXi::LinSpaced(X.rows(),0,X.rows()-1);
-    E.col(1) = Eigen::VectorXi::LinSpaced(X.rows(),X.rows(),2*X.rows()-1);
-    viewer.data.set_edges(XP,E,Eigen::RowVector3d(0.3,0.3,0.3));
-  };
-  const auto & reset = [&]()
-  {
-    VX = OVX;
-    set_meshes();
-  };
-  viewer.callback_pre_draw = [&](igl::viewer::Viewer &)->bool
-  {
-    if(viewer.core.is_animating)
-    {
-      ////////////////////////////////////////////////////////////////////////
-      // Perform single iteration of ICP method
-      ////////////////////////////////////////////////////////////////////////
-      Eigen::Matrix3d R;
-      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();
-      set_meshes();
-      if(show_samples)
-      {
-        set_points();
-      }
-    }
-    return false;
-  };
-  viewer.callback_key_pressed = 
-    [&](igl::viewer::Viewer &,unsigned char key,int)->bool
-  {
-    switch(key)
-    {
-      case ' ':
-        viewer.core.is_animating ^= 1;
-        break;
-      case 'H':
-      case 'h':
-        std::cout<<"D_{H}(X -> Y) >= "<<
-          hausdorff_lower_bound(VX,FX,VY,FY,num_samples)<<std::endl;
-        break;
-      case 'M':
-      case 'm':
-        method = (ICPMethod)((((int)method)+1)%((int)NUM_ICP_METHODS));
-        std::cout<< "point-to-"<<
-          (method==ICP_METHOD_POINT_TO_PLANE?"plane":"point")<<std::endl;
-        break;
-      case 'P':
-      case 'p':
-        show_samples ^= 1;
-        break;
-      case 'R':
-      case 'r':
-        reset();
-        if(show_samples) set_points();
-        break;
-      case 'S':
-        num_samples = (num_samples-1)*2;
-        break;
-      case 's':
-        num_samples = (num_samples/2)+1;
-        break;
-      default:
-        return false;
-    }
-    return true;
-  };
+	// predefined colors
+	const Eigen::RowVector3d orange(1.0, 0.7, 0.2);
+	const Eigen::RowVector3d blue(0.2, 0.3, 0.8);
+	const auto & set_meshes = [&]()
+	{
+		// Concatenate meshes into one big mesh
+			Eigen::MatrixXd V(VX.rows()+VY.rows(),VX.cols());
+			V << VX, VY;
+			Eigen::MatrixXi F(FX.rows()+FY.rows(),FX.cols());
+			F<<FX,FY.array()+VX.rows();
+			viewer.data().clear();
+			viewer.data().set_mesh(V,F);
+			// Assign orange and blue colors to each mesh's faces
+			Eigen::MatrixXd C(F.rows(),3);
+			C.topLeftCorner(FX.rows(),3).rowwise() = orange;
+			C.bottomLeftCorner(FY.rows(),3).rowwise() = blue;
+			viewer.data().set_colors(C);
+		};
+	const auto & set_points = [&]()
+	{
+		Eigen::MatrixXd X,P;
+		random_points_on_mesh(num_samples,VX,FX,X);
+		Eigen::VectorXd D;
+		Eigen::MatrixXd N;
+		point_mesh_distance(X,VY,FY,D,P,N);
+		Eigen::MatrixXd XP(X.rows()+P.rows(),3);
+		XP<<X,P;
+		Eigen::MatrixXd C(XP.rows(),3);
+		C.array().topRows(X.rows()).rowwise() = (1.-(1.-orange.array())*.8);
+		C.array().bottomRows(P.rows()).rowwise() = (1.-(1.-blue.array())*.4);
+		viewer.data().set_points(XP,C);
+		Eigen::MatrixXi E(X.rows(),2);
+		E.col(0) = Eigen::VectorXi::LinSpaced(X.rows(),0,X.rows()-1);
+		E.col(1) = Eigen::VectorXi::LinSpaced(X.rows(),X.rows(),2*X.rows()-1);
+		viewer.data().set_edges(XP,E,Eigen::RowVector3d(0.3,0.3,0.3));
+	};
+	const auto & reset = [&]()
+	{
+		VX = OVX;
+		set_meshes();
+	};
+	viewer.callback_pre_draw = [&](igl::opengl::glfw::Viewer &)->bool
+	{
+		if(viewer.core.is_animating)
+		{
+			////////////////////////////////////////////////////////////////////////
+			// Perform single iteration of ICP method
+			////////////////////////////////////////////////////////////////////////
+			Eigen::Matrix3d R;
+			Eigen::RowVector3d t;
+			icp_single_iteration(VX,FX,VY,FY,num_samples,method,R,t);
+			// Apply transformation to source mesh
+			VX = ((R*VX.transpose()).transpose().rowwise() + t);//.eval();
+			set_meshes();
+			if(show_samples)
+			{
+				set_points();
+			}
+		}
+		return false;
+	};
+	viewer.callback_key_pressed =
+			[&](igl::opengl::glfw::Viewer &,unsigned char key,int)->bool
+			{
+				switch(key)
+				{
+					case ' ':
+					viewer.core.is_animating ^= 1;
+					break;
+					case 'H':
+					case 'h':
+					std::cout<<"D_{H}(X -> Y) >= "<<
+					hausdorff_lower_bound(VX,FX,VY,FY,num_samples)<<std::endl;
+					break;
+					case 'M':
+					case 'm':
+					method = (ICPMethod)((((int)method)+1)%((int)NUM_ICP_METHODS));
+					std::cout<< "point-to-"<<
+					(method==ICP_METHOD_POINT_TO_PLANE?"plane":"point")<<std::endl;
+					break;
+					case 'P':
+					case 'p':
+					show_samples ^= 1;
+					break;
+					case 'R':
+					case 'r':
+					reset();
+					if(show_samples) set_points();
+					break;
+					case 'S':
+					num_samples = (num_samples-1)*2;
+					break;
+					case 's':
+					num_samples = (num_samples/2)+1;
+					break;
+					default:
+					return false;
+				}
+				return true;
+			};
 
-  reset();
-  viewer.core.is_animating = true;
-  viewer.core.point_size = 10;
-  viewer.launch();
+	reset();
+	viewer.core.is_animating = true;
+	// viewer.core().point_size = 10;
+	viewer.launch();
 
-  return EXIT_SUCCESS;
+	return EXIT_SUCCESS;
 }

src/closest_rotation.cpp

@@ -1,9 +1,18 @@
 #include "closest_rotation.h"
 
-void closest_rotation(
-  const Eigen::Matrix3d & M,
-  Eigen::Matrix3d & R)
-{
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
+#include <Eigen/SVD>
+#include <Eigen/Dense>
+
+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 V = svd.matrixV(), U = svd.matrixU();
+	// Matrix3d S = U.inverse() * M * V.transpose().inverse(); // S = U^-1 * A * VT * -1
+
+	R(2, 2) = (V * U.transpose()).determinant();
+	R = V * R * U.transpose();
 }
+

src/hausdorff_lower_bound.cpp

@@ -1,12 +1,24 @@
 #include "hausdorff_lower_bound.h"
 
-double hausdorff_lower_bound(
-  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;
+#include "random_points_on_mesh.h"
+#include "point_mesh_distance.h"
+
+double hausdorff_lower_bound(const Eigen::MatrixXd & VX,
+		const Eigen::MatrixXi & FX, const Eigen::MatrixXd & VY,
+		const Eigen::MatrixXi & FY, const int n) {
+	// Replace with your code
+	Eigen::MatrixXd Xsam;
+	random_points_on_mesh(n, VX, FX, Xsam);
+
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P, N;
+	point_mesh_distance(Xsam, VY, FY, D, P, N);
+
+	double nre = 0;
+	for (int i = 0; i < n; i++) {
+		if (nre < D[i])
+			nre = D[i];
+	}
+
+	return nre;
 }

src/icp_single_iteration.cpp

@@ -1,16 +1,30 @@
 #include "icp_single_iteration.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)
-{
-  // Replace with your code
-  R = Eigen::Matrix3d::Identity();
-  t = Eigen::RowVector3d::Zero();
+#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,
+		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();
+
+	// first sample
+	Eigen::MatrixXd Xsam;
+	random_points_on_mesh(num_samples, VX, FX, Xsam);
+
+	// close correspondence
+	Eigen::VectorXd D;
+	Eigen::MatrixXd P, N;
+	point_mesh_distance(Xsam, VY, FY, D, P, N);
+
+	// match
+	// point_to_point_rigid_matching(Xsam, P, R, t);
+	point_to_plane_rigid_matching(Xsam, P, N, R, t);
 }
+