added debug info for gradient code
							parent
							
								
									41bb99b48a
								
							
						
					
					
						commit
						e3ec28ebeb
					
				|  | @ -185,7 +185,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const | |||
|   double stepsize = 2/mu_max; // = 1/(a b dG)
 | ||||
| 
 | ||||
|   std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; | ||||
| 
 | ||||
|   double maxGrad; | ||||
|   // gradient iterations
 | ||||
|   size_t it; | ||||
|   for(it=0; it < maxIter; it++){ | ||||
|  | @ -193,7 +193,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const | |||
|     // compute the gradient at each node
 | ||||
|     //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
 | ||||
|     //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
 | ||||
|     double maxGrad = 0; | ||||
|     maxGrad = 0; | ||||
|     BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { | ||||
|       Key key = key_value.key; | ||||
|       //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
 | ||||
|  | @ -234,7 +234,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const | |||
|       break; | ||||
|   } // enf of gradient iterations
 | ||||
| 
 | ||||
|   std::cout << "nr of gradient iterations " << it <<  std::endl; | ||||
|   std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl; | ||||
| 
 | ||||
|   // Return correct rotations
 | ||||
|   const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
 | ||||
|  | @ -287,13 +287,13 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl | |||
|   Vector3 logRot = Rot3::Logmap(R1.between(R2)); | ||||
| 
 | ||||
|   double th = logRot.norm(); | ||||
|   if(th < 1e-4 || th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
 | ||||
|   if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
 | ||||
|     Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
 | ||||
|     logRot = Rot3::Logmap(R1pert.between(R2)); | ||||
|     th = logRot.norm(); | ||||
|   } | ||||
|   // exclude small or invalid rotations
 | ||||
|   if (th > 1e-5 || th == th){ // the second case means that th = nan (logRot does not work well for +/-pi)
 | ||||
|   if (th > 1e-5 && th == th){ // nonzero valid rotations
 | ||||
|     logRot = logRot / th; | ||||
|   }else{ | ||||
|     logRot = Vector3::Zero(); | ||||
|  | @ -334,7 +334,13 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) { | |||
| 
 | ||||
|   // Create optimizer
 | ||||
|   GaussNewtonParams params; | ||||
|   params.maxIterations = 1; | ||||
|   bool singleIter = false; | ||||
|   if(singleIter){ | ||||
|     params.maxIterations = 1; | ||||
|   }else{ | ||||
|     std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl; | ||||
|     params.setVerbosity("TERMINATION"); | ||||
|   } | ||||
|   GaussNewtonOptimizer optimizer(pose3graph, initialPose, params); | ||||
|   Values GNresult = optimizer.optimize(); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue