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

fixed gradient

release/4.3a0
Luca 2014-09-06 10:57:22 -04:00
parent 8444680e6e
commit 41bb99b48a
3 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -57,6 +57,9 @@ int main(const int argc, const char *argv[]) {
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {

20
gtsam/slam/InitializePose3.cpp
View File

@ -184,6 +184,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
double mu_max = maxNodeDeg * rho;
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;
// gradient iterations
size_t it;
for(it=0; it < maxIter; it++){
@ -217,7 +219,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
@ -235,7 +237,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
std::cout << "nr of gradient iterations " << it << std::endl;
// Return correct rotations
const Rot3& Rref = Rot3(); // inverseRot.at<Rot3>(keyAnchor);
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
@ -284,13 +286,19 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
Vector3 logRot = Rot3::Logmap(R1.between(R2));
if(logRot.norm()<1e-4){ // some tolerance
double th = logRot.norm();
if(th < 1e-4 || 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();
}
double th = logRot.norm();
if (th > 1e-5)
// 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)
logRot = logRot / th;
}else{
logRot = Vector3::Zero();
th = 0.0;
}
double fdot = a*b*th*exp(-b*th);
return fdot*logRot;
@ -372,6 +380,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
else
orientations = computeOrientationsChordal(pose3Graph);
// orientations.print("orientations\n");
// Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations);

2
gtsam/slam/InitializePose3.h
View File

@ -39,7 +39,7 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 1000);
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000);
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph);