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

added examples

release/4.3a0
Luca 2014-09-03 15:40:57 -04:00
parent 316ce41a22
commit 544e56f634
4 changed files with 108 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

93
examples/Pose3SLAMExample_initializePose3Chordal.cpp Normal file
View File

@ -0,0 +1,93 @@
/* ----------------------------------------------------------------------------
* 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 <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 << "Initializing Pose3" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
// This should be deleted: only for debug
// if (argc == 4){
// const string inputFileRewritten = argv[3];
// std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// // Additional: rewrite input with simplified keys 0,1,...
// Values simpleInitial;
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
// Key key = key_value.key - firstKey;
// simpleInitial.insert(key, initial->at(key_value.key));
// }
// NonlinearFactorGraph simpleGraph;
// BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
// boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
// boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
// if (pose3Between){
// Key key1 = pose3Between->key1() - firstKey;
// Key key2 = pose3Between->key2() - firstKey;
// NonlinearFactor::shared_ptr simpleFactor(
// new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
// simpleGraph.add(simpleFactor);
// }
// }
// writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
// }
}
return 0;
}

28
examples/Pose3SLAMExample_initializePose3.cpp → examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -53,7 +53,8 @@ int main(const int argc, const char *argv[]) {
}
std::cout << "Initializing Pose3" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
if (argc < 3) {
@ -63,31 +64,6 @@ int main(const int argc, const char *argv[]) {
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
// This should be deleted: only for debug
if (argc == 4){
const string inputFileRewritten = argv[3];
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
Key key = key_value.key - firstKey;
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1 = pose3Between->key1() - firstKey;
Key key2 = pose3Between->key2() - firstKey;
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
simpleGraph.add(simpleFactor);
}
}
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
}
}
return 0;
}

0
examples/Pose3SLAMExample_rotOnly.cpp → examples/Pose3SLAMExample_rotOnlyGN.cpp
View File

23
gtsam/slam/InitializePose3.cpp
View File

@ -366,16 +366,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
else
orientations = computeOrientationsChordal(pose3Graph);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
Key key = key_value.key;
if (key != keyAnchor) {
const Point3& pos = givenGuess.at<Pose3>(key).translation();
const Rot3& rot = orientations.at<Rot3>(key);
Pose3 initializedPoses = Pose3(rot, pos);
initialValues.insert(key, initializedPoses);
}
}
return initialValues;
// Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations);
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
// Key key = key_value.key;
// if (key != keyAnchor) {
// const Point3& pos = givenGuess.at<Pose3>(key).translation();
// const Rot3& rot = orientations.at<Rot3>(key);
// Pose3 initializedPoses = Pose3(rot, pos);
// initialValues.insert(key, initializedPoses);
// }
// }
// return initialValues;
}
} // end of namespace lago