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