songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/Surface_modeling/include/CGAL/Deform_mesh.h

1269 lines
44 KiB
C++
Raw Blame History

// Copyright (c) 2011 GeometryFactory
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL:$
// $Id:$
//
// Author(s) : Yin Xu, Andreas Fabri
#ifndef CGAL_DEFORM_MESH_H
#define CGAL_DEFORM_MESH_H
#include <CGAL/internal/Surface_modeling/Weights.h>
#include <CGAL/internal/Surface_modeling/Spokes_and_rims_iterator.h>
#include <CGAL/trace.h>
#include <CGAL/Timer.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/boost/graph/properties_Polyhedron_3.h>
#include <CGAL/boost/graph/halfedge_graph_traits_Polyhedron_3.h>
#include <CGAL/FPU_extension.h>
#include <Eigen/Eigen>
#include <Eigen/SVD>
#include <set>
#include <vector>
#include <list>
#define CGAL_DEFORM_SPOKES_AND_RIMS // it uses all edges in facets around a vertex
namespace CGAL {
/// \ingroup PkgSurfaceModeling
/**
* @brief Class providing the functionalities for deforming a triangulated surface mesh
*
* @pre @a polyhedron.is_pure_triangle()
* @tparam Polyhedron a model of HalfedgeGraph
* @tparam SparseLinearAlgebraTraitsWithPreFactor_d sparse linear solver for square sparse linear systems
* @tparam VertexIndexMap a <a href="http://www.boost.org/doc/libs/release/libs/property_map/doc/ReadWritePropertyMap.html">`ReadWritePropertyMap`</a> with vertex_descriptor as key and `unsigned int` as value type
* @tparam EdgeIndexMap a <a href="http://www.boost.org/doc/libs/release/libs/property_map/doc/ReadWritePropertyMap.html">`ReadWritePropertyMap`</a> with edge_descriptor as key and `unsigned int` as value type
* @tparam WeightCalculator how to document this (should I provide a concept, like in SegmentationGeomTraits ?) */
/// \code
/// // a simple model to WeightCalculator concept, which provides uniform weights
/// template<class Polyhedron>
/// class Uniform_weight
/// {
/// public:
/// typedef typename boost::graph_traits<Polyhedron>::edge_descriptor edge_descriptor;
///
/// Uniform_weight(Polyhedron& /*polyhedron*/) { }
///
/// double operator()(edge_descriptor e)
/// { return 1.0; }
/// };
/// \endcode
template <
class Polyhedron,
class SparseLinearAlgebraTraits_d,
class VertexIndexMap,
class EdgeIndexMap,
#if defined(CGAL_DEFORM_SPOKES_AND_RIMS)
class WeightCalculator = internal::Single_cotangent_weight<Polyhedron >
#else
class WeightCalculator = internal::Cotangent_weight<Polyhedron >
#endif
>
class Deform_mesh
{
//Typedefs
public:
typedef typename boost::graph_traits<Polyhedron>::vertex_descriptor vertex_descriptor; /**< The type for vertex representative objects */
typedef typename boost::graph_traits<Polyhedron>::edge_descriptor edge_descriptor; /**< The type for edge representative objects */
typedef typename Polyhedron::Traits::Vector_3 Vector; /**<The type for Vector_3 from Polyhedron traits */
typedef typename Polyhedron::Traits::Point_3 Point; /**<The type for Point_3 from Polyhedron traits */
private:
// Geometric types
typedef typename Polyhedron::Traits Kernel;
// Repeat Polyhedron types
typedef typename boost::graph_traits<Polyhedron>::vertex_iterator vertex_iterator;
typedef typename boost::graph_traits<Polyhedron>::edge_iterator edge_iterator;
typedef typename boost::graph_traits<Polyhedron>::in_edge_iterator in_edge_iterator;
typedef typename boost::graph_traits<Polyhedron>::out_edge_iterator out_edge_iterator;
typedef internal::Spokes_and_rims_iterator<Polyhedron> Rims_iterator;
// Handle container types
typedef std::vector<vertex_descriptor> Handle_container;
typedef std::list<Handle_container> Handle_group_container;
public:
/** The type for returned handle group representative from insert_handle(vertex_descriptor vd), insert_handle(InputIterator begin, InputIterator end) */
typedef typename Handle_group_container::iterator Handle_group;
// Data members.
public:
Polyhedron& polyhedron; /**< Source triangulated surface mesh for modeling */
private:
std::vector<Point> original; // original positions of roi (size: ros + boundary_of_ros)
std::vector<Point> solution; // storing position of ros vertices during iterations (size: ros + boundary_of_ros)
VertexIndexMap vertex_index_map; // storing indices of ros vertices
EdgeIndexMap edge_index_map; // storing indices of ros related edges
std::vector<vertex_descriptor> ros; // region of solution, including roi and hard constraints on boundary of roi
std::vector<bool> is_roi; // (size: ros)
std::vector<bool> is_hdl; // (size: ros)
std::vector<double> edge_weight; // weight of edges only those who are incident to ros
std::vector<Eigen::Matrix3d> rot_mtr; // rotation matrices of ros vertices (size: ros)
SparseLinearAlgebraTraits_d m_solver; // linear sparse solver
unsigned int iterations; // number of maximal iterations
double tolerance; // tolerance of convergence
bool need_preprocess; // is there any need to call preprocess() function
Handle_group_container handle_groups; // user specified handles
WeightCalculator weight_calculator; // calculate weight for an edge
// Public methods
public:
/**
* The constructor for deformation object
*
* @pre @a polyhedron.is_pure_triangle()
* @param polyhedron a triangulated surface mesh for modeling
* @param vertex_index_map_ vertex index map for associating ids with region of interest vertices
* @param edge_index_map_ edge index map for associating ids with region of interest edges
* @param iterations number of iterations for each call to deform()
* @param tolerance ...
*/
Deform_mesh(Polyhedron& polyhedron,
VertexIndexMap vertex_index_map,
EdgeIndexMap edge_index_map,
unsigned int iterations = 5,
double tolerance = 1e-4)
: polyhedron(polyhedron), vertex_index_map(vertex_index_map), edge_index_map(edge_index_map),
iterations(iterations), tolerance(tolerance), need_preprocess(true), weight_calculator(polyhedron)
{
CGAL_precondition(polyhedron.is_pure_triangle());
}
///////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////// Vertex insertion deletion //////////////////////////////////
/**
* Clear the internal state of the object, after cleanup the object can be treated as if it is just constructed
*/
void clear()
{
need_preprocess = true;
// clear vertices
//roi.clear();
ros.clear();
handle_groups.clear();
// no need to clear vertex index map (or edge) since they are going to be reassigned
// (at least the useful parts will be reassigned)
}
/**
* Create a new empty handle group for inserting handles.
* insert_handle(Handle_group handle_group, vertex_descriptor vd) or insert_handle(Handle_group handle_group, InputIterator begin, InputIterator end)
* can be used for populating a group.
* After inserting vertices, one can use translate(Handle_group handle_group, const Vector& translation) or rotate(...)
* to apply transformations on all vertices inside the group.
* @return created handle group representative (returned representative is valid until erase_handle(Handle_group handle_group) is called [or copy constructor what to do about it?])
* @see insert_handle(vertex_descriptor vd), insert_handle(InputIterator begin, InputIterator end)
*/
Handle_group create_handle_group()
{
need_preprocess = true;
handle_groups.push_back(Handle_container());
return --handle_groups.end();
}
/**
* -- I think this function can be removed, since combination of other functions simply accomplish the same task.
\code
Handle_group handle_group = create_handle_group();
insert_handle(handle_group, vd);
// or
Handle_group handle_group = insert_handle(vd);
\endcode
* Create a new empty handle group and insert vd in it.
* @param vd vertex to be inserted
* @return created handle group representative
* @see insert_handle(Handle_group handle_group, vertex_descriptor vd),
* insert_handle(Handle_group handle_group, InputIterator begin, InputIterator end)
* for inserting more vertices into a handle group
*/
Handle_group insert_handle(vertex_descriptor vd)
{
need_preprocess = true;
Handle_group handle_group = create_handle_group();
insert_handle(handle_group, vd);
return handle_group;
}
/**
* Insert a vertex into a handle group
* @param handle_group group to be inserted into
* @param vd vertex to be inserted
*/
void insert_handle(Handle_group handle_group, vertex_descriptor vd)
{
need_preprocess = true;
handle_group->push_back(vd);
}
/**
* -- I think this function can be removed, since combination of other functions simply accomplish the same task.
\code
Handle_group handle_group = create_handle_group();
insert_handle(handle_group, begin, end);
// or
Handle_group handle_group = insert_handle(begin, end);
\endcode
* Create a new handle group and insert vertices in the range.
* @tparam InputIterator input iterator type which points to vertex descriptors
* @param begin iterators spesifying the range of vertices i.e. [begin, end)
* @param end iterators spesifying the range of vertices i.e. [begin, end)
* It simply corresponds to:
*/
template<class InputIterator>
Handle_group insert_handle(InputIterator begin, InputIterator end)
{
need_preprocess = true;
Handle_group handle_group = create_handle_group();
insert_handle(handle_group, begin, end);
return handle_group;
}
/**
* Insert vertices in the range to provided handle group
* @tparam InputIterator input iterator type which points to vertex descriptors
* @param handle_group group to be inserted in
* @param begin iterators spesifying the range of vertices [begin, end)
* @param end iterators spesifying the range of vertices [begin, end)
*/
template<class InputIterator>
void insert_handle(Handle_group handle_group, InputIterator begin, InputIterator end)
{
need_preprocess = true;
for( ;begin != end; ++begin)
{
insert_handle(handle_group, *begin);
}
}
/**
* Erase handle group, and invalidate the representative so that it should not be used anymore.
* @param handle_group group to be erased
*/
void erase_handle(Handle_group handle_group)
{
need_preprocess = true;
handle_groups.erase(handle_group);
}
/**
* Erase a vertex from a handle group, note that handle group is not erased even if it becomes empty.
* @param handle_group group to be erased from
* @param vd vertex to be erased
*/
void erase_handle(Handle_group handle_group, vertex_descriptor vd)
{
need_preprocess = true;
typename Handle_container::iterator it
= std::find(handle_group->begin(), handle_group->end(), vd);
if(vd != handle_group->end())
{
handle_group->erase(it);
// Although the handle group might get empty, we do not delete it from handle_groups
}
}
/**
* Insert vertices in the range to region of interest
* @tparam InputIterator input iterator type which points to vertex descriptors
* @param begin iterators spesifying the range of vertices [begin, end)
* @param end iterators spesifying the range of vertices [begin, end)
*/
template<class InputIterator>
void insert_roi(InputIterator begin, InputIterator end)
{
need_preprocess = true;
for( ;begin != end; ++begin)
{
insert_roi(*begin);
}
}
/**
* Insert a vertex to region of interest
* @param vd vertex to be inserted
*/
void insert_roi(vertex_descriptor vd)
{
need_preprocess = true;
ros.push_back(vd);
}
/**
* Erease a vertex from ROI
* @param vd vertex to be erased
*/
void erase_roi(vertex_descriptor vd)
{
need_preprocess = true;
typename std::vector<vertex_descriptor>::iterator it = std::find(ros.begin(), ros.end(), vd);
if(vd != ros.end())
{
ros.erase(it);
}
}
//////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////// Other utilities ///////////////////////////////////////////
/**
* Set the number of iterations used in deform()
*/
void set_iterations(unsigned int iterations)
{
this->iterations = iterations;
}
/**
* Set the tolerance of convergence used in deform()
* Set to zero if energy based termination is not required.
*/
void set_tolerance(double tolerance)
{
this->tolerance = tolerance;
}
/**
* Translate the handle group by translation,
* in other words every handle vertex in the handle_group is translated from its original position
* @param handle_group representative of the handle group which is subject to translation
* @param translation translation vector
*/
void translate(Handle_group handle_group, const Vector& translation)
{
for(typename Handle_container::iterator it = handle_group->begin();
it != handle_group->end(); ++it)
{
std::size_t v_id = id(*it);
solution[v_id] = solution[v_id] + translation;
}
}
/**
* Rotate the handle group around rotation center by quaternion then translate it by translation
* @tparam Quaternion quaternion type which defines a multiplication operator with Vect as quad * vector
* @tparam Vect vector type 3 param constructable and has operator[] ...
*/
template <typename Quaternion, typename Vect>
void rotate(Handle_group handle_group, const Point& rotation_center, const Quaternion& quat, const Vect& translation)
{
for(typename Handle_container::iterator it = handle_group->begin();
it != handle_group->end(); ++it)
{
std::size_t v_id = id(*it);
Point p = CGAL::ORIGIN + ( original[v_id] - rotation_center);
Vect v = quat * Vect(p.x(),p.y(),p.z());
p = Point(v[0], v[1], v[2]) + ( rotation_center - CGAL::ORIGIN);
p = p + Vector(translation[0],translation[1],translation[2]);
solution[v_id] = p;
}
}
/**
* Assign positions in the range as target positions for the vertices in the handle group
* @tparam InputIterator input iterator type which points to Polyhedron::Traits::Point_3
* @param handle_group group of target vertices
* @param begin iterators spesifying the range of positions [begin, end)
* @param end iterators spesifying the range of positions [begin, end)
*/
template<class InputIterator>
void assign(Handle_group handle_group, InputIterator begin, InputIterator end)
{
for(typename Handle_container::iterator it = handle_group->begin();
(it != handle_group->end()) && (begin != end);
++it, ++begin)
{
solution[id(*it)] = *begin;
}
}
/**
* Assign the target position for the handle vertes
* @param vd handle vertex to be assigned target position
* @param target_position constrained position
*/
void assign(vertex_descriptor vd, const Point& target_position)
{
solution[id(vd)] = target_position;
}
/**
* Reset position of deformed vertices to their original positions (i.e. positions at the time of last preprocess() call)
*/
void undo()
{
for(std::size_t i = 0; i < ros.size(); ++i)
{
ros[i]->point() = original[id(ros[i])];
}
}
///////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////// Deformation Core ///////////////////////////////////////////
/**
* Necessary precomputation work before beginning deformation.
* It needs to be called after insertion of vertices as handles or roi is done.
* @return true if Laplacian matrix factorization is successful
*/
bool preprocess()
{
need_preprocess = false;
region_of_solution();
compute_edge_weight(); // compute_edge_weight() has to come later then region_of_solution()
// Assemble linear system A*X=B
typename SparseLinearAlgebraTraits_d::Matrix A(ros.size()); // matrix is definite positive, and not necessarily symmetric
assemble_laplacian(A);
// Pre-factorizing the linear system A*X=B
double D;
return m_solver.pre_factor(A, D);
}
/**
* Deformation on roi vertices. Default iteration and tolerance values are used.
* @see set_iterations(unsigned int iterations), set_tolerance(double tolerance), deform(unsigned int iterations, double tolerance)
*/
void deform()
{
deform(iterations, tolerance);
}
/**
* Deformation on roi vertices,
*/
void deform(unsigned int iterations, double tolerance)
{
CGAL_precondition(!need_preprocess || !"preprocess() need to be called before deforming!");
// Note: no energy based termination occurs at first iteration
// because comparing energy of original model (before deformation) and deformed model (deformed_1_iteration)
// simply does not make sense, comparison is meaningful between deformed_(i)_iteration & deformed_(i+1)_iteration
double energy_this = 0; // initial value is not important, because we skip first iteration
double energy_last;
// iterations
for ( unsigned int ite = 0; ite < iterations; ++ite)
{
// main steps of optimization
update_solution();
optimal_rotations_svd();
// energy based termination
if(tolerance > 0.0 && (ite + 1) < iterations) // if tolerance <= 0 then don't compute energy
{ // also no need compute energy if this iteration is the last iteration
energy_last = energy_this;
energy_this = energy();
if(energy_this < 0)
{
std::cout << "Negative energy" << std::endl;
}
if(ite != 0) // skip first iteration
{
double energy_dif = std::abs((energy_last - energy_this) / energy_this);
if ( energy_dif < tolerance ) { break; }
}
}
}
// copy solution to target mesh
assign_solution();
}
private:
/// Compute cotangent weights of all edges
void compute_edge_weight()
{
#if defined(CGAL_DEFORM_SPOKES_AND_RIMS)
compute_edge_weight_spokes_and_rims();
#else
compute_edge_weight_arap();
#endif
}
void compute_edge_weight_arap()
{
std::set<edge_descriptor> have_id; // edges which has assigned ids (and also weights are calculated)
// iterate over ros vertices and calculate weights for edges which are incident to ros
std::size_t next_edge_id = 0;
for (std::size_t i = 0; i < ros.size(); i++)
{
vertex_descriptor vi = ros[i];
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
typename std::set<edge_descriptor>::iterator it = have_id.find(*e);
if(it != have_id.end()) { continue; } // we have assigned an id already, which means we also calculted the weight
put(edge_index_map, *e, next_edge_id++);
have_id.insert(*e);
double weight = weight_calculator(*e);
edge_weight.push_back(weight);
}// end of edge loop
}// end of ros loop
}
void compute_edge_weight_spokes_and_rims()
{
std::set<edge_descriptor> have_id; // edges which has assigned ids (and also weights are calculated)
// iterate over ros vertices and calculate weights for edges which are incident to ros
std::size_t next_edge_id = 0;
for (std::size_t i = 0; i < ros.size(); i++)
{
vertex_descriptor vi = ros[i];
out_edge_iterator e_begin, e_end;
boost::tie(e_begin, e_end) = boost::out_edges(vi, polyhedron);
for (Rims_iterator rims_it(e_begin, polyhedron); rims_it.get_iterator() != e_end; ++rims_it )
{
edge_descriptor active_edge = rims_it.get_descriptor();
typename std::set<edge_descriptor>::iterator it = have_id.find(active_edge);
if(it == have_id.end()) // we have not assigned an id yet
{
put(edge_index_map, active_edge, next_edge_id++);
have_id.insert(active_edge);
double wji = weight_calculator(active_edge); // edge(pj - pi)
edge_weight.push_back(wji);
edge_descriptor opp = CGAL::opposite_edge(active_edge, polyhedron);
put(edge_index_map, opp, next_edge_id++);
have_id.insert(opp);
double wij = weight_calculator(opp); // edge(pi - pj)
edge_weight.push_back(wij);
}
}// end of edge loop
}// end of ros loop
}
/// Assigns id to one rign neighbor of vd, and also push them into push_vector
void assign_id_to_one_ring(vertex_descriptor vd,
std::size_t& next_id,
std::vector<vertex_descriptor>& push_vector,
std::set<vertex_descriptor>& have_id)
{
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vd, polyhedron); e != e_end; e++)
{
vertex_descriptor vt = boost::source(*e, polyhedron);
typename std::set<vertex_descriptor>::iterator it = have_id.find(vt);
if( it == have_id.end() ) // neighboring vertex which is outside of roi and not visited previously (i.e. need an id)
{
put(vertex_index_map, vt, next_id++);
have_id.insert(vt);
push_vector.push_back(vt);
}
}
}
/// Find region of solution, including roi and hard constraints, which is the 1-ring vertices out roi
void region_of_solution()
{
// Important: at this point ros contains the roi vertices only.
// copy roi vertices to roi vector
std::vector<vertex_descriptor> roi; // we can remove this temp, but I try to simplify things below
roi.insert(roi.end(), ros.begin(), ros.end());
////////////////////////////////////////////////////////////////
// assign id to vertices inside: roi, boundary of roi (roi + boundary of roi = ros),
// and boundary of ros
// keep in mind that id order does not have to be compatible with ros order
std::set<vertex_descriptor> have_id; // keep vertices which are assigned an id
for(std::size_t i = 0; i < roi.size(); i++) // assign id to all roi vertices
{
put(vertex_index_map, roi[i], i);
}
have_id.insert(roi.begin(), roi.end()); // mark roi vertices since they have ids now
// now assign an id to vertices on boundary of roi
std::size_t next_ros_index = roi.size();
for(std::size_t i = 0; i < roi.size(); i++)
{
assign_id_to_one_ring(roi[i], next_ros_index, ros, have_id);
}
std::vector<vertex_descriptor> outside_ros;
// boundary of ros also must have ids because in SVD calculation,
// one-ring neighbor of ROS vertices are reached.
for(std::size_t i = roi.size(); i < ros.size(); i++)
{
assign_id_to_one_ring(ros[i], next_ros_index, outside_ros, have_id);
}
////////////////////////////////////////////////////////////////
// initialize the rotation matrices (size: ros)
rot_mtr.resize(ros.size());
for(std::size_t i = 0; i < rot_mtr.size(); i++)
{
rot_mtr[i].setIdentity();
}
// initialize solution and original (size: ros + boundary_of_ros)
// for simplifying coding effort, I also put boundary of ros into solution and original
// because boundary of ros vertices are reached in optimal_rotations_svd() and energy()
solution.resize(ros.size() + outside_ros.size());
original.resize(ros.size() + outside_ros.size());
for(std::size_t i = 0; i < ros.size(); i++)
{
std::size_t v_id = id(ros[i]);
solution[v_id] = ros[i]->point();
original[v_id] = ros[i]->point();
}
for(std::size_t i = 0; i < outside_ros.size(); ++i)
{
std::size_t v_id = id(outside_ros[i]);
original[v_id] = outside_ros[i]->point();
solution[v_id] = outside_ros[i]->point();
}
// initialize flag vectors of roi, handle, ros
is_roi.assign(ros.size(), false);
is_hdl.assign(ros.size(), false);
for(std::size_t i = 0; i < roi.size(); i++)
{
std::size_t v_id = id(roi[i]);
is_roi[v_id] = true;
}
for(typename Handle_group_container::iterator it_group = handle_groups.begin();
it_group != handle_groups.end(); ++it_group)
{
for(typename Handle_container::iterator it_vertex = it_group->begin();
it_vertex != it_group->end(); ++it_vertex)
{
std::size_t v_id = id(*it_vertex);
is_hdl[v_id] = true;
}
}
}
/// Assemble Laplacian matrix A of linear system A*X=B
void assemble_laplacian(typename SparseLinearAlgebraTraits_d::Matrix& A)
{
#if defined(CGAL_DEFORM_SPOKES_AND_RIMS)
assemble_laplacian_spokes_and_rims(A);
#else
assemble_laplacian_arap(A);
#endif
}
/// Construct matrix that corresponds to left-hand side of eq:lap_ber in user manual
/// Also constraints are integrated as eq:lap_energy_system in user manual
void assemble_laplacian_arap(typename SparseLinearAlgebraTraits_d::Matrix& A)
{
/// assign cotangent Laplacian to ros vertices
for(std::size_t k = 0; k < ros.size(); k++)
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
if ( is_roi[vi_id] && !is_hdl[vi_id] ) // vertices of ( roi - hdl )
{
double diagonal = 0;
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::source(*e, polyhedron);
double wij = edge_weight[id(*e)]; // edge(pi - pj)
double wji = edge_weight[id(CGAL::opposite_edge(*e, polyhedron))]; // edge(pi - pj)
double total_weight = wij + wji;
A.set_coef(vi_id, id(vj), -total_weight, true); // off-diagonal coefficient
diagonal += total_weight;
}
// diagonal coefficient
A.set_coef(vi_id, vi_id, diagonal, true);
}
else
{
A.set_coef(vi_id, vi_id, 1.0, true);
}
}
}
/// Construct matrix that corresponds to left-hand side of eq:lap_ber_rims in user manual
/// Also constraints are integrated as eq:lap_energy_system in user manual
void assemble_laplacian_spokes_and_rims(typename SparseLinearAlgebraTraits_d::Matrix& A)
{
/// assign cotangent Laplacian to ros vertices
for(std::size_t k = 0; k < ros.size(); k++)
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
if ( is_roi[vi_id] && !is_hdl[vi_id] ) // vertices of ( roi - hdl ): free vertices
{
double diagonal = 0;
out_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::out_edges(vi, polyhedron); e != e_end; e++)
{
double total_weight = 0;
// an edge contribute to energy only if it is part of an incident triangle
// (i.e it should not be a border edge)
if(!boost::get(CGAL::edge_is_border, polyhedron, *e))
{
double wji = edge_weight[id(*e)]; // edge(pj - pi)
total_weight += wji;
}
edge_descriptor opp = CGAL::opposite_edge(*e, polyhedron);
if(!boost::get(CGAL::edge_is_border, polyhedron, opp))
{
double wij = edge_weight[id(opp)]; // edge(pi - pj)
total_weight += wij;
}
// place coefficient to matrix
vertex_descriptor vj = boost::target(*e, polyhedron);
A.set_coef(vi_id, id(vj), -total_weight, true); // off-diagonal coefficient
diagonal += total_weight;
}
// diagonal coefficient
A.set_coef(vi_id, vi_id, diagonal, true);
}
else // constrained vertex
{
A.set_coef(vi_id, vi_id, 1.0, true);
}
}
}
/// Local step of iterations, computing optimal rotation matrices using SVD decomposition, stable
void optimal_rotations_svd()
{
#if defined(CGAL_DEFORM_SPOKES_AND_RIMS)
optimal_rotations_svd_spokes_and_rims();
#else
optimal_rotations_svd_arap();
#endif
}
void optimal_rotations_svd_arap()
{
Eigen::Matrix3d cov; // covariance matrix
Eigen::JacobiSVD<Eigen::Matrix3d> svd; // SVD solver
// only accumulate ros vertices
for ( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
// compute covariance matrix (user manual eq:cov_matrix)
cov.setZero();
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::source(*e, polyhedron);
std::size_t vj_id = id(vj);
Eigen::Vector3d pij = sub_to_col(original[vi_id], original[vj_id]);
Eigen::RowVector3d qij = sub_to_row(solution[vi_id], solution[vj_id]);
double wij = edge_weight[id(*e)];
cov += wij * (pij * qij);
}
// svd decomposition
svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
const Eigen::Matrix3d& u = svd.matrixU();
const Eigen::Matrix3d& v = svd.matrixV();
// extract rotation matrix
rot_mtr[vi_id] = v*u.transpose();
// checking negative determinant of r
if ( rot_mtr[vi_id].determinant() < 0 ) // changing the sign of column corresponding to smallest singular value
{
Eigen::Matrix3d u_m = u;
u_m.col(2) *= -1; // singular values are always sorted in decresing order so use column 2
rot_mtr[vi_id] = v*u_m.transpose(); // re-extract rotation matrix
}
}
}
void optimal_rotations_svd_spokes_and_rims()
{
Eigen::Matrix3d cov; // covariance matrix
Eigen::JacobiSVD<Eigen::Matrix3d> svd; // SVD solver
// only accumulate ros vertices
for ( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
// compute covariance matrix
cov.setZero();
//iterate through all triangles
out_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::out_edges(vi, polyhedron); e != e_end; e++)
{
if(boost::get(CGAL::edge_is_border, polyhedron, *e)) { continue; } // no facet
// iterate edges around facet
edge_descriptor edge_around_facet = *e;
do
{
vertex_descriptor v1 = boost::target(edge_around_facet, polyhedron);
vertex_descriptor v2 = boost::source(edge_around_facet, polyhedron);
std::size_t v1_id = id(v1); std::size_t v2_id = id(v2);
Eigen::Vector3d p12 = sub_to_col(original[v1_id], original[v2_id]);
Eigen::RowVector3d q12 = sub_to_row(solution[v1_id], solution[v2_id]);
double w12 = edge_weight[id(edge_around_facet)];
cov += w12 * (p12 * q12);
} while( (edge_around_facet = CGAL::next_edge(edge_around_facet, polyhedron)) != *e);
}
// svd decomposition
svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
const Eigen::Matrix3d& u = svd.matrixU();
const Eigen::Matrix3d& v = svd.matrixV();
// extract rotation matrix
rot_mtr[vi_id] = v*u.transpose();
// checking negative determinant of r
if ( rot_mtr[vi_id].determinant() < 0 ) // changing the sign of column corresponding to smallest singular value
{
Eigen::Matrix3d u_m = u;
u_m.col(2) *= -1; // singular values are always sorted in decresing order so use column 2
rot_mtr[vi_id] = v*u_m.transpose(); // re-extract rotation matrix
}
}
}
/// Global step of iterations, updating solution
void update_solution()
{
#if defined(CGAL_DEFORM_SPOKES_AND_RIMS)
update_solution_spokes_and_rims();
#else
update_solution_arap();
#endif
}
void update_solution_arap()
{
typename SparseLinearAlgebraTraits_d::Vector X(ros.size()), Bx(ros.size());
typename SparseLinearAlgebraTraits_d::Vector Y(ros.size()), By(ros.size());
typename SparseLinearAlgebraTraits_d::Vector Z(ros.size()), Bz(ros.size());
// assemble right columns of linear system
for ( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
if ( is_roi[vi_id] && !is_hdl[vi_id] )
{// free vertices
Eigen::Vector3d xyz; xyz.setZero(); // sum of right-hand side of eq:lap_ber in user manual
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::source(*e, polyhedron);
std::size_t vj_id = id(vj);
Eigen::Vector3d pij = sub_to_col(original[vi_id], original[vj_id]);
double wij = edge_weight[id(*e)];
double wji = edge_weight[id(CGAL::opposite_edge(*e, polyhedron))];
xyz += (wij*rot_mtr[vi_id] + wji*rot_mtr[vj_id]) * pij;
}
Bx[vi_id] = xyz(0,0); By[vi_id] = xyz(1,0); Bz[vi_id] = xyz(2,0);
}
else
{// constrained vertex
Bx[vi_id] = solution[vi_id].x(); By[vi_id] = solution[vi_id].y(); Bz[vi_id] = solution[vi_id].z();
}
}
// solve "A*X = B".
m_solver.linear_solver(Bx, X); m_solver.linear_solver(By, Y); m_solver.linear_solver(Bz, Z);
// copy to solution
for (std::size_t i = 0; i < ros.size(); i++)
{
std::size_t v_id = id(ros[i]);
Point p(X[v_id], Y[v_id], Z[v_id]);
solution[v_id] = p;
}
}
void update_solution_spokes_and_rims()
{
typename SparseLinearAlgebraTraits_d::Vector X(ros.size()), Bx(ros.size());
typename SparseLinearAlgebraTraits_d::Vector Y(ros.size()), By(ros.size());
typename SparseLinearAlgebraTraits_d::Vector Z(ros.size()), Bz(ros.size());
// assemble right columns of linear system
for ( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
if ( is_roi[vi_id] && !is_hdl[vi_id] )
{// free vertices
Eigen::Vector3d xyz; xyz.setZero(); // sum of right-hand side of eq:lap_ber_rims in user manual
out_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::out_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::target(*e, polyhedron);
std::size_t vj_id = id(vj);
Eigen::Vector3d pij = sub_to_col(original[vi_id], original[vj_id]);
if(!boost::get(CGAL::edge_is_border, polyhedron, *e))
{
vertex_descriptor vn = boost::target(CGAL::next_edge(*e, polyhedron), polyhedron); // opp vertex of e_ij
double wji = edge_weight[id(*e)] / 3.0; // edge(pj - pi)
xyz += wji*(rot_mtr[vi_id] + rot_mtr[vj_id] + rot_mtr[id(vn)])*pij;
}
edge_descriptor opp = CGAL::opposite_edge(*e, polyhedron);
if(!boost::get(CGAL::edge_is_border, polyhedron, opp))
{
vertex_descriptor vm = boost::target(CGAL::next_edge(opp, polyhedron), polyhedron); // other opp vertex of e_ij
double wij = edge_weight[id(opp)] / 3.0; // edge(pi - pj)
xyz += wij*(rot_mtr[vi_id] + rot_mtr[vj_id] + rot_mtr[id(vm)])*pij;
}
}
Bx[vi_id] = xyz(0,0); By[vi_id] = xyz(1,0); Bz[vi_id] = xyz(2,0);
}
else
{// constrained vertices
Bx[vi_id] = solution[vi_id].x(); By[vi_id] = solution[vi_id].y(); Bz[vi_id] = solution[vi_id].z();
}
}
// solve "A*X = B".
m_solver.linear_solver(Bx, X); m_solver.linear_solver(By, Y); m_solver.linear_solver(Bz, Z);
// copy to solution
for (std::size_t i = 0; i < ros.size(); i++)
{
std::size_t v_id = id(ros[i]);
Point p(X[v_id], Y[v_id], Z[v_id]);
solution[v_id] = p;
}
}
/// Assign solution to target mesh
void assign_solution()
{
for(std::size_t i = 0; i < ros.size(); ++i){
std::size_t v_id = id(ros[i]);
if(is_roi[v_id])
{
ros[i]->point() = solution[v_id];
}
}
}
/// Compute modeling energy
double energy()
{
#ifdef CGAL_DEFORM_SPOKES_AND_RIMS
return energy_spokes_and_rims();
#else
return energy_arap();
#endif
}
double energy_arap()
{
double sum_of_energy = 0;
// only accumulate ros vertices
for( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::source(*e, polyhedron);
std::size_t vj_id = id(vj);
Eigen::Vector3d pij = sub_to_col(original[vi_id], original[vj_id]);
Eigen::Vector3d qij = sub_to_col(solution[vi_id], solution[vj_id]);
double wij = edge_weight[id(*e)];
sum_of_energy += wij * (qij - rot_mtr[vi_id]*pij).squaredNorm();
}
}
return sum_of_energy;
}
double energy_spokes_and_rims()
{
double sum_of_energy = 0;
// only accumulate ros vertices
for( std::size_t k = 0; k < ros.size(); k++ )
{
vertex_descriptor vi = ros[k];
std::size_t vi_id = id(vi);
//iterate through all triangles
out_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::out_edges(vi, polyhedron); e != e_end; e++)
{
if(boost::get(CGAL::edge_is_border, polyhedron, *e)) { continue; } // no facet
// iterate edges around facet
edge_descriptor edge_around_facet = *e;
do
{
vertex_descriptor v1 = boost::target(edge_around_facet, polyhedron);
vertex_descriptor v2 = boost::source(edge_around_facet, polyhedron);
std::size_t v1_id = id(v1); std::size_t v2_id = id(v2);
Eigen::Vector3d p12 = sub_to_col(original[v1_id], original[v2_id]);
Eigen::Vector3d q12 = sub_to_col(solution[v1_id], solution[v2_id]);
double w12 = edge_weight[id(edge_around_facet)];
sum_of_energy += w12 * (q12 - rot_mtr[vi_id]*p12).squaredNorm();
} while( (edge_around_facet = CGAL::next_edge(edge_around_facet, polyhedron)) != *e);
}
}
return sum_of_energy;
}
/// p1 - p2, return Eigen Column Vector
Eigen::Vector3d sub_to_col(const Point& p1, const Point& p2)
{
return Eigen::Vector3d(p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
}
/// p1 - p2, return Eigen Row Vector
Eigen::RowVector3d sub_to_row(const Point& p1, const Point& p2)
{
return Eigen::RowVector3d(p1.x() - p2.x(), p1.y() - p2.y(), p1.z() - p2.z());
}
/// shorthand of get(vertex_index_map, v)
std::size_t id(vertex_descriptor v)
{
return get(vertex_index_map, v);
}
/// shorthand of get(edge_index_map, e)
std::size_t id(edge_descriptor e)
{
return get(edge_index_map, e);
}
#ifdef CGAL_DEFORM_EXPERIMENTAL // Experimental stuff, needs further testing
double norm_1(const Eigen::Matrix3d& X)
{
double sum = 0;
for ( int i = 0; i < 3; i++ )
{
for ( int j = 0; j < 3; j++ )
{
sum += abs(X(i,j));
}
}
return sum;
}
double norm_inf(const Eigen::Matrix3d& X)
{
double max_abs = abs(X(0,0));
for ( int i = 0; i < 3; i++ )
{
for ( int j = 0; j < 3; j++ )
{
double new_abs = abs(X(i,j));
if ( new_abs > max_abs )
{
max_abs = new_abs;
}
}
}
return max_abs;
}
// polar decomposition using Newton's method, with warm start, stable but slow
// not used, need to be investigated later
void polar_newton(const Eigen::Matrix3d& A, Eigen::Matrix3d &U, double tole)
{
Eigen::Matrix3d X = A;
Eigen::Matrix3d Y;
double alpha, beta, gamma;
do
{
Y = X.inverse();
alpha = sqrt( norm_1(X) * norm_inf(X) );
beta = sqrt( norm_1(Y) * norm_inf(Y) );
gamma = sqrt(beta/alpha);
X = 0.5*( gamma*X + Y.transpose()/gamma );
} while ( abs(gamma-1) > tole );
U = X;
}
// polar decomposition using Eigen, 5 times faster than SVD
template<typename Mat>
void polar_eigen(const Mat& A, Mat& R, bool& SVD)
{
typedef typename Mat::Scalar Scalar;
typedef Eigen::Matrix<typename Mat::Scalar,3,1> Vec;
const Scalar th = std::sqrt(Eigen::NumTraits<Scalar>::dummy_precision());
Eigen::SelfAdjointEigenSolver<Mat> eig;
feclearexcept(FE_UNDERFLOW);
eig.computeDirect(A.transpose()*A);
if(fetestexcept(FE_UNDERFLOW) || eig.eigenvalues()(0)/eig.eigenvalues()(2)<th)
{
// The computation of the eigenvalues might have diverged.
// Fallback to an accurate SVD based decomposiiton method.
Eigen::JacobiSVD<Mat> svd;
svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV();
R = u*v.transpose();
SVD = true;
return;
}
Vec S = eig.eigenvalues().cwiseSqrt();
R = A * eig.eigenvectors() * S.asDiagonal().inverse()
* eig.eigenvectors().transpose();
SVD = false;
if(std::abs(R.squaredNorm()-3.) > th)
{
// The computation of the eigenvalues might have diverged.
// Fallback to an accurate SVD based decomposiiton method.
Eigen::JacobiSVD<Mat> svd;
svd.compute(A, Eigen::ComputeFullU | Eigen::ComputeFullV );
const Mat& u = svd.matrixU(); const Mat& v = svd.matrixV();
R = u*v.transpose();
SVD = true;
return;
}
}
// Local step of iterations, computing optimal rotation matrices using Polar decomposition
void optimal_rotations_polar()
{
Eigen::Matrix3d u, v; // orthogonal matrices
Eigen::Vector3d w; // singular values
Eigen::Matrix3d cov; // covariance matrix
Eigen::Matrix3d r;
Eigen::JacobiSVD<Eigen::Matrix3d> svd; // SVD solver, for non-positive covariance matrices
int num_svd = 0;
bool SVD = false;
// only accumulate ros vertices
for ( std::size_t i = 0; i < ros.size(); i++ )
{
vertex_descriptor vi = ros[i];
// compute covariance matrix
cov.setZero();
in_edge_iterator e, e_end;
for (boost::tie(e,e_end) = boost::in_edges(vi, polyhedron); e != e_end; e++)
{
vertex_descriptor vj = boost::source(*e, polyhedron);
Vector pij = original[get(vertex_index_map, vi)] - original[get(vertex_index_map, vj)];
Vector qij = solution[get(vertex_index_map, vi)] - solution[get(vertex_index_map, vj)];
double wij = edge_weight[get(edge_index_map, *e)];
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
cov(j, k) += wij*pij[j]*qij[k];
}
}
}
// svd decomposition
if (cov.determinant() > 0)
{
polar_eigen<Eigen::Matrix3d> (cov, r, SVD);
//polar_newton(cov, r, 1e-4);
if(SVD)
num_svd++;
r.transposeInPlace(); // the optimal rotation matrix should be transpose of decomposition result
}
else
{
svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
r = v*u.transpose();
num_svd++;
}
// checking negative determinant of covariance matrix
if ( r.determinant() < 0 ) // back to SVD method
{
if (cov.determinant() > 0)
{
svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
num_svd++;
}
for (int j = 0; j < 3; j++)
{
int j0 = j;
int j1 = (j+1)%3;
int j2 = (j1+1)%3;
if ( w[j0] <= w[j1] && w[j0] <= w[j2] ) // smallest singular value as j0
{
u(0, j0) = - u(0, j0);
u(1, j0) = - u(1, j0);
u(2, j0) = - u(2, j0);
break;
}
}
// re-extract rotation matrix
r = v*u.transpose();
}
rot_mtr[i] = r;
}
double svd_percent = (double)(num_svd)/ros.size();
CGAL_TRACE_STREAM << svd_percent*100 << "% percentage SVD decompositions;";
CGAL_TRACE_STREAM << num_svd << " SVD decompositions\n";
}
#endif
};
} //namespace CGAL
#endif // CGAL_DEFORM_MESH_H
Reference in New Issue View Git Blame Copy Permalink