diff --git a/README.html b/README.html
index 6c13281..c163d77 100644
--- a/README.html
+++ b/README.html
@@ -343,7 +343,7 @@ Constant function approximation
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\):
diff --git a/README.md b/README.md
index bce5dc2..ce58753 100644
--- a/README.md
+++ b/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$:
diff --git a/main.cpp b/main.cpp
index 3115cc9..8498097 100644
--- a/main.cpp
+++ b/main.cpp
@@ -3,27 +3,27 @@
#include "random_points_on_mesh.h"
#include "point_mesh_distance.h"
#include
-#include
+#include
#include
#include
#include
-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<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)<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)<
+#include
+
+void closest_rotation(const Eigen::Matrix3d & M, Eigen::Matrix3d & R) {
+ // Replace with your code
+ R = Eigen::Matrix3d::Identity();
+
+ Eigen::JacobiSVD 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();
}
+
diff --git a/src/hausdorff_lower_bound.cpp b/src/hausdorff_lower_bound.cpp
index 8608964..b85af50 100644
--- a/src/hausdorff_lower_bound.cpp
+++ b/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;
}
diff --git a/src/icp_single_iteration.cpp b/src/icp_single_iteration.cpp
index 1e8fda9..24cca87 100644
--- a/src/icp_single_iteration.cpp
+++ b/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);
}
+
diff --git a/src/point_mesh_distance.cpp b/src/point_mesh_distance.cpp
index 2e6a070..5ecd252 100644
--- a/src/point_mesh_distance.cpp
+++ b/src/point_mesh_distance.cpp
@@ -1,16 +1,44 @@
#include "point_mesh_distance.h"
-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)
-{
- // Replace with your code
- P.resizeLike(X);
- N = Eigen::MatrixXd::Zero(X.rows(),X.cols());
- for(int i = 0;i
-void point_to_plane_rigid_matching(
- 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();
+#include "closest_rotation.h"
+
+void point_to_plane_rigid_matching(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();
+
+ int row = X.rows();
+ Eigen::VectorXd Z0 = Eigen::VectorXd::Zero(row);
+ Eigen::VectorXd Z1 = Eigen::VectorXd::Ones(row);
+
+ Eigen::VectorXd X0 = X.col(0);
+ Eigen::VectorXd X1 = X.col(1);
+ Eigen::VectorXd X2 = X.col(2);
+ Eigen::MatrixXd A(3 * row, 6);
+ A << Z0, X2, -X1, Z1, Z0, Z0, -X2, Z0, X0, Z0, Z1, Z0, X1, -X0, Z0, Z0, Z0, Z1;
+
+ Eigen::MatrixXd b(3 * row, 1);
+ b << P.col(0) - X0, P.col(1) - X1, P.col(2) - X2;
+
+ /* faster
+ Eigen::MatrixXd diag(row, 3 * row);
+ diag << Eigen::MatrixXd(N.col(0).asDiagonal()), Eigen::MatrixXd(
+ N.col(1).asDiagonal()), Eigen::MatrixXd(N.col(2).asDiagonal());
+
+ A = diag * A;
+ b = diag * b;
+ */
+
+ Eigen::MatrixXd ATA = A.transpose() * A;
+ Eigen::MatrixXd ATb = A.transpose() * b;
+
+ Eigen::VectorXd u = ATA.inverse() * ATb;
+ Eigen::Matrix3d M = Eigen::Matrix3d::Identity();
+ double alpha = u[0];
+ double beta = u[1];
+ double gamma = u[2];
+
+ M(1, 0) += -gamma;
+ M(2, 0) += beta;
+ M(0, 1) += gamma;
+ M(2, 1) += -alpha;
+ M(0, 2) += -beta;
+ M(1, 2) += alpha;
+
+ closest_rotation(M, R);
+
+ t[0] = u[3];
+ t[1] = u[4];
+ t[2] = u[5];
}
diff --git a/src/point_to_point_rigid_matching.cpp b/src/point_to_point_rigid_matching.cpp
index 079151f..f2b2b13 100644
--- a/src/point_to_point_rigid_matching.cpp
+++ b/src/point_to_point_rigid_matching.cpp
@@ -1,14 +1,32 @@
#include "point_to_point_rigid_matching.h"
#include
-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();
+#include "closest_rotation.h"
+
+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();
+
+ // R * PL = PR
+ std::cout << X.colwise().mean() << std::endl;
+ Eigen::RowVector3d PLavg = X.colwise().mean();
+ Eigen::RowVector3d PRavg = P.colwise().mean();
+
+ // subtranct mean
+ Eigen::MatrixXd PL, PR;
+ PL.resizeLike(X);
+ PR.resizeLike(X);
+ for (int i = 0; i < X.rows(); i++) {
+ PL.row(i) = X.row(i) - PLavg;
+ PR.row(i) = P.row(i) - PRavg;
+ }
+
+ Eigen::Matrix3d tmp = PL.transpose() * PR;
+ closest_rotation(tmp, R);
+
+ t = (PRavg.transpose() - R * PLavg.transpose()).transpose();
}
diff --git a/src/point_triangle_distance.cpp b/src/point_triangle_distance.cpp
index 6405100..8f828aa 100644
--- a/src/point_triangle_distance.cpp
+++ b/src/point_triangle_distance.cpp
@@ -1,14 +1,109 @@
#include "point_triangle_distance.h"
-void point_triangle_distance(
- const Eigen::RowVector3d & x,
- const Eigen::RowVector3d & a,
- const Eigen::RowVector3d & b,
- const Eigen::RowVector3d & c,
- double & d,
- Eigen::RowVector3d & p)
-{
- // Replace with your code
- d = 0;
- p = a;
+double clipdouble(double a, double amin, double amax) {
+ if (a > amax)
+ return amax;
+
+ if (a < amin)
+ return amin;
+
+ return a;
}
+
+void point_triangle_distance(const Eigen::RowVector3d & x,
+ const Eigen::RowVector3d & a, const Eigen::RowVector3d & b,
+ const Eigen::RowVector3d & c, double & d, Eigen::RowVector3d & p) {
+ // Replace with your code
+
+ // fast
+ Eigen::RowVector3d ax = x - a;
+ Eigen::RowVector3d bx = x - b;
+ Eigen::RowVector3d cx = x - c;
+
+ double dax = ax.norm();
+ double dbx = bx.norm();
+ double dcx = cx.norm();
+
+ if (dax < dbx) {
+ if (dax < dcx) {
+ d = dax;
+ p = a;
+ } else {
+ d = dcx;
+ p = c;
+ }
+ } else {
+ if (dbx < dcx) {
+ d = dbx;
+ p = b;
+ } else {
+ d = dcx;
+ p = c;
+ }
+ }
+
+ return;
+
+ // calculate a point d in the plane of abc
+ // s.t xd is minimal
+ // the same as solve equation
+ Eigen::MatrixXd A(3, 2);
+ A.col(0) = (b - a).transpose();
+ A.col(1) = (c - a).transpose();
+
+ Eigen::MatrixXd B(3, 1);
+ B.col(0) = (x - a).transpose();
+
+ Eigen::MatrixXd ATA = A.transpose() * A;
+ Eigen::MatrixXd ATB = A.transpose() * B;
+ Eigen::MatrixXd ATAINV;
+ ATAINV.resizeLike(ATA);
+ ATAINV(0, 0) = ATA(1, 1);
+ ATAINV(1, 0) = -ATA(1, 0);
+ ATAINV(0, 1) = -ATA(0, 1);
+ ATAINV(1, 1) = ATA(0, 0);
+
+ double det = ATA(0, 0) * ATA(1, 1) - ATA(0, 1) * ATA(1, 0);
+ if (det != 0) {
+ ATAINV /= det;
+ Eigen::MatrixXd X = ATAINV * ATB;
+
+ double a1 = X(0, 0);
+ double a2 = X(1, 0);
+ double a3 = 1 - a1 - a2;
+
+ a1 = clipdouble(a1, 0, 1);
+ a2 = clipdouble(a2, 0, 1);
+ a3 = clipdouble(a3, 0, 1);
+
+ p = a1 * a + a2 * b + a3 * c;
+ } else {
+
+ Eigen::RowVector3d ax = x - a;
+ Eigen::RowVector3d bx = x - b;
+ Eigen::RowVector3d cx = x - c;
+
+ double dax = ax.norm();
+ double dbx = bx.norm();
+ double dcx = cx.norm();
+
+ if (dax < dbx) {
+ if (dax < dcx) {
+ d = dax;
+ p = a;
+ } else {
+ d = dcx;
+ p = c;
+ }
+ } else {
+ if (dbx < dcx) {
+ d = dbx;
+ p = b;
+ } else {
+ d = dcx;
+ p = c;
+ }
+ }
+ }
+}
+
diff --git a/src/random_points_on_mesh.cpp b/src/random_points_on_mesh.cpp
index 6e85d75..affd7fb 100644
--- a/src/random_points_on_mesh.cpp
+++ b/src/random_points_on_mesh.cpp
@@ -1,13 +1,55 @@
#include "random_points_on_mesh.h"
-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(rand()) / static_cast(RAND_MAX);
+ return r;
+}
+
+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);
+
+ // faster
+ int vnum = V.rows();
+ for (int i = 0; i < n; i++) {
+ // find where it is
+ int idx = int(randomfloat() * vnum);
+ X.row(i) = V.row(idx);
+ }
+ return;
+
+ // first, calculate the area of each triangle
+ Eigen::MatrixXd areas;
+ igl::doublearea(V, F, areas);
+
+ // second, calculate the cumsum of the points
+ Eigen::MatrixXd consumareas;
+ igl::cumsum(areas, 1, consumareas);
+
+ // for each random
+ int fnum = F.rows();
+ for (int i = 0; i < n; i++) {
+ float tmp = randomfloat() * consumareas(fnum - 1, 0);
+
+ // find where it is
+ int idx = 0;
+ for (int j = 0; j < fnum; j++) {
+ if (tmp <= consumareas(j, 0)) {
+ idx = j;
+ break;
+ }
+ }
+
+ double a1 = randomfloat();
+ double a2 = (1 - a1) * randomfloat();
+ double a3 = 1 - a1 - a2;
+
+ X.row(i) = a1 * V.row(F(idx, 0)) + a2 * V.row(F(idx, 1))
+ + a3 * V.row(F(idx, 2));
+ }
}