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

added comment

release/4.3a0
Luca 2014-05-27 18:23:57 -04:00
parent bf8acfda9a
commit 8a1c6fe77f
1 changed files with 1 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/nonlinear/LagoInitializer.cpp
View File

@ -145,6 +145,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spann
double key1_DeltaTheta_key2 = deltaTheta(0); double key1_DeltaTheta_key2 = deltaTheta(0);
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = round(k2pi_noise/(2*M_PI)); double k = round(k2pi_noise/(2*M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
} }