treat gradient zeros at residual!=0
This commit is contained in:
parent
fcea39b0d8
commit
913ec86fdd
|
@ -47,7 +47,9 @@
|
|||
#include "traits.hpp"
|
||||
#include "object.hpp"
|
||||
#include "equations.hpp"
|
||||
#include <Base/Console.h>
|
||||
|
||||
class T;
|
||||
namespace mpl = boost::mpl;
|
||||
namespace fusion = boost::fusion;
|
||||
|
||||
|
@ -86,6 +88,7 @@ protected:
|
|||
void resetType(creator_type& c);
|
||||
|
||||
void calculate(Scalar scale, bool rotation_only = false);
|
||||
void treatLGZ();
|
||||
|
||||
void setMaps(MES& mes);
|
||||
|
||||
|
@ -118,6 +121,7 @@ protected:
|
|||
virtual ~placeholder() {}
|
||||
virtual placeholder* resetConstraint(geom_ptr first, geom_ptr second) const = 0;
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false) = 0;
|
||||
virtual void treatLGZ(geom_ptr first, geom_ptr second) = 0;
|
||||
virtual int equationCount() = 0;
|
||||
virtual void setMaps(MES& mes, geom_ptr first, geom_ptr second) = 0;
|
||||
virtual void collectPseudoPoints(geom_ptr first, geom_ptr second, Vec& vec1, Vec& vec2) = 0;
|
||||
|
@ -202,6 +206,15 @@ public:
|
|||
void operator()(T& val) const;
|
||||
};
|
||||
|
||||
struct LGZ {
|
||||
geom_ptr first,second;
|
||||
|
||||
LGZ(geom_ptr f, geom_ptr s);
|
||||
|
||||
template< typename T >
|
||||
void operator()(T& val) const;
|
||||
};
|
||||
|
||||
struct GenericEquations {
|
||||
std::vector<boost::any>& vec;
|
||||
GenericEquations(std::vector<boost::any>& v);
|
||||
|
@ -230,6 +243,7 @@ public:
|
|||
holder(Objects& obj);
|
||||
|
||||
virtual void calculate(geom_ptr first, geom_ptr second, Scalar scale, bool rotation_only = false);
|
||||
virtual void treatLGZ(geom_ptr first, geom_ptr second);
|
||||
virtual placeholder* resetConstraint(geom_ptr first, geom_ptr second) const;
|
||||
virtual void setMaps(MES& mes, geom_ptr first, geom_ptr second);
|
||||
virtual void collectPseudoPoints(geom_ptr f, geom_ptr s, Vec& vec1, Vec& vec2);
|
||||
|
@ -328,7 +342,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::initialize(ConstraintVect
|
|||
//and now store it
|
||||
content = c.p;
|
||||
//geometry order needs to be the one needed by equations
|
||||
if(c.need_swap) first.swap(second);
|
||||
if(c.need_swap)
|
||||
first.swap(second);
|
||||
|
||||
};
|
||||
|
||||
|
@ -342,7 +357,8 @@ template< typename creator_type>
|
|||
void Constraint<Sys, Derived, Signals, MES, Geometry>::resetType(creator_type& c) {
|
||||
boost::apply_visitor(c, first->m_geometry, second->m_geometry);
|
||||
content = c.p;
|
||||
if(c.need_swap) first.swap(second);
|
||||
if(c.need_swap)
|
||||
first.swap(second);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
@ -350,6 +366,11 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::calculate(Scalar scale, b
|
|||
content->calculate(first, second, scale, rotation_only);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::treatLGZ() {
|
||||
content->treatLGZ(first, second);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::setMaps(MES& mes) {
|
||||
content->setMaps(mes, first, second);
|
||||
|
@ -427,7 +448,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
val.m_diff_first_rot.setZero();
|
||||
val.m_diff_first.setZero();
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
val.m_diff_first.setZero();
|
||||
|
||||
if(second->getClusterMode()) {
|
||||
|
@ -435,7 +457,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
val.m_diff_second_rot.setZero();
|
||||
val.m_diff_second.setZero();
|
||||
}
|
||||
} else
|
||||
}
|
||||
else
|
||||
val.m_diff_second.setZero();
|
||||
|
||||
}
|
||||
|
@ -465,7 +488,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
second->m_parameter, block);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientFirstComplete(first->m_parameter, second->m_parameter, val.m_diff_first);
|
||||
}
|
||||
|
@ -487,7 +511,8 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
second->m_parameter, block);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
//not in cluster, so allow the constraint to optimize the gradient calculation
|
||||
val.m_eq.calculateGradientSecondComplete(first->m_parameter, second->m_parameter, val.m_diff_second);
|
||||
}
|
||||
|
@ -515,7 +540,9 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
mes.setJacobiMap(equation, first->m_offset_rot, 3, val.m_diff_first_rot);
|
||||
mes.setJacobiMap(equation, first->m_offset, 3, val.m_diff_first);
|
||||
}
|
||||
} else mes.setJacobiMap(equation, first->m_offset, first->m_parameterCount, val.m_diff_first);
|
||||
}
|
||||
else
|
||||
mes.setJacobiMap(equation, first->m_offset, first->m_parameterCount, val.m_diff_first);
|
||||
|
||||
|
||||
if(second->getClusterMode()) {
|
||||
|
@ -523,7 +550,9 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
mes.setJacobiMap(equation, second->m_offset_rot, 3, val.m_diff_second_rot);
|
||||
mes.setJacobiMap(equation, second->m_offset, 3, val.m_diff_second);
|
||||
}
|
||||
} else mes.setJacobiMap(equation, second->m_offset, second->m_parameterCount, val.m_diff_second);
|
||||
}
|
||||
else
|
||||
mes.setJacobiMap(equation, second->m_offset, second->m_parameterCount, val.m_diff_second);
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
|
@ -539,15 +568,92 @@ template< typename T >
|
|||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::PseudoCollector::operator()(T& val) const {
|
||||
if(first->m_isInCluster && second->m_isInCluster) {
|
||||
val.m_eq.calculatePseudo(first->m_rotated, points1, second->m_rotated, points2);
|
||||
} else if(first->m_isInCluster) {
|
||||
typename Kernel::Vector sec = second->m_parameter;
|
||||
val.m_eq.calculatePseudo(first->m_rotated, points1, sec, points2);
|
||||
} else if(second->m_isInCluster) {
|
||||
typename Kernel::Vector fir = first->m_parameter;
|
||||
val.m_eq.calculatePseudo(fir, points1, second->m_rotated, points2);
|
||||
}
|
||||
else
|
||||
if(first->m_isInCluster) {
|
||||
typename Kernel::Vector sec = second->m_parameter;
|
||||
val.m_eq.calculatePseudo(first->m_rotated, points1, sec, points2);
|
||||
}
|
||||
else
|
||||
if(second->m_isInCluster) {
|
||||
typename Kernel::Vector fir = first->m_parameter;
|
||||
val.m_eq.calculatePseudo(fir, points1, second->m_rotated, points2);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::LGZ::LGZ(geom_ptr f, geom_ptr s)
|
||||
: first(f), second(s) {
|
||||
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
template< typename T >
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::LGZ::operator()(T& val) const {
|
||||
|
||||
//to treat local gradient zeros we calculate a approximate second derivative of the equations
|
||||
//only do that if neseccary: residual is not zero
|
||||
Base::Console().Message("res: %f\n", val.m_residual(0));
|
||||
if(val.m_residual(0) > 1e-7) { //TODO: use exact precission and scale value
|
||||
|
||||
//rotations exist only in cluster
|
||||
if(first->getClusterMode() && !first->isClusterFixed()) {
|
||||
//LGZ exists for rotations only
|
||||
for(int i=0; i<3; i++) {
|
||||
|
||||
//only treat if the gradient realy is zero
|
||||
Base::Console().Message("local grad: %f\n", val.m_diff_first_rot(i));
|
||||
if(std::abs(val.m_diff_first_rot(i)) < 1e-7) {
|
||||
|
||||
//to get the approximated second derivative we need the slightly moved geometrie
|
||||
typename Kernel::Vector incr = first->m_parameter + first->m_diffparam.col(i)*1e-3;
|
||||
//with this changed geometrie we test if a gradient exist now
|
||||
typename Kernel::VectorMap block(&first->m_diffparam(0,i),first->m_parameterCount,1, DS(1,1));
|
||||
typename Kernel::VectorMap block2(&incr(0),first->m_parameterCount,1, DS(1,1));
|
||||
typename Kernel::number_type res = val.m_eq.calculateGradientFirst(block2,
|
||||
second->m_parameter, block);
|
||||
|
||||
//let's see if the initial LGZ was a real one
|
||||
Base::Console().Message("approx second: %f\n", res);
|
||||
if(std::abs(res) > 1e-7) {
|
||||
|
||||
//is a fake zero, let's correct it
|
||||
val.m_diff_first_rot(i) = res;
|
||||
};
|
||||
};
|
||||
};
|
||||
}
|
||||
//and the same for the second one too
|
||||
if(second->getClusterMode() && !second->isClusterFixed()) {
|
||||
|
||||
for(int i=0; i<3; i++) {
|
||||
|
||||
//only treat if the gradient realy is zero
|
||||
Base::Console().Message("local grad: %f\n", val.m_diff_second_rot(i));
|
||||
if(std::abs(val.m_diff_second_rot(i)) < 1e-7) {
|
||||
|
||||
//to get the approximated second derivative we need the slightly moved geometrie
|
||||
typename Kernel::Vector incr = second->m_parameter + second->m_diffparam.col(i)*1e-3;
|
||||
//with this changed geometrie we test if a gradient exist now
|
||||
typename Kernel::VectorMap block(&second->m_diffparam(0,i),second->m_parameterCount,1, DS(1,1));
|
||||
typename Kernel::VectorMap block2(&incr(0),second->m_parameterCount,1, DS(1,1));
|
||||
typename Kernel::number_type res = val.m_eq.calculateGradientFirst(block2,
|
||||
second->m_parameter, block);
|
||||
|
||||
//let's see if the initial LGZ was a real one
|
||||
Base::Console().Message("approx second: %f\n", res);
|
||||
if(std::abs(res) > 1e-7) {
|
||||
|
||||
//is a fake zero, let's correct it
|
||||
val.m_diff_second_rot(i) = res;
|
||||
};
|
||||
};
|
||||
};
|
||||
};
|
||||
};
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
|
@ -605,6 +711,12 @@ void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector,
|
|||
fusion::for_each(m_sets, Calculater(first, second, scale, rotation_only));
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
void Constraint<Sys, Derived, Signals, MES, Geometry>::holder<ConstraintVector, EquationVector>::treatLGZ(geom_ptr first, geom_ptr second) {
|
||||
fusion::for_each(m_sets, LGZ(first, second));
|
||||
};
|
||||
|
||||
template<typename Sys, typename Derived, typename Signals, typename MES, typename Geometry>
|
||||
template<typename ConstraintVector, typename EquationVector>
|
||||
typename Constraint<Sys, Derived, Signals, MES, Geometry>::placeholder*
|
||||
|
|
|
@ -99,44 +99,48 @@ struct Dogleg {
|
|||
// compute the dogleg step
|
||||
if(h_gn.norm() <= delta) {
|
||||
h_dl = h_gn;
|
||||
} else if((alpha*h_sd).norm() >= delta) {
|
||||
//h_dl = alpha*h_sd;
|
||||
h_dl = (delta/(h_sd.norm()))*h_sd;
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(h_dl.norm())) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg descent detected: "<<h_dl.norm();
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
//compute beta
|
||||
number_type beta = 0;
|
||||
typename Kernel::Vector a = alpha*h_sd;
|
||||
typename Kernel::Vector b = h_gn;
|
||||
number_type c = a.transpose()*(b-a);
|
||||
number_type bas = (b-a).squaredNorm(), as = a.squaredNorm();
|
||||
if(c<0) {
|
||||
beta = -c+std::sqrt(std::pow(c,2)+bas*(std::pow(delta,2)-as));
|
||||
beta /= bas;
|
||||
} else {
|
||||
beta = std::pow(delta,2)-as;
|
||||
beta /= c+std::sqrt(std::pow(c,2) + bas*(std::pow(delta,2)-as));
|
||||
};
|
||||
|
||||
// and update h_dl and dL with beta
|
||||
h_dl = alpha*h_sd + beta*(b-a);
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(c)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg c detected: "<<c;
|
||||
}
|
||||
if(!boost::math::isfinite(bas)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg bas detected: "<<bas;
|
||||
}
|
||||
if(!boost::math::isfinite(beta)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg beta detected: "<<beta;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else
|
||||
if((alpha*h_sd).norm() >= delta) {
|
||||
//h_dl = alpha*h_sd;
|
||||
h_dl = (delta/(h_sd.norm()))*h_sd;
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(h_dl.norm())) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg descent detected: "<<h_dl.norm();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else {
|
||||
//compute beta
|
||||
number_type beta = 0;
|
||||
typename Kernel::Vector a = alpha*h_sd;
|
||||
typename Kernel::Vector b = h_gn;
|
||||
number_type c = a.transpose()*(b-a);
|
||||
number_type bas = (b-a).squaredNorm(), as = a.squaredNorm();
|
||||
if(c<0) {
|
||||
beta = -c+std::sqrt(std::pow(c,2)+bas*(std::pow(delta,2)-as));
|
||||
beta /= bas;
|
||||
}
|
||||
else {
|
||||
beta = std::pow(delta,2)-as;
|
||||
beta /= c+std::sqrt(std::pow(c,2) + bas*(std::pow(delta,2)-as));
|
||||
};
|
||||
|
||||
// and update h_dl and dL with beta
|
||||
h_dl = alpha*h_sd + beta*(b-a);
|
||||
|
||||
#ifdef USE_LOGGING
|
||||
if(!boost::math::isfinite(c)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg c detected: "<<c;
|
||||
}
|
||||
if(!boost::math::isfinite(bas)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg bas detected: "<<bas;
|
||||
}
|
||||
if(!boost::math::isfinite(beta)) {
|
||||
BOOST_LOG(log)<< "Unnormal dogleg beta detected: "<<beta;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
};
|
||||
|
||||
int solve(typename Kernel::MappedEquationSystem& sys) {
|
||||
|
@ -190,16 +194,21 @@ struct Dogleg {
|
|||
// check if finished
|
||||
if(fx_inf <= tolf*sys.Scaling) // Success
|
||||
stop = 1;
|
||||
else if(g_inf <= tolg)
|
||||
throw solving_error() << boost::errinfo_errno(2) << error_message("g infinity norm smaller below limit");
|
||||
else if(delta <= tolx)
|
||||
throw solving_error() << boost::errinfo_errno(3) << error_message("step size below limit");
|
||||
else if(iter >= maxIterNumber)
|
||||
throw solving_error() << boost::errinfo_errno(4) << error_message("maximal iterations reached");
|
||||
else if(!boost::math::isfinite(err))
|
||||
throw solving_error() << boost::errinfo_errno(5) << error_message("error is inf or nan");
|
||||
else if(err > diverging_lim)
|
||||
throw solving_error() << boost::errinfo_errno(6) << error_message("error diverged");
|
||||
else
|
||||
if(g_inf <= tolg)
|
||||
throw solving_error() << boost::errinfo_errno(2) << error_message("g infinity norm smaller below limit");
|
||||
else
|
||||
if(delta <= tolx)
|
||||
throw solving_error() << boost::errinfo_errno(3) << error_message("step size below limit");
|
||||
else
|
||||
if(iter >= maxIterNumber)
|
||||
throw solving_error() << boost::errinfo_errno(4) << error_message("maximal iterations reached");
|
||||
else
|
||||
if(!boost::math::isfinite(err))
|
||||
throw solving_error() << boost::errinfo_errno(5) << error_message("error is inf or nan");
|
||||
else
|
||||
if(err > diverging_lim)
|
||||
throw solving_error() << boost::errinfo_errno(6) << error_message("error diverged");
|
||||
|
||||
|
||||
// see if we are already finished
|
||||
|
@ -210,6 +219,10 @@ struct Dogleg {
|
|||
number_type dF=0, dL=0;
|
||||
number_type rho;
|
||||
|
||||
//handle possible lgz's
|
||||
if(iter==0)
|
||||
sys.removeLocalGradientZeros();
|
||||
|
||||
//get the update step
|
||||
calculateStep(g, sys.Jacobi, sys.Residual, h_dl, delta);
|
||||
|
||||
|
@ -238,15 +251,18 @@ struct Dogleg {
|
|||
dF = err - err_new;
|
||||
rho = dF/dL;
|
||||
|
||||
if(dF<=0 || dL<=0) rho = -1;
|
||||
if(dF<=0 || dL<=0)
|
||||
rho = -1;
|
||||
// update delta
|
||||
if(rho>0.85) {
|
||||
delta = std::max(delta,2*h_dl.norm());
|
||||
nu = 2;
|
||||
} else if(rho < 0.25) {
|
||||
delta = delta/nu;
|
||||
nu = 2*nu;
|
||||
}
|
||||
else
|
||||
if(rho < 0.25) {
|
||||
delta = delta/nu;
|
||||
nu = 2*nu;
|
||||
}
|
||||
|
||||
if(dF > 0 && dL > 0) {
|
||||
|
||||
|
@ -259,11 +275,12 @@ struct Dogleg {
|
|||
sys.recalculate();
|
||||
}
|
||||
//it can also happen that the differentials get too small, however, we cant check for that
|
||||
else if(iter>1 && (counter>50)) {
|
||||
rescale();
|
||||
sys.recalculate();
|
||||
counter = 0;
|
||||
}
|
||||
else
|
||||
if(iter>1 && (counter>50)) {
|
||||
rescale();
|
||||
sys.recalculate();
|
||||
counter = 0;
|
||||
}
|
||||
|
||||
F_old = sys.Residual;
|
||||
J_old = sys.Jacobi;
|
||||
|
@ -275,7 +292,8 @@ struct Dogleg {
|
|||
g_inf = g.template lpNorm<E::Infinity>();
|
||||
fx_inf = sys.Residual.template lpNorm<E::Infinity>();
|
||||
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
sys.Residual = F_old;
|
||||
sys.Jacobi = J_old;
|
||||
sys.Parameter -= h_dl;
|
||||
|
@ -401,7 +419,8 @@ struct Kernel {
|
|||
new(&map) VectorMap(&m_parameter(m_param_rot_offset), number, DynStride(1,1));
|
||||
m_param_rot_offset += number;
|
||||
return m_param_rot_offset-number;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
m_param_trans_offset -= number;
|
||||
new(&map) VectorMap(&m_parameter(m_param_trans_offset), number, DynStride(1,1));
|
||||
return m_param_trans_offset;
|
||||
|
@ -413,7 +432,8 @@ struct Kernel {
|
|||
new(&map) Vector3Map(&m_parameter(m_param_rot_offset));
|
||||
m_param_rot_offset += 3;
|
||||
return m_param_rot_offset-3;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
m_param_trans_offset -= 3;
|
||||
new(&map) Vector3Map(&m_parameter(m_param_trans_offset));
|
||||
return m_param_trans_offset;
|
||||
|
@ -431,7 +451,8 @@ struct Kernel {
|
|||
};
|
||||
|
||||
bool isValid() {
|
||||
if(!m_params || !m_eqns) return false;
|
||||
if(!m_params || !m_eqns)
|
||||
return false;
|
||||
return true;
|
||||
};
|
||||
|
||||
|
@ -440,15 +461,19 @@ struct Kernel {
|
|||
if(t==complete) {
|
||||
new(&Jacobi) MatrixMap(&m_jacobi(0,0),m_eqns,m_params,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(0),m_params,DynStride(1,1));
|
||||
} else if(t==rotation) {
|
||||
int num = m_param_trans_offset;
|
||||
new(&Jacobi) MatrixMap(&m_jacobi(0,0),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(0),num,DynStride(1,1));
|
||||
} else if(t==general) {
|
||||
int num = m_params - m_param_trans_offset;
|
||||
new(&Jacobi) MatrixMap(&m_jacobi(0,m_param_trans_offset),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(m_param_trans_offset),num,DynStride(1,1));
|
||||
}
|
||||
else
|
||||
if(t==rotation) {
|
||||
int num = m_param_trans_offset;
|
||||
new(&Jacobi) MatrixMap(&m_jacobi(0,0),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(0),num,DynStride(1,1));
|
||||
}
|
||||
else
|
||||
if(t==general) {
|
||||
int num = m_params - m_param_trans_offset;
|
||||
new(&Jacobi) MatrixMap(&m_jacobi(0,m_param_trans_offset),m_eqns,num,DynStride(m_eqns,1));
|
||||
new(&Parameter) VectorMap(&m_parameter(m_param_trans_offset),num,DynStride(1,1));
|
||||
}
|
||||
};
|
||||
|
||||
void setGeneralEquationAccess(bool general) {
|
||||
|
@ -459,13 +484,15 @@ struct Kernel {
|
|||
|
||||
if(t==rotation)
|
||||
return (m_param_rot_offset>0);
|
||||
else if(t==general)
|
||||
return (m_param_trans_offset<m_params);
|
||||
else
|
||||
return (m_params>0);
|
||||
if(t==general)
|
||||
return (m_param_trans_offset<m_params);
|
||||
else
|
||||
return (m_params>0);
|
||||
};
|
||||
|
||||
virtual void recalculate() = 0;
|
||||
virtual void removeLocalGradientZeros() = 0;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
@ -51,12 +51,13 @@ struct MES : public system_traits<Sys>::Kernel::MappedEquationSystem {
|
|||
typedef typename module3d::math_prop math_prop;
|
||||
typedef typename module3d::fix_prop fix_prop;
|
||||
typedef typename Kernel::number_type Scalar;
|
||||
typedef typename system_traits<Sys>::Kernel::MappedEquationSystem Base;
|
||||
typedef typename system_traits<Sys>::Kernel::MappedEquationSystem base;
|
||||
|
||||
boost::shared_ptr<Cluster> m_cluster;
|
||||
|
||||
MES(boost::shared_ptr<Cluster> cl, int par, int eqn);
|
||||
virtual void recalculate();
|
||||
virtual void removeLocalGradientZeros();
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
|
@ -126,7 +127,7 @@ struct SystemSolver : public Job<Sys> {
|
|||
|
||||
|
||||
template<typename Sys>
|
||||
MES<Sys>::MES(boost::shared_ptr<Cluster> cl, int par, int eqn) : Base(par, eqn), m_cluster(cl) {
|
||||
MES<Sys>::MES(boost::shared_ptr<Cluster> cl, int par, int eqn) : base(par, eqn), m_cluster(cl) {
|
||||
|
||||
};
|
||||
|
||||
|
@ -153,7 +154,26 @@ void MES<Sys>::recalculate() {
|
|||
std::pair< oiter, oiter > oit = m_cluster->template getObjects<Constraint3D>(*eit.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
if(*oit.first)
|
||||
(*oit.first)->calculate(Base::Scaling, Base::rot_only);
|
||||
(*oit.first)->calculate(base::Scaling, base::rot_only);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Sys>
|
||||
void MES<Sys>::removeLocalGradientZeros() {
|
||||
|
||||
Base::Console().Message("remove local gradient zero\n");
|
||||
//let the constraints treat the local zeros
|
||||
typedef typename Cluster::template object_iterator<Constraint3D> oiter;
|
||||
typedef typename boost::graph_traits<Cluster>::edge_iterator eiter;
|
||||
std::pair<eiter, eiter> eit = boost::edges(*m_cluster);
|
||||
for(; eit.first != eit.second; eit.first++) {
|
||||
//as always: every local edge can hold multiple global ones, so iterate over all constraints
|
||||
//hold by the individual edge
|
||||
std::pair< oiter, oiter > oit = m_cluster->template getObjects<Constraint3D>(*eit.first);
|
||||
for(; oit.first != oit.second; oit.first++) {
|
||||
if(*oit.first)
|
||||
(*oit.first)->treatLGZ();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
@ -178,7 +198,8 @@ typename SystemSolver<Sys>::Scalar SystemSolver<Sys>::Rescaler::scaleClusters()
|
|||
Scalar sc = 0;
|
||||
for(cit = cluster->clusters(); cit.first != cit.second; cit.first++) {
|
||||
//fixed cluster are irrelevant for scaling
|
||||
if((*cit.first).second->template getClusterProperty<fix_prop>()) continue;
|
||||
if((*cit.first).second->template getClusterProperty<fix_prop>())
|
||||
continue;
|
||||
|
||||
//get the biggest scale factor
|
||||
details::ClusterMath<Sys>& math = (*cit.first).second->template getClusterProperty<math_prop>();
|
||||
|
@ -200,7 +221,8 @@ typename SystemSolver<Sys>::Scalar SystemSolver<Sys>::Rescaler::scaleClusters()
|
|||
boost::shared_ptr<Cluster> c = cluster->getVertexCluster(*it.first);
|
||||
c->template getClusterProperty<math_prop>().applyClusterScale(sc,
|
||||
c->template getClusterProperty<fix_prop>());
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(sc*SKALEFAKTOR);
|
||||
}
|
||||
|
@ -283,7 +305,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
if(!cluster->template getSubclusterProperty<fix_prop>(*it.first)) {
|
||||
params += 6;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
params += cluster->template getObject<Geometry3D>(*it.first)->m_parameterCount;
|
||||
};
|
||||
}
|
||||
|
@ -328,14 +351,17 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
cm.setParameterOffset(offset, general);
|
||||
//wirte initial values
|
||||
cm.initMaps();
|
||||
} else cm.initFixMaps();
|
||||
}
|
||||
else
|
||||
cm.initFixMaps();
|
||||
|
||||
//map all geometrie within that cluster to it's rotation matrix
|
||||
//for collecting all geometries which need updates
|
||||
cm.clearGeometry();
|
||||
cm.mapClusterDownstreamGeometry(c);
|
||||
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
int offset = mes.setParameterMap(g->m_parameterCount, g->getParameterMap());
|
||||
g->m_offset = offset;
|
||||
|
@ -356,7 +382,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
|
||||
//set the maps
|
||||
Cons c = *oit.first;
|
||||
if(c) c->setMaps(mes);
|
||||
if(c)
|
||||
c->setMaps(mes);
|
||||
//TODO: else throw (as every global edge was counted as one equation)
|
||||
}
|
||||
}
|
||||
|
@ -370,7 +397,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
DummyScaler re;
|
||||
Kernel::solve(mes, re);
|
||||
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
|
||||
// we have rotations, so let's check our options. first search for cycles, as systems with them
|
||||
// always need the full solver power
|
||||
|
@ -378,18 +406,18 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
cycle_dedector cd(has_cycle);
|
||||
//create te needed property map, fill it and run the test
|
||||
property_map<vertex_index_prop, Cluster> vi_map(cluster);
|
||||
property_map<vertex_color_prop, Cluster> vc_map(cluster);
|
||||
property_map<edge_color_prop, Cluster> ec_map(cluster);
|
||||
property_map<vertex_color_prop, Cluster> vc_map(cluster);
|
||||
property_map<edge_color_prop, Cluster> ec_map(cluster);
|
||||
cluster->initIndexMaps();
|
||||
boost::undirected_dfs(*cluster.get(), boost::visitor(cd).vertex_index_map(vi_map).vertex_color_map(vc_map).edge_color_map(ec_map));
|
||||
|
||||
bool done = false;
|
||||
|
||||
bool done = false;
|
||||
if(!has_cycle) {
|
||||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "non-cyclic system dedected"
|
||||
#endif
|
||||
//cool, lets do uncylic. first all rotational constraints with rotational parameters
|
||||
mes.setAccess(rotation);
|
||||
//cool, lets do uncylic. first all rotational constraints with rotational parameters
|
||||
mes.setAccess(rotation);
|
||||
mes.setGeneralEquationAccess(false);
|
||||
//solve can be done without catching exceptions, because this only fails if the system is
|
||||
//unsolvable
|
||||
|
@ -408,7 +436,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
DummyScaler re;
|
||||
Kernel::solve(mes, re);
|
||||
done=true;
|
||||
} catch(boost::exception& ) {
|
||||
}
|
||||
catch(boost::exception&) {
|
||||
//not successful, so we need brute force
|
||||
done = false;
|
||||
}
|
||||
|
@ -420,7 +449,7 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
#ifdef USE_LOGGING
|
||||
BOOST_LOG(log)<< "Full scale solver used"
|
||||
#endif
|
||||
Rescaler re(cluster, mes);
|
||||
Rescaler re(cluster, mes);
|
||||
re();
|
||||
Kernel::solve(mes, re);
|
||||
#ifdef USE_LOGGING
|
||||
|
@ -444,7 +473,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
for(typename std::vector<Geom>::iterator vit = vec.begin(); vit != vec.end(); vit++)
|
||||
(*vit)->finishCalculation();
|
||||
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
Geom g = cluster->template getObject<Geometry3D>(*it.first);
|
||||
g->scale(mes.Scaling);
|
||||
g->finishCalculation();
|
||||
|
@ -453,7 +483,8 @@ void SystemSolver<Sys>::solveCluster(boost::shared_ptr<Cluster> cluster, Sys& sy
|
|||
//we have solved this cluster
|
||||
cluster->template setClusterProperty<changed_prop>(false);
|
||||
|
||||
} catch(boost::exception& ) {
|
||||
}
|
||||
catch(boost::exception&) {
|
||||
throw;
|
||||
}
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue
Block a user