From 0099bfd0fd78b58c3307e860a79f663aeae6aa68 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Stefan=20Tr=C3=B6ger?= <stefantroeger@gmx.net>
Date: Thu, 24 Oct 2013 06:00:04 +0200
Subject: [PATCH] fix failing angle constraint on bad starting position: treat
 LGZ before first error check

---
 src/Mod/Assembly/App/opendcm/core/kernel.hpp | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/Mod/Assembly/App/opendcm/core/kernel.hpp b/src/Mod/Assembly/App/opendcm/core/kernel.hpp
index 97b6a8021..f3d748cfc 100644
--- a/src/Mod/Assembly/App/opendcm/core/kernel.hpp
+++ b/src/Mod/Assembly/App/opendcm/core/kernel.hpp
@@ -79,7 +79,7 @@ struct Dogleg {
     number_type tolg, tolx;
     Kernel* m_kernel;
 
-    Dogleg(Kernel* k) : m_kernel(k), tolg(1e-40), tolx(1e-20){
+    Dogleg(Kernel* k) : m_kernel(k), tolg(1e-40), tolx(1e-20) {
 
 #ifdef USE_LOGGING
         log.add_attribute("Tag", attrs::constant< std::string >("Dogleg"));
@@ -182,6 +182,12 @@ struct Dogleg {
                       << "residual: "<<sys.Residual.transpose()<<std::endl
                       << "maximal differential: "<<sys.Jacobi.template lpNorm<Eigen::Infinity>();
 #endif
+        sys.removeLocalGradientZeros();
+
+#ifdef USE_LOGGING
+        BOOST_LOG(log)<< "LGZ jacobi: "<<std::endl<<sys.Jacobi<<std::endl
+                      << "maximal differential: "<<sys.Jacobi.template lpNorm<Eigen::Infinity>();
+#endif
 
         number_type err = sys.Residual.norm();
 
@@ -232,9 +238,6 @@ struct Dogleg {
             number_type dF=0, dL=0;
             number_type rho;
 
-            if(iter==0)
-                sys.removeLocalGradientZeros();
-
             //get the update step
             calculateStep(g, sys.Jacobi,  sys.Residual, h_dl, delta);
 
@@ -535,7 +538,7 @@ struct Kernel : public PropertyOwner< mpl::vector<precision> > {
     bool isOpposite(const E::MatrixBase<DerivedA>& p1,const E::MatrixBase<DerivedB>& p2) {
         return ((p1+p2).squaredNorm() < getProperty<precision>());
     }
-    
+
     int solve(MappedEquationSystem& mes) {
         nothing n;
         return NonlinearSolver(this).solve(mes, n);
@@ -545,7 +548,7 @@ struct Kernel : public PropertyOwner< mpl::vector<precision> > {
     int solve(MappedEquationSystem& mes, Functor& f) {
         return NonlinearSolver(this).solve(mes, f);
     };
-    
+
     typedef mpl::vector1<precision> properties;
 };