diff --git a/src/Mod/Assembly/App/opendcm/core/constraint.hpp b/src/Mod/Assembly/App/opendcm/core/constraint.hpp index a54685bac..961b6a3cb 100644 --- a/src/Mod/Assembly/App/opendcm/core/constraint.hpp +++ b/src/Mod/Assembly/App/opendcm/core/constraint.hpp @@ -47,7 +47,9 @@ #include "traits.hpp" #include "object.hpp" #include "equations.hpp" +#include +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& vec; GenericEquations(std::vector& 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::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::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 @@ -350,6 +366,11 @@ void Constraint::calculate(Scalar scale, b content->calculate(first, second, scale, rotation_only); }; +template +void Constraint::treatLGZ() { + content->treatLGZ(first, second); +}; + template void Constraint::setMaps(MES& mes) { content->setMaps(mes, first, second); @@ -427,7 +448,8 @@ void Constraint::holdergetClusterMode()) { @@ -435,7 +457,8 @@ void Constraint::holder::holderm_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::holderm_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::holderm_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::holderm_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 @@ -539,15 +568,92 @@ template< typename T > void Constraint::holder::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 +template +Constraint::holder::LGZ::LGZ(geom_ptr f, geom_ptr s) + : first(f), second(s) { + +}; + +template +template +template< typename T > +void Constraint::holder::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 template @@ -605,6 +711,12 @@ void Constraint::holder +template +void Constraint::holder::treatLGZ(geom_ptr first, geom_ptr second) { + fusion::for_each(m_sets, LGZ(first, second)); +}; + template template typename Constraint::placeholder* diff --git a/src/Mod/Assembly/App/opendcm/core/kernel.hpp b/src/Mod/Assembly/App/opendcm/core/kernel.hpp index 392dc43da..1e738ad82 100644 --- a/src/Mod/Assembly/App/opendcm/core/kernel.hpp +++ b/src/Mod/Assembly/App/opendcm/core/kernel.hpp @@ -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: "<= 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: "<= 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(); fx_inf = sys.Residual.template lpNorm(); - } 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_offset0); + if(t==general) + return (m_param_trans_offset0); }; virtual void recalculate() = 0; + virtual void removeLocalGradientZeros() = 0; }; diff --git a/src/Mod/Assembly/App/opendcm/module3d/solver.hpp b/src/Mod/Assembly/App/opendcm/module3d/solver.hpp index 3f8d5a114..75fc9ff27 100644 --- a/src/Mod/Assembly/App/opendcm/module3d/solver.hpp +++ b/src/Mod/Assembly/App/opendcm/module3d/solver.hpp @@ -51,12 +51,13 @@ struct MES : public system_traits::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::Kernel::MappedEquationSystem Base; + typedef typename system_traits::Kernel::MappedEquationSystem base; boost::shared_ptr m_cluster; MES(boost::shared_ptr cl, int par, int eqn); virtual void recalculate(); + virtual void removeLocalGradientZeros(); }; template @@ -126,7 +127,7 @@ struct SystemSolver : public Job { template -MES::MES(boost::shared_ptr cl, int par, int eqn) : Base(par, eqn), m_cluster(cl) { +MES::MES(boost::shared_ptr cl, int par, int eqn) : base(par, eqn), m_cluster(cl) { }; @@ -153,7 +154,26 @@ void MES::recalculate() { std::pair< oiter, oiter > oit = m_cluster->template getObjects(*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 +void MES::removeLocalGradientZeros() { + + Base::Console().Message("remove local gradient zero\n"); + //let the constraints treat the local zeros + typedef typename Cluster::template object_iterator oiter; + typedef typename boost::graph_traits::edge_iterator eiter; + std::pair 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(*eit.first); + for(; oit.first != oit.second; oit.first++) { + if(*oit.first) + (*oit.first)->treatLGZ(); } } }; @@ -178,7 +198,8 @@ typename SystemSolver::Scalar SystemSolver::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()) continue; + if((*cit.first).second->template getClusterProperty()) + continue; //get the biggest scale factor details::ClusterMath& math = (*cit.first).second->template getClusterProperty(); @@ -200,7 +221,8 @@ typename SystemSolver::Scalar SystemSolver::Rescaler::scaleClusters() boost::shared_ptr c = cluster->getVertexCluster(*it.first); c->template getClusterProperty().applyClusterScale(sc, c->template getClusterProperty()); - } else { + } + else { Geom g = cluster->template getObject(*it.first); g->scale(sc*SKALEFAKTOR); } @@ -283,7 +305,8 @@ void SystemSolver::solveCluster(boost::shared_ptr cluster, Sys& sy if(!cluster->template getSubclusterProperty(*it.first)) { params += 6; } - } else { + } + else { params += cluster->template getObject(*it.first)->m_parameterCount; }; } @@ -328,14 +351,17 @@ void SystemSolver::solveCluster(boost::shared_ptr 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(*it.first); int offset = mes.setParameterMap(g->m_parameterCount, g->getParameterMap()); g->m_offset = offset; @@ -356,7 +382,8 @@ void SystemSolver::solveCluster(boost::shared_ptr 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::solveCluster(boost::shared_ptr 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::solveCluster(boost::shared_ptr cluster, Sys& sy cycle_dedector cd(has_cycle); //create te needed property map, fill it and run the test property_map vi_map(cluster); - property_map vc_map(cluster); - property_map ec_map(cluster); + property_map vc_map(cluster); + property_map 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::solveCluster(boost::shared_ptr 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::solveCluster(boost::shared_ptr 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::solveCluster(boost::shared_ptr cluster, Sys& sy for(typename std::vector::iterator vit = vec.begin(); vit != vec.end(); vit++) (*vit)->finishCalculation(); - } else { + } + else { Geom g = cluster->template getObject(*it.first); g->scale(mes.Scaling); g->finishCalculation(); @@ -453,7 +483,8 @@ void SystemSolver::solveCluster(boost::shared_ptr cluster, Sys& sy //we have solved this cluster cluster->template setClusterProperty(false); - } catch(boost::exception& ) { + } + catch(boost::exception&) { throw; } };