12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

removed comments

release/4.3a0
Luca 2014-09-03 15:31:30 -04:00
parent b19e758f5a
commit 316ce41a22
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/slam/InitializePose3.cpp
View File

@ -188,8 +188,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
for(size_t it=0; it < maxIter; it++){ for(size_t it=0; it < maxIter; it++){
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
// compute the gradient at each node // compute the gradient at each node
std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
<<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
double maxGrad = 0; double maxGrad = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key; Key key = key_value.key;
@ -216,7 +216,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
grad.at(key) = stepsize * gradKey; grad.at(key) = stepsize * gradKey;
double normGradKey = (gradKey).norm(); double normGradKey = (gradKey).norm();
std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
if(normGradKey>maxGrad) if(normGradKey>maxGrad)
maxGrad = normGradKey; maxGrad = normGradKey;
} // end of loop over nodes } // end of loop over nodes
@ -225,8 +225,6 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// update estimates // update estimates
inverseRot = inverseRot.retract(grad); inverseRot = inverseRot.retract(grad);
inverseRot.print("inverseRot \n");
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
// check stopping condition // check stopping condition
if (it>20 && maxGrad < 5e-3) if (it>20 && maxGrad < 5e-3)