From 028b90e9eca37f5e2ba411e0fa46913337b1a3ec Mon Sep 17 00:00:00 2001 From: Sven Oesau Date: Wed, 2 Oct 2024 19:18:32 +0200 Subject: [PATCH] added apply_non_rigid_transformation to CHANGES.md removing warnings (unused variables and signed/unsigned comparison) switching to ARAP formulation also used in Surface deformation --- Installation/CHANGES.md | 2 +- .../non_rigid_mesh_registration_example.cpp | 8 +- .../non_rigid_mesh_registration.h | 112 ++++++++++++------ 3 files changed, 83 insertions(+), 39 deletions(-) diff --git a/Installation/CHANGES.md b/Installation/CHANGES.md index 2175a174b9bc..71e2703a847d 100644 --- a/Installation/CHANGES.md +++ b/Installation/CHANGES.md @@ -182,7 +182,7 @@ Release date: June 2024 which can be used to refine a polygon mesh along an isocurve. - Added the function [`CGAL::Polygon_mesh_processing::add_bbox()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PkgPolygonMeshProcessingRef.html#gabaf98d2fd9ae599ff1f3a5a6cde79cf3), which enables adding a tight or extended, triangulated or not, bounding box to a face graph. -- Added two functions for non-rigid registration of mesh to mesh and mesh to point cloud [`CGAL::Polygon_mesh_processing::non_rigid_mesh_to_mesh_registration()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PMP__registration__grp.html#ga8f009d2c73e58129c96ec9ae7f4f365c) and [`CGAL::Polygon_mesh_processing::non_rigid_mesh_to_points_registration()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PMP__registration__grp.html#gafebc57d7544dd2c7c9feaaf2f24f2eda) +- Added two functions for non-rigid registration of mesh to mesh and mesh to point cloud [`CGAL::Polygon_mesh_processing::non_rigid_mesh_to_mesh_registration()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PMP__registration__grp.html#ga04e4101a21663bebb689de30bb7d2f4e) and [`CGAL::Polygon_mesh_processing::non_rigid_mesh_to_points_registration()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PMP__registration__grp.html#ga466fd5b15e5de0b65faf48947a7f2bef) as well as a function to apply a transformation [`CGAL::Polygon_mesh_processing::apply_non_rigid_transformation()`](https://doc.cgal.org/6.0/Polygon_mesh_processing/group__PMP__registration__grp.html#gac022fccce4796cba9ff710865154b127) ### [2D Triangulations](https://doc.cgal.org/6.0/Manual/packages.html#PkgTriangulation2) - **Breaking change**: the concept [`TriangulationTraits_2`](https://doc.cgal.org/6.0/Triangulation_2/classTriangulationTraits__2.html) now requires an additional functor `Compare_xy_2`. diff --git a/Polygon_mesh_processing/examples/Polygon_mesh_processing/non_rigid_mesh_registration_example.cpp b/Polygon_mesh_processing/examples/Polygon_mesh_processing/non_rigid_mesh_registration_example.cpp index 738a6fff43b4..80a83c964da4 100644 --- a/Polygon_mesh_processing/examples/Polygon_mesh_processing/non_rigid_mesh_registration_example.cpp +++ b/Polygon_mesh_processing/examples/Polygon_mesh_processing/non_rigid_mesh_registration_example.cpp @@ -53,13 +53,13 @@ int main(int argc, char** argv) { Mesh::Property_map> vrm = source.add_property_map>("v:rotation").first; Mesh::Property_map vtm = source.add_property_map("v:translation").first; - FT w1 = 1.0; + FT w1 = 2.0; FT w2 = 2.0; - FT w3 = 500; + FT w3 = 50; - PMP::non_rigid_mesh_to_mesh_registration(source, target, vtm, vrm, CGAL::parameters::point_to_point_energy(w1).point_to_plane_energy(w2).as_rigid_as_possible_energy(w3).correspondences(correspondences_mesh)); + PMP::non_rigid_mesh_to_mesh_registration(source, target, vtm, vrm, CGAL::parameters::point_to_point_energy(w1).point_to_plane_energy(w2).as_rigid_as_possible_energy(w3).correspondences(std::cref(correspondences_mesh))); PMP::apply_non_rigid_transformation(source, vtm); - CGAL::IO::write_polygon_mesh("bear" + std::to_string(w1) + "_" + std::to_string(w2) + "_" + std::to_string(w3) + ".off", source); + CGAL::IO::write_polygon_mesh("bear_" + std::to_string(w1) + "_" + std::to_string(w2) + "_" + std::to_string(w3) + ".off", source); return EXIT_SUCCESS; } \ No newline at end of file diff --git a/Polygon_mesh_processing/include/CGAL/Polygon_mesh_processing/non_rigid_mesh_registration.h b/Polygon_mesh_processing/include/CGAL/Polygon_mesh_processing/non_rigid_mesh_registration.h index adfc280255bd..a530f9b00b3d 100644 --- a/Polygon_mesh_processing/include/CGAL/Polygon_mesh_processing/non_rigid_mesh_registration.h +++ b/Polygon_mesh_processing/include/CGAL/Polygon_mesh_processing/non_rigid_mesh_registration.h @@ -28,6 +28,9 @@ #include #include +#include +#include +#include #include #include @@ -92,8 +95,6 @@ std::pair nearest_neighbor(Vertices& points, V using Search_traits = CGAL::Search_traits_adapter>>; using Neighbor_search = CGAL::Orthogonal_k_neighbor_search; using KDTree = typename Neighbor_search::Tree; - const int leaf_max_size = 10; - const int ndim = 3; Eigen_matrix_to_point_map a(points); @@ -106,7 +107,7 @@ std::pair nearest_neighbor(Vertices& points, V for (Index i = 0; i < query.rows(); ++i) { Point_3 query_pt = { query(i, 0), query(i, 1), query(i, 2) }; Neighbor_search search(kdtree, query_pt, k, 0, true, Neighbor_search::Distance(Eigen_matrix_to_point_map(points))); - std::size_t j = 0; + Index j = 0; for (auto it = search.begin(); it != search.end() && j < k; ++it) { idz(i, j) = it->first; dist(i, j) = it->second; @@ -139,6 +140,31 @@ Eigen::Matrix rot(T a, T b, T c) { return R; } +template +Eigen::Matrix rotation(std::size_t index, Visitor &v, TriangleMesh &source, const Vertices& X, const Vertices& Z, const std::set& neighbors, const std::vector &weights, std::vector> &rotations) { + using Vertex_index = typename boost::graph_traits::vertex_descriptor; + Eigen::Matrix3d cov; + cov.setZero(); + + Eigen::Vector3d x_src = X.row(index), z_src = Z.row(index); + auto n = neighbors.begin(); + auto w = weights.begin(); + + v.rotation_matrix_pre(Vertex_index(index), source); + + for (std::size_t idx = 0; (n != neighbors.end()) && (w != weights.end()); idx++, n++, w++) { + Eigen::Vector3d x_dst = X.row(*n); + Eigen::Vector3d z_dst = Z.row(*n); + Eigen::Vector3d source_edge = x_src - x_dst; + Eigen::Vector3d moving_edge = z_src - z_dst; + + cov += weights[idx] * (source_edge * moving_edge.transpose()); + v.update_covariance_matrix(cov, rotations[index]); + } + + return cov; +} + #endif } // namespace registration } // namespace internal @@ -268,6 +294,8 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, Vertices X(num_vertices(source), 3), Y(target.size(), 3); Faces XF(num_faces(source), 3); + using Vertex_index = typename boost::graph_traits::vertex_descriptor; + using NP_helper = Point_set_processing_3_np_helper; using Point_map = typename NP_helper::Point_map; using Normal_map = typename NP_helper::Normal_map; @@ -284,20 +312,33 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, typedef typename CGAL::internal_np::Lookup_named_param_def::vertex_descriptor, std::size_t>>>::reference Correspondences_type; Correspondences_type correspondences = CGAL::parameters::choose_parameter(CGAL::parameters::get_parameter_reference(np1, CGAL::internal_np::correspondences), std::vector::vertex_descriptor, std::size_t>>()); + using ARAP_visitor = typename CGAL::internal::Types_selectors < TriangleMesh, Vertex_point_map, SRE_ARAP>::ARAP_visitor; + using WC = typename CGAL::internal::Types_selectors < TriangleMesh, Vertex_point_map, SRE_ARAP>::Weight_calculator; + WC wc; + std::size_t idx = 0; + std::map::vertex_descriptor, std::size_t> vi; + std::vector::vertex_descriptor> iv; + std::vector > he_weights; + std::map, FT> halfedge_weights; + iv.resize(num_vertices(source)); for (auto v : vertices(source)) { X(idx, 0) = get(vpm, v).x(); X(idx, 1) = get(vpm, v).y(); X(idx, 2) = get(vpm, v).z(); + iv[idx] = v; + vi[v] = idx; idx++; } idx = 0; for (auto f : faces(source)) { - Vertex_around_face_circulator vit = Vertex_around_face_circulator(halfedge(f, source), source); - XF(idx, 0) = *vit++; - XF(idx, 1) = *vit++; - XF(idx, 2) = *vit++; + auto h = halfedge(f, source); + XF(idx, 0) = CGAL::target(h, source); + h = next(h, source); + XF(idx, 1) = CGAL::target(h, source); + h = next(h, source); + XF(idx, 2) = CGAL::target(h, source); idx++; } std::cout << std::endl; @@ -337,6 +378,22 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, neighbors[c].insert(b); } + // Calculate edge weights + he_weights.resize(X.rows()); + for (Index i = 0; i < X.rows(); ++i) { + std::size_t vi = XF(i, 0); + he_weights[vi].resize(neighbors[vi].size()); + std::size_t idx = 0; + for (int vj : neighbors[vi]) { + auto h = halfedge(Vertex_index(vi), Vertex_index(vj), source); + assert(h.second); + he_weights[vi][idx++] = wc(h.first, source, vpm); + } + } + + ARAP_visitor visitor; + visitor.init(source, vpm); + // Non-rigid ICP Vertices Z(X); Index dim = Z.rows() * Z.cols(); @@ -442,7 +499,10 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, //cg.setMaxIterations(1000); //cg.setTolerance(1e-6); Eigen::JacobiSVD> svd; - std::vector> Rotations(Z.rows()); + std::vector> rotations(Z.rows()); + + for (std::size_t i = 0; i < Z.rows(); i++) + rotations[i].setIdentity(); size_t coefficients_size = coefficients.size(); @@ -538,34 +598,19 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, if (it == (iter - 1)) { // Should replace with CGAL's ARAP implementation https://github.com/CGAL/cgal/blob/master/Surface_mesh_deformation/include/CGAL/Surface_mesh_deformation.h // See also: compute_close_rotation https://github.com/CGAL/cgal/blob/master/Surface_mesh_deformation/include/CGAL/Deformation_Eigen_closest_rotation_traits_3.h + for (Index i = 0; i < Z.rows(); ++i) + rotations[i] = rotation(std::size_t(i), visitor, source, X, Z, neighbors[i], he_weights[i], rotations); + for (Index i = 0; i < Z.rows(); ++i) { - std::vector nbrs(neighbors[i].begin(), neighbors[i].end()); - Matrix A = X(nbrs, Eigen::indexing::all).rowwise() - X.row(i); - Matrix B_ = Z(nbrs, Eigen::indexing::all).rowwise() - Z.row(i); - - svd.compute(A.transpose() * B_, Eigen::ComputeFullU | Eigen::ComputeFullV); - Rotations[i] = svd.matrixV() * svd.matrixU().transpose(); - if (Rotations[i].determinant() < 0) { - Eigen::Matrix M = Eigen::Matrix3d::Identity(); - M(2, 2) = -1; - Rotations[i] = svd.matrixV() * M * svd.matrixU().transpose(); + rotations[i] = rotation(std::size_t(i), visitor, source, X, Z, neighbors[i], he_weights[i], rotations); + BX.row(i) = BX.row(i) * rotations[i].transpose(); } - } - for (Index i = 0; i < edim; ++i) { - Matrix R = Rotations[Ni(i)]; - BX.row(i) = BX.row(i) * R.transpose(); - } } - else { - // Regular matrix update is happening here (taking linearized coefficients, recreating matrix and rotating edges) - for (Index i = 0; i < edim; ++i) { - int ni = Ni(i); - Matrix R = rot(x(dim + 2 * Z.rows() + ni), - x(dim + Z.rows() + ni), - x(dim + ni)); - BX.row(i) = BX.row(i) * R.transpose(); + else + for (Index i = 0; i < Z.rows(); ++i) { + rotations[i] = rotation(std::size_t(i), visitor, source, X, Z, neighbors[i], he_weights[i], rotations); + BX.row(i) = BX.row(i) * rotations[i].transpose(); } - } } idx = 0; @@ -575,7 +620,7 @@ void non_rigid_mesh_to_points_registration(TriangleMesh& source, /* Aff_transformation_3 t1(CGAL::TRANSLATION, CGAL::ORIGIN - x); Aff_transformation_3 t2(CGAL::TRANSLATION, -(CGAL::ORIGIN - z));*/ - const auto& r = Rotations[idx]; + const auto& r = rotations[idx]; Aff_transformation_3 rota(r(0, 0), r(0, 1), r(0, 2), 0, r(1, 0), r(1, 1), r(1, 2), 0, r(2, 0), r(2, 1), r(2, 2), 0, @@ -795,7 +840,6 @@ void apply_non_rigid_transformation(TriangleMesh& mesh, NamedParameters, Default_vector_map>::type; using Point = typename Gt::Point_3; - using Vector = typename Gt::Vector_3; Vertex_point_map vpm = parameters::choose_parameter(parameters::get_parameter(np, internal_np::vertex_point), get_property_map(CGAL::vertex_point, mesh)); Vertex_normal_map vnm = parameters::choose_parameter(parameters::get_parameter(np, internal_np::vertex_normal_map), get(Vector_map_tag(), mesh));