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

fixed issues in dataset and added comments

release/4.3a0
Luca 2014-09-03 20:18:46 -04:00
parent 544e56f634
commit 08a6bff803
7 changed files with 115 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
examples/Pose3SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -52,7 +52,7 @@ int main(const int argc, const char *argv[]) {
break;
}
std::cout << "Initializing Pose3" << std::endl;
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
std::cout << "done!" << std::endl;

4
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
@ -52,7 +52,7 @@ int main(const int argc, const char *argv[]) {
break;
}
std::cout << "Initializing Pose3" << std::endl;
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;

3
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -76,6 +76,9 @@ void NonlinearOptimizer::defaultOptimize() {
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
params.errorTol, currentError, this->error(), params.verbosity));
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION)
cout << "Number of iterations: " << this->iterations() << endl;
// Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
this->iterations() >= params.maxIterations)

12
gtsam/slam/InitializePose3.cpp
View File

@ -185,7 +185,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
double stepsize = 2/mu_max; // = 1/(a b dG)
// gradient iterations
for(size_t it=0; it < maxIter; it++){
size_t it;
for(it=0; it < maxIter; it++){
//////////////////////////////////////////////////////////////////////////
// compute the gradient at each node
//std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
@ -231,12 +232,17 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
break;
} // enf of gradient iterations
std::cout << "nr of gradient iterations " << it << std::endl;
// Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor);
Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
const Rot3& R = inverseRot.at<Rot3>(key);
estimateRot.insert(key, R.inverse());
if (key != keyAnchor) {
const Rot3& R = inverseRot.at<Rot3>(key);
estimateRot.insert(key, Rref.compose(R.inverse()));
}
}
return estimateRot;
}

15
gtsam/slam/dataset.cpp
View File

@ -439,9 +439,15 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
Matrix InfoG2o = eye(6);
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
stream << " " << Info(i, j);
stream << " " << InfoG2o(i, j);
}
}
stream << endl;
@ -524,7 +530,12 @@ GraphAndValues load3D(const string& filename) {
m(j, i) = mij;
}
}
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
Matrix mgtsam = eye(6);
mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
}

43
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -123,25 +123,6 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
}
/* *************************************************************************** *
TEST( InitializePose3, orientationsCheckGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,simple::pose0);
givenPoses.insert(x2,simple::pose0);
givenPoses.insert(x3,simple::pose0);
Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, singleGradient ) {
Rot3 R1 = Rot3();
@ -174,12 +155,12 @@ TEST( InitializePose3, iterationGradient ) {
size_t maxIter = 1; // test gradient at the first iteration
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter);
const Key keyAnchor = symbol('Z', 9999999);
Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984,
0.002460082752984, 0.999993962808392, -0.002454045561375,
-0.002454045561375, 0.002460082752984, 0.999993962808392);
Rot3 RzExpected = Rot3(Mz);
EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-6));
// const Key keyAnchor = symbol('Z', 9999999);
// Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984,
// 0.002460082752984, 0.999993962808392, -0.002454045561375,
// -0.002454045561375, 0.002460082752984, 0.999993962808392);
// Rot3 RzExpected = Rot3(Mz);
// EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-6));
Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352,
0.036032601656108, 0.999346013522419, -0.003032634950127,
@ -225,12 +206,12 @@ TEST( InitializePose3, orientationsGradient ) {
// string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
// writeG2o(pose3Graph, givenPoses, g2oFile);
const Key keyAnchor = symbol('Z', 9999999);
Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948,
0.181688570817424, 0.983350839452522, -0.003223318529546,
-0.003987804220587, 0.004014645856811, 0.999983989889910);
Rot3 RzExpected = Rot3(Mz);
EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-4));
// const Key keyAnchor = symbol('Z', 9999999);
// Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948,
// 0.181688570817424, 0.983350839452522, -0.003223318529546,
// -0.003987804220587, 0.004014645856811, 0.999983989889910);
// Rot3 RzExpected = Rot3(Mz);
// EXPECT(assert_equal(RzExpected, orientations.at<Rot3>(keyAnchor), 1e-4));
Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630,
0.321308000189570, 0.946969747977338, -0.003076593882320,