Skip to content

Commit

Permalink
Update point-to-plane icp
Browse files Browse the repository at this point in the history
  • Loading branch information
@unclearness
unclearness committed May 9, 2023
1 parent 2f95717 commit c5e8acc
Show file tree
Hide file tree
Showing 6 changed files with 257 additions and 196 deletions.
2 changes: 1 addition & 1 deletion README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

# Design concept

## Minimal dependency but scalable
## Minimal dependency yet scalable functionality

Mandatory dependency is only [Eigen](https://gitlab.com/libeigen/eigen).

Expand Down
25 changes: 14 additions & 11 deletions app_gui/mesh_viewer/main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,18 +235,21 @@ void IcpProcess() {
timer.Start();

if (g_icp_data.point2plane) {
RigidIcpPointToPlane(g_icp_data.src_points, g_icp_data.dst_points,
g_icp_data.src_normals, g_icp_data.dst_normals,
g_icp_data.dst_faces, g_icp_data.terminate_criteria,
g_icp_data.corresp_criteria, g_icp_data.output,
g_icp_data.with_scale, g_icp_data.corresp_finder,
g_icp_data.callback);
RigidIcp(g_icp_data.src_points, g_icp_data.dst_points,
g_icp_data.src_normals, g_icp_data.dst_normals,
g_icp_data.dst_faces, IcpCorrespType::kPointToPlane,
IcpLossType::kPointToPlane, g_icp_data.terminate_criteria,
g_icp_data.corresp_criteria, g_icp_data.output,
g_icp_data.with_scale, nullptr, g_icp_data.corresp_finder, -1,
g_icp_data.callback);
} else {
RigidIcpPointToPoint(g_icp_data.src_points, g_icp_data.dst_points,
g_icp_data.src_normals, g_icp_data.dst_normals,
g_icp_data.terminate_criteria,
g_icp_data.corresp_criteria, g_icp_data.output,
g_icp_data.with_scale, nullptr, g_icp_data.callback);
RigidIcp(g_icp_data.src_points, g_icp_data.dst_points,
g_icp_data.src_normals, g_icp_data.dst_normals,
g_icp_data.dst_faces, IcpCorrespType::kPointToPoint,
IcpLossType::kPointToPoint, g_icp_data.terminate_criteria,
g_icp_data.corresp_criteria, g_icp_data.output,
g_icp_data.with_scale, nullptr, g_icp_data.corresp_finder, -1,
g_icp_data.callback);
}
timer.End();
g_callback_message =
Expand Down
89 changes: 72 additions & 17 deletions examples/ex23_rigid.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ auto AddNoise(ugu::Mesh& mesh) {
}
noised_mesh.set_vertices(noised_vertices);

Eigen::Vector3f noise_t{bb_mean / 10, bb_mean * 2 / 10, bb_mean * -3 / 10};
Eigen::Vector3f noise_t{bb_mean / 5, bb_mean * 2 / 5, bb_mean * -3 / 5};
Eigen::Vector3f axis(5, 2, 1);
axis.normalize();
Eigen::AngleAxisf noise_R(ugu::radians(10.f), axis);
Expand All @@ -71,6 +71,8 @@ void TestAlignmentWithCorresp() {
std::string data1_dir = "../data/bunny/";
std::string in_obj_path1 = data1_dir + "bunny.obj";
ugu::Mesh bunny;
std::string out_dir = "../out/ex23/corresp/";
ugu::EnsureDirExists(out_dir);
bunny.LoadObj(in_obj_path1, data1_dir);

// Add noise
Expand Down Expand Up @@ -98,7 +100,7 @@ void TestAlignmentWithCorresp() {
Eigen::Affine3f T_Rt_gt = Eigen::Translation3f(noise_t) * noise_R.matrix();
ugu::Mesh noised_bunny_Rt = ugu::Mesh(noised_bunny);
noised_bunny_Rt.Transform(T_Rt_gt.rotation(), T_Rt_gt.translation());
noised_bunny_Rt.WriteObj(data1_dir, "noised_Rt_gt");
noised_bunny_Rt.WriteObj(out_dir, "noised_Rt_gt");
std::cout << "GT Rt" << std::endl;
std::cout << T_Rt_gt.matrix() << std::endl;

Expand All @@ -108,7 +110,7 @@ void TestAlignmentWithCorresp() {
std::cout << "Estimated Rt" << std::endl;
std::cout << T_Rt_estimated.matrix() << std::endl;
bunny.Transform(T_Rt_estimated.rotation(), T_Rt_estimated.translation());
bunny.WriteObj(data1_dir, "noised_Rt_estimated");
bunny.WriteObj(out_dir, "noised_Rt_estimated");
bunny.Transform(T_Rt_estimated.inverse().rotation(),
T_Rt_estimated.inverse().translation());

Expand All @@ -117,7 +119,7 @@ void TestAlignmentWithCorresp() {
Eigen::Scaling(noise_s);
noised_bunny_Rts.Scale(noise_s);
noised_bunny_Rts.Transform(T_Rts_gt.rotation(), T_Rts_gt.translation());
noised_bunny_Rts.WriteObj(data1_dir, "noised_Rts_gt");
noised_bunny_Rts.WriteObj(out_dir, "noised_Rts_gt");
std::cout << "GT Rts" << std::endl;
std::cout << T_Rts_gt.matrix() << std::endl;

Expand All @@ -135,18 +137,21 @@ void TestAlignmentWithCorresp() {
T_Rts_estimated.matrix().block(0, 0, 3, 3).row(2).norm();
bunny.Scale(estimated_s_x, estimated_s_y, estimated_s_z);
bunny.Transform(T_Rts_estimated.rotation(), T_Rts_estimated.translation());
bunny.WriteObj(data1_dir, "noised_Rts_estimated");
bunny.WriteObj(out_dir, "noised_Rts_estimated");
}

void TestAlignmentWithoutCorresp() {
std::string data1_dir = "../data/bunny/";

std::string out_dir = "../out/ex23/icp/";
ugu::EnsureDirExists(out_dir);
std::string in_obj_path1 = data1_dir + "bunny.obj";
ugu::Mesh bunny;
bunny.LoadObj(in_obj_path1, data1_dir);

auto [T_Rts_gt, noised_mesh] = AddNoise(bunny);

noised_mesh.WriteObj(data1_dir, "noised_init");
noised_mesh.WriteObj(out_dir, "noised_init");

ugu::Mesh org_noised_mesh = noised_mesh;
ugu::Timer<double> timer;
Expand All @@ -156,16 +161,39 @@ void TestAlignmentWithoutCorresp() {
ugu::IcpCorrespCriteria crc;
ugu::IcpOutput output;
timer.Start();
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpLossType::kPointToPoint, tmc, crc,
output, false);
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpCorrespType::kPointToPoint,
ugu::IcpLossType::kPointToPoint, tmc, crc, output, false);
timer.End();
ugu::LOGI(
"Correspondence: Point-to-Point, Loss: Point-to-Point ICP: %fms\n",
timer.elapsed_msec());
for (size_t i = 0; i < output.transform_histry.size(); i++) {
ugu::LOGI("iter %d: %f\n", i, output.loss_histroty[i]);
noised_mesh = org_noised_mesh;
noised_mesh.Transform(output.transform_histry[i].cast<float>());
noised_mesh.WriteObj(out_dir,
"noised_icp_point_point_" + std::to_string(i));
}
}

{
noised_mesh = org_noised_mesh;
ugu::IcpTerminateCriteria tmc;
ugu::IcpCorrespCriteria crc;
ugu::IcpOutput output;
timer.Start();
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpCorrespType::kPointToPlane,
ugu::IcpLossType::kPointToPoint, tmc, crc, output, false);
timer.End();
ugu::LOGI("Point-To-Point ICP: %fms\n", timer.elapsed_msec());
ugu::LOGI(
"Correspondence: Point-to-Plane, Loss: Point-to-Point ICP: %fms\n",
timer.elapsed_msec());
for (size_t i = 0; i < output.transform_histry.size(); i++) {
ugu::LOGI("iter %d: %f\n", i, output.loss_histroty[i]);
noised_mesh = org_noised_mesh;
noised_mesh.Transform(output.transform_histry[i].cast<float>());
noised_mesh.WriteObj(data1_dir,
"noised_rigidicp_point_" + std::to_string(i));
noised_mesh.WriteObj(out_dir,
"noised_icp_plane_point_" + std::to_string(i));
}
}

Expand All @@ -175,25 +203,52 @@ void TestAlignmentWithoutCorresp() {
ugu::IcpCorrespCriteria crc;
ugu::IcpOutput output;
timer.Start();
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpLossType::kPointToPlane, tmc, crc,
output, false);
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpCorrespType::kPointToPoint,
ugu::IcpLossType::kPointToPlane, tmc, crc, output, false);
timer.End();

ugu::LOGI("Point-To-Plane ICP: %f ms\n", timer.elapsed_msec());
ugu::LOGI(
"Correspondence: Point-to-Point, Loss: Point-to-Plane ICP: %f ms\n",
timer.elapsed_msec());
for (size_t i = 0; i < output.transform_histry.size(); i++) {
ugu::LOGI("iter %d: %f\n", i, output.loss_histroty[i]);
noised_mesh = org_noised_mesh;
noised_mesh.Transform(output.transform_histry[i].cast<float>());
noised_mesh.WriteObj(data1_dir,
"noised_rigidicp_plane_" + std::to_string(i));
noised_mesh.WriteObj(out_dir,
"noised_icp_point_plane_" + std::to_string(i));
}
}

{
noised_mesh = org_noised_mesh;
ugu::IcpTerminateCriteria tmc;
ugu::IcpCorrespCriteria crc;
ugu::IcpOutput output;
timer.Start();
ugu::RigidIcp(noised_mesh, bunny, ugu::IcpCorrespType::kPointToPlane,
ugu::IcpLossType::kPointToPlane, tmc, crc, output, false);
timer.End();

ugu::LOGI(
"Correspondence: Point-to-Plane, Loss: Point-to-Plane ICP: %f ms\n",
timer.elapsed_msec());
for (size_t i = 0; i < output.transform_histry.size(); i++) {
ugu::LOGI("iter %d: %f\n", i, output.loss_histroty[i]);
noised_mesh = org_noised_mesh;
noised_mesh.Transform(output.transform_histry[i].cast<float>());
noised_mesh.WriteObj(out_dir,
"noised_icp_plane_plane_" + std::to_string(i));
}
}
}

} // namespace

int main() {
// TestAlignmentWithCorresp();
ugu::EnsureDirExists("../out/");
ugu::EnsureDirExists("../out/ex23/");

TestAlignmentWithCorresp();

TestAlignmentWithoutCorresp();

Expand Down
87 changes: 39 additions & 48 deletions include/ugu/registration/rigid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,16 @@ Eigen::Affine3d FindRigidTransformFrom3dCorrespondences(
const std::vector<Eigen::Vector3f>& src,
const std::vector<Eigen::Vector3f>& dst);

Eigen::Affine3f FindRigidTransformFrom3dCorrespondencesWithNormals(
const std::vector<Eigen::Vector3f>& src,
const std::vector<Eigen::Vector3f>& dst,
const std::vector<Eigen::Vector3f>& dst_normals);

Eigen::Affine3d FindRigidTransformFrom3dCorrespondencesWithNormals(
const std::vector<Eigen::Vector3d>& src,
const std::vector<Eigen::Vector3d>& dst,
const std::vector<Eigen::Vector3d>& dst_normals);

// "Least-squares estimation of transformation parameters between two point
// patterns ", Shinji Umeyama, PAMI 1991, :DOI:`10.1109/34.88573`
// implementation reference:
Expand Down Expand Up @@ -50,6 +60,7 @@ void DecomposeRts(const Eigen::Affine3f& T, Eigen::Matrix3f& R,
void DecomposeRts(const Eigen::Affine3d& T, Eigen::Matrix3d& R,
Eigen::Vector3d& t, Eigen::Vector3d& s);

enum class IcpCorrespType { kPointToPoint = 0, kPointToPlane = 1 };
enum class IcpLossType { kPointToPoint = 0, kPointToPlane = 1 };

struct IcpTerminateCriteria {
Expand All @@ -72,56 +83,36 @@ struct IcpOutput {
using IcpCallbackFunc =
std::function<void(const IcpTerminateCriteria&, const IcpOutput&)>;

// WARNING: with_scale = true is not recommended. In most cases, the
// transformation will incorrectly converge to zero scale and a certain
// translation, which corresponds to a certain target point.
bool RigidIcpPointToPoint(const std::vector<Eigen::Vector3f>& src,
const std::vector<Eigen::Vector3f>& dst,
const std::vector<Eigen::Vector3f>& src_normals,
const std::vector<Eigen::Vector3f>& dst_normals,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria,
IcpOutput& output, bool with_scale = false,
KdTreePtr<float, 3> kdtree = nullptr,
IcpCallbackFunc callback = nullptr);

bool RigidIcpPointToPoint(const std::vector<Eigen::Vector3d>& src,
const std::vector<Eigen::Vector3d>& dst,
const std::vector<Eigen::Vector3d>& src_normals,
const std::vector<Eigen::Vector3d>& dst_normals,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria,
IcpOutput& output, bool with_scale = false,
KdTreePtr<double, 3> kdtree = nullptr,
IcpCallbackFunc callback = nullptr);

bool RigidIcpPointToPlane(const std::vector<Eigen::Vector3f>& src_points,
const std::vector<Eigen::Vector3f>& dst_points,
const std::vector<Eigen::Vector3f>& src_normals,
const std::vector<Eigen::Vector3f>& dst_normals,
const std::vector<Eigen::Vector3i>& dst_faces,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria,
IcpOutput& output, bool with_scale = false,
CorrespFinderPtr corresp_finder = nullptr,
IcpCallbackFunc callback = nullptr);

bool RigidIcpPointToPlane(const std::vector<Eigen::Vector3d>& src_points,
const std::vector<Eigen::Vector3d>& dst_points,
const std::vector<Eigen::Vector3d>& src_normals,
const std::vector<Eigen::Vector3d>& dst_normals,
const std::vector<Eigen::Vector3i>& dst_faces,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria,
IcpOutput& output, bool with_scale = false,
CorrespFinderPtr corresp_finder = nullptr,
IcpCallbackFunc callback = nullptr);

bool RigidIcp(const Mesh& src, const Mesh& dst, const IcpLossType& loss_type,
bool RigidIcp(const std::vector<Eigen::Vector3f>& src_points,
const std::vector<Eigen::Vector3f>& dst_points,
const std::vector<Eigen::Vector3f>& src_normals,
const std::vector<Eigen::Vector3f>& dst_normals,
const std::vector<Eigen::Vector3i>& dst_faces,
const IcpCorrespType& corresp_type, const IcpLossType& loss_type,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria, IcpOutput& output,
bool with_scale = false, KdTreePtr<float, 3> kdtree = nullptr,
CorrespFinderPtr corresp_finder = nullptr, int num_theads = -1,
IcpCallbackFunc callback = nullptr, uint32_t approx_nn_num = 10);

bool RigidIcp(const std::vector<Eigen::Vector3d>& src_points,
const std::vector<Eigen::Vector3d>& dst_points,
const std::vector<Eigen::Vector3d>& src_normals,
const std::vector<Eigen::Vector3d>& dst_normals,
const std::vector<Eigen::Vector3i>& dst_faces,
const IcpCorrespType& corresp_type, const IcpLossType& loss_type,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria, IcpOutput& output,
bool with_scale = false, KdTreePtr<double, 3> kdtree = nullptr,
CorrespFinderPtr corresp_finder = nullptr, int num_theads = -1,
IcpCallbackFunc callback = nullptr, uint32_t approx_nn_num = 10);

bool RigidIcp(const Mesh& src, const Mesh& dst,
const IcpCorrespType& corresp_type, const IcpLossType& loss_type,
const IcpTerminateCriteria& terminate_criteria,
const IcpCorrespCriteria& corresp_criteria, IcpOutput& output,
bool with_scale = false, KdTreePtr<float, 3> kdtree = nullptr,
CorrespFinderPtr corresp_finder = nullptr,
IcpCallbackFunc callback = nullptr);
CorrespFinderPtr corresp_finder = nullptr, int num_theads = -1,
IcpCallbackFunc callback = nullptr, uint32_t approx_nn_num = 10);

} // namespace ugu
6 changes: 3 additions & 3 deletions src/registration/nonrigid.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,10 +18,10 @@
#endif
#endif

#include <Eigen/IterativeLinearSolvers>
// #include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
// #include <Eigen/SparseLU>
// #include <Eigen/SparseQR>
#ifdef _WIN32
#pragma warning(pop)
#endif
Expand Down
Loading

0 comments on commit c5e8acc

Please sign in to comment.