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

Merge remote-tracking branch 'origin/develop' into feature/tbb_fixes

release/4.3a0
Fan Jiang 2020-08-21 11:56:51 -04:00
parent 056c835cea 69f7a27a7e
commit 7f32553440
22 changed files with 671 additions and 219 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
examples/ShonanAveragingCLI.cpp Normal file
View File

@ -0,0 +1,125 @@
/* ----------------------------------------------------------------------------
* 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 ShonanAveragingCLI.cpp
* @brief Run Shonan Rotation Averaging Algorithm on a file or example dataset
* @author Frank Dellaert
* @date August, 2020
*
* Example usage:
*
* Running without arguments will run on tiny 3D example pose3example-grid
* ./ShonanAveragingCLI
*
* Read 2D dataset w10000 (in examples/data) and output to w10000-rotations.g2o
* ./ShonanAveragingCLI -d 2 -n w10000 -o w10000-rotations.g2o
*
* Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
* ./ShonanAveragingCLI -i spere2500.txt
*
*/
#include <gtsam/base/timing.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/slam/dataset.h>
#include <boost/program_options.hpp>
using namespace std;
using namespace gtsam;
namespace po = boost::program_options;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
string datasetName;
string inputFile;
string outputFile;
int d, seed;
po::options_description desc(
"Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
"rotation constraints, and runs the Shonan algorithm.");
desc.add_options()("help", "Print help message")(
"named_dataset,n",
po::value<string>(&datasetName)->default_value("pose3example-grid"),
"Find and read frome example dataset file")(
"input_file,i", po::value<string>(&inputFile)->default_value(""),
"Read pose constraints graph from the specified file")(
"output_file,o",
po::value<string>(&outputFile)->default_value("shonan.g2o"),
"Write solution to the specified file")(
"dimension,d", po::value<int>(&d)->default_value(3),
"Optimize over 2D or 3D rotations")(
"seed,s", po::value<int>(&seed)->default_value(42),
"Random seed for initial estimate");
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
po::notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
return 1;
}
// Get input file
if (inputFile.empty()) {
if (datasetName.empty()) {
cout << "You must either specify a named dataset or an input file\n"
<< desc << endl;
return 1;
}
inputFile = findExampleDataFile(datasetName);
}
// Seed random number generator
static std::mt19937 rng(seed);
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
Values poses;
if (d == 2) {
cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
ShonanAveraging2 shonan(inputFile);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load2D(inputFile);
auto priorModel = noiseModel::Unit::Create(3);
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
cout << "recovering 2D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
} else if (d == 3) {
cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
ShonanAveraging3 shonan(inputFile);
auto initial = shonan.initializeRandomly(rng);
auto result = shonan.run(initial);
// Parse file again to set up translation problem, adding a prior
boost::tie(inputGraph, posesInFile) = load3D(inputFile);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
cout << "recovering 3D translations" << endl;
auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
poses = initialize::computePoses<Pose3>(result.first, &poseGraph);
} else {
cout << "Can only run SO(2) or SO(3) averaging\n" << desc << endl;
return 1;
}
cout << "Writing result to " << outputFile << endl;
writeG2o(NonlinearFactorGraph(), poses, outputFile);
return 0;
}
/* ************************************************************************* */

2
gtsam/geometry/Pose3.cpp
View File

@ -430,7 +430,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
// Both Rot3 and Point3 have ostream definitions so we use them.
os << "R: " << pose.rotation() << "\n";
os << "t: " << pose.translation();
os << "t: " << pose.translation().transpose();
return os;
}

15
gtsam/geometry/tests/testPose3.cpp
View File

@ -857,15 +857,11 @@ TEST( Pose3, adjointTranspose) {
}
/* ************************************************************************* */
TEST( Pose3, stream)
{
Pose3 T;
TEST( Pose3, stream) {
std::ostringstream os;
os << T;
string expected;
expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0\n0\n0";;
os << Pose3();
string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0 0 0";
EXPECT(os.str() == expected);
}
@ -1038,10 +1034,7 @@ TEST(Pose3, print) {
// Add expected rotation
expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
expected << "t: 1\n"
"2\n"
"3\n";
expected << "t: 1 2 3\n";
// reset cout to the original stream
std::cout.rdbuf(oldbuf);

29
gtsam/gtsam.i
View File

@ -2816,7 +2816,19 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose2]
class BetweenFactorPose2s
{
BetweenFactorPose2s();
size_t size() const;
gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
};
gtsam::BetweenFactorPose2s parse2DFactors(string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose3]
class BetweenFactorPose3s
{
BetweenFactorPose3s();
@ -2824,6 +2836,14 @@ class BetweenFactorPose3s
gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
#include <gtsam/slam/InitializePose3.h>
class InitializePose3 {
@ -2845,14 +2865,6 @@ class InitializePose3 {
static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
#include <gtsam/slam/KarcherMeanFactor-inl.h>
template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
@ -3376,6 +3388,7 @@ namespace utilities {
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
gtsam::Values allPose2s(gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);

5
gtsam/nonlinear/utilities.h
View File

@ -107,6 +107,11 @@ Matrix extractPoint3(const Values& values) {
return result;
}
/// Extract all Pose3 values
Values allPose2s(const Values& values) {
return values.filter<Pose2>();
}
/// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();

2
gtsam/sfm/tests/testShonanAveraging.cpp
View File

@ -302,7 +302,7 @@ TEST(ShonanAveraging3, runKlausKarcher) {
}
/* ************************************************************************* */
TEST(ShonanAveraging2, runKlausKarcher) {
TEST(ShonanAveraging2, noisyToyGraph) {
// Load 2D toy example
auto lmParams = LevenbergMarquardtParams::CeresDefaults();
// lmParams.setVerbosityLM("SUMMARY");

99
gtsam/slam/InitializePose.h Normal file
View File

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------------
* 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 InitializePose.h
* @author Frank Dellaert
* @date August, 2020
* @brief common code between lago.* (2D) and InitializePose3.* (3D)
*/
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
namespace initialize {
static constexpr Key kAnchorKey = 99999999;
/**
* Select the subgraph of betweenFactors and transforms priors into between
* wrt a fictitious node
*/
template <class Pose>
static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph poseGraph;
for (const auto& factor : graph) {
// recast to a between on Pose
if (auto between =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
poseGraph.add(between);
// recast PriorFactor<Pose> to BetweenFactor<Pose>
if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
poseGraph.emplace_shared<BetweenFactor<Pose> >(
kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
}
return poseGraph;
}
/**
* Use Gauss-Newton optimizer to optimize for poses given rotation estimates
*/
template <class Pose>
static Values computePoses(const Values& initialRot,
NonlinearFactorGraph* posegraph,
bool singleIter = true) {
const auto origin = Pose().translation();
// Upgrade rotations to full poses
Values initialPose;
for (const auto& key_value : initialRot) {
Key key = key_value.key;
const auto& rot = initialRot.at<typename Pose::Rotation>(key);
Pose initializedPose = Pose(rot, origin);
initialPose.insert(key, initializedPose);
}
// add prior on dummy node
auto priorModel = noiseModel::Unit::Create(Pose::dimension);
initialPose.insert(kAnchorKey, Pose());
posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
// Create optimizer
GaussNewtonParams params;
if (singleIter) {
params.maxIterations = 1;
} else {
params.setVerbosity("TERMINATION");
}
GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
Values GNresult = optimizer.optimize();
// put into Values structure
Values estimate;
for (const auto& key_value : GNresult) {
Key key = key_value.key;
if (key != kAnchorKey) {
const Pose& pose = GNresult.at<Pose>(key);
estimate.insert(key, pose);
}
}
return estimate;
}
} // namespace initialize
} // namespace gtsam

121
gtsam/slam/InitializePose3.cpp
View File

@ -10,28 +10,31 @@
* -------------------------------------------------------------------------- */
/**
* @file InitializePose3.h
* @file InitializePose3.cpp
* @author Luca Carlone
* @author Frank Dellaert
* @date August, 2014
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>
#include <boost/math/special_functions.hpp>
#include <utility>
using namespace std;
namespace gtsam {
static const Key kAnchorKey = symbol('Z', 9999999);
/* ************************************************************************* */
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
@ -62,7 +65,7 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
}
// prior on the anchor orientation
linearGraph.add(
kAnchorKey, I_9x9,
initialize::kAnchorKey, I_9x9,
(Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0)
.finished(),
noiseModel::Isotropic::Precision(9, 1));
@ -78,7 +81,7 @@ Values InitializePose3::normalizeRelaxedRotations(
Values validRot3;
for(const auto& it: relaxedRot3) {
Key key = it.first;
if (key != kAnchorKey) {
if (key != initialize::kAnchorKey) {
Matrix3 M;
M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
@ -91,24 +94,10 @@ Values InitializePose3::normalizeRelaxedRotations(
}
/* ************************************************************************* */
NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph InitializePose3::buildPose3graph(
const NonlinearFactorGraph& graph) {
gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph;
for(const auto& factor: graph) {
// recast to a between on Pose3
const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between)
pose3Graph.add(pose3Between);
// recast PriorFactor<Pose3> to BetweenFactor<Pose3>
const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
if (pose3Prior)
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
pose3Prior->prior(), pose3Prior->noiseModel());
}
return pose3Graph;
return initialize::buildPoseGraph<Pose3>(graph);
}
/* ************************************************************************* */
@ -134,7 +123,7 @@ Values InitializePose3::computeOrientationsGradient(
// this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot;
inverseRot.insert(kAnchorKey, Rot3());
inverseRot.insert(initialize::kAnchorKey, Rot3());
for(const auto& key_value: givenGuess) {
Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key);
@ -145,7 +134,7 @@ Values InitializePose3::computeOrientationsGradient(
KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap;
createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
// calculate max node degree & allocate gradient
size_t maxNodeDeg = 0;
@ -212,11 +201,11 @@ Values InitializePose3::computeOrientationsGradient(
} // enf of gradient iterations
// Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
Values estimateRot;
for(const auto& key_value: inverseRot) {
Key key = key_value.key;
if (key != kAnchorKey) {
if (key != initialize::kAnchorKey) {
const Rot3& R = inverseRot.at<Rot3>(key);
if(setRefFrame)
estimateRot.insert(key, Rref.compose(R.inverse()));
@ -228,32 +217,34 @@ Values InitializePose3::computeOrientationsGradient(
}
/* ************************************************************************* */
void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph) {
void InitializePose3::createSymbolicGraph(
const NonlinearFactorGraph& pose3Graph, KeyVectorMap* adjEdgesMap,
KeyRotMap* factorId2RotMap) {
size_t factorId = 0;
for(const auto& factor: pose3Graph) {
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
for (const auto& factor : pose3Graph) {
auto pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between) {
Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
factorId2RotMap->emplace(factorId, Rij);
Key key1 = pose3Between->key1();
if (adjEdgesMap.find(key1) != adjEdgesMap.end()){ // key is already in
adjEdgesMap.at(key1).push_back(factorId);
}else{
if (adjEdgesMap->find(key1) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key1).push_back(factorId);
} else {
vector<size_t> edge_id;
edge_id.push_back(factorId);
adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
adjEdgesMap->emplace(key1, edge_id);
}
Key key2 = pose3Between->key2();
if (adjEdgesMap.find(key2) != adjEdgesMap.end()){ // key is already in
adjEdgesMap.at(key2).push_back(factorId);
}else{
if (adjEdgesMap->find(key2) != adjEdgesMap->end()) { // key is already in
adjEdgesMap->at(key2).push_back(factorId);
} else {
vector<size_t> edge_id;
edge_id.push_back(factorId);
adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
adjEdgesMap->emplace(key2, edge_id);
}
}else{
} else {
cout << "Error in createSymbolicGraph" << endl;
}
factorId++;
@ -293,50 +284,16 @@ Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph
}
///* ************************************************************************* */
Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
Values InitializePose3::computePoses(const Values& initialRot,
NonlinearFactorGraph* posegraph,
bool singleIter) {
gttic(InitializePose3_computePoses);
// put into Values structure
Values initialPose;
for (const auto& key_value : initialRot) {
Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
initialPose.insert(key, initializedPose);
}
// add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
initialPose.insert(kAnchorKey, Pose3());
pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
// Create optimizer
GaussNewtonParams params;
bool singleIter = true;
if (singleIter) {
params.maxIterations = 1;
} else {
cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" << endl;
params.setVerbosity("TERMINATION");
}
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
Values GNresult = optimizer.optimize();
// put into Values structure
Values estimate;
for (const auto& key_value : GNresult) {
Key key = key_value.key;
if (key != kAnchorKey) {
const Pose3& pose = GNresult.at<Pose3>(key);
estimate.insert(key, pose);
}
}
return estimate;
return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
}
/* ************************************************************************* */
Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
bool useGradient) {
Values InitializePose3::initialize(const NonlinearFactorGraph& graph,
const Values& givenGuess, bool useGradient) {
gttic(InitializePose3_initialize);
Values initialValues;
@ -352,7 +309,7 @@ Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Valu
orientations = computeOrientationsChordal(pose3Graph);
// Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations);
return computePoses(orientations, &pose3Graph);
}
/* ************************************************************************* */

17
gtsam/slam/InitializePose3.h
View File

@ -26,6 +26,9 @@
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <map>
#include <vector>
namespace gtsam {
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
@ -50,9 +53,9 @@ struct GTSAM_EXPORT InitializePose3 {
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
size_t maxIter = 10000, const bool setRefFrame = true);
static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph);
static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
KeyVectorMap* adjEdgesMap,
KeyRotMap* factorId2RotMap);
static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
const double b);
@ -64,8 +67,12 @@ struct GTSAM_EXPORT InitializePose3 {
static NonlinearFactorGraph buildPose3graph(
const NonlinearFactorGraph& graph);
static Values computePoses(NonlinearFactorGraph& pose3graph,
Values& initialRot);
/**
* Use Gauss-Newton optimizer to optimize for poses given rotation estimates
*/
static Values computePoses(const Values& initialRot,
NonlinearFactorGraph* poseGraph,
bool singleIter = true);
/**
* "extract" the Pose3 subgraph of the original graph, get orientations from

15
gtsam/slam/dataset.cpp
View File

@ -77,6 +77,7 @@ string findExampleDataFile(const string &name) {
namesToSearch.push_back(name + ".txt");
namesToSearch.push_back(name + ".out");
namesToSearch.push_back(name + ".xml");
namesToSearch.push_back(name + ".g2o");
// Find first name that exists
for (const fs::path root : rootsToSearch) {
@ -87,10 +88,12 @@ string findExampleDataFile(const string &name) {
}
// If we did not return already, then we did not find the file
throw invalid_argument("gtsam::findExampleDataFile could not find a matching "
throw invalid_argument(
"gtsam::findExampleDataFile could not find a matching "
"file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
" or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" +
name + ", " + name + ".graph, or " + name + ".txt");
name + ", " + name + ".g2o, " + ", " + name + ".graph, or " + name +
".txt");
}
/* ************************************************************************* */
@ -1281,7 +1284,13 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) {
return initial;
}
// To be deprecated when pybind wrapper is merged:
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
BetweenFactorPose2s
parse2DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
return parseFactors<Pose2>(filename, model, maxIndex);
}
BetweenFactorPose3s
parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {

8
gtsam/slam/dataset.h
View File

@ -341,7 +341,13 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// To be deprecated when pybind wrapper is merged:
// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose2s
parse2DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,

48
gtsam/slam/lago.cpp
View File

@ -17,6 +17,8 @@
*/
#include <gtsam/slam/lago.h>
#include <gtsam/slam/InitializePose.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
@ -33,7 +35,6 @@ namespace lago {
static const Matrix I = I_1x1;
static const Matrix I3 = I_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished());
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
@ -79,7 +80,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
key2doubleMap thetaToRootMap;
// Orientation of the roo
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
thetaToRootMap.insert(pair<Key, double>(initialize::kAnchorKey, 0.0));
// for all nodes in the tree
for(const key2doubleMap::value_type& it: deltaThetaMap) {
@ -189,41 +190,16 @@ GaussianFactorGraph buildLinearOrientationGraph(
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
}
// prior on the anchor orientation
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
lagoGraph.add(initialize::kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
return lagoGraph;
}
/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
gttic(lago_buildPose2graph);
NonlinearFactorGraph pose2Graph;
for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
// recast to a between on Pose2
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (pose2Between)
pose2Graph.add(pose2Between);
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
if (pose2Prior)
pose2Graph.add(
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->noiseModel()));
}
return pose2Graph;
}
/* ************************************************************************* */
static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree;
Key minKey = keyAnchor; // this initialization does not matter
Key minKey = initialize::kAnchorKey; // this initialization does not matter
bool minUnassigned = true;
for(const boost::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
@ -240,8 +216,8 @@ static PredecessorMap<Key> findOdometricPath(
minKey = key1;
}
}
tree.insert(minKey, keyAnchor);
tree.insert(keyAnchor, keyAnchor); // root
tree.insert(minKey, initialize::kAnchorKey);
tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
return tree;
}
@ -284,7 +260,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
// Get orientations from relative orientation measurements
return computeOrientations(pose2Graph, useOdometricPath);
@ -338,7 +314,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
}
}
// add prior
linearPose2graph.add(keyAnchor, I3, Vector3(0.0, 0.0, 0.0),
linearPose2graph.add(initialize::kAnchorKey, I3, Vector3(0.0, 0.0, 0.0),
priorPose2Noise);
// optimize
@ -348,7 +324,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
Values initialGuessLago;
for(const VectorValues::value_type& it: posesLago) {
Key key = it.first;
if (key != keyAnchor) {
if (key != initialize::kAnchorKey) {
const Vector& poseVector = it.second;
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
orientationsLago.at(key)(0) + poseVector(2));
@ -364,7 +340,7 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
// Get orientations from relative orientation measurements
VectorValues orientationsLago = computeOrientations(pose2Graph,
@ -385,7 +361,7 @@ Values initialize(const NonlinearFactorGraph& graph,
// for all nodes in the tree
for(const VectorValues::value_type& it: orientations) {
Key key = it.first;
if (key != keyAnchor) {
if (key != initialize::kAnchorKey) {
const Pose2& pose = initialGuess.at<Pose2>(key);
const Vector& orientation = it.second;
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));

82
gtsam/slam/tests/testInitializePose.cpp Normal file
View File

@ -0,0 +1,82 @@
/* ----------------------------------------------------------------------------
* 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 testInitializePose3.cpp
* @brief Unit tests for 3D SLAM initialization, using rotation relaxation
*
* @author Luca Carlone
* @author Frank Dellaert
* @date August, 2014
*/
#include <gtsam/slam/InitializePose.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/dataset.h>
#include <cmath>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST(InitializePose3, computePoses2D) {
const string g2oFile = findExampleDataFile("noisyToyGraph.txt");
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
bool is3D = false;
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
auto priorModel = noiseModel::Unit::Create(3);
inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
auto I = genericValue(Rot3());
Values orientations;
for (size_t i : {0, 1, 2, 3})
orientations.insert(i, posesInFile->at<Pose2>(i).rotation());
const Values poses = initialize::computePoses<Pose2>(orientations, &poseGraph);
// posesInFile is seriously noisy, so we check error of recovered poses
EXPECT_DOUBLES_EQUAL(0.0810283, inputGraph->error(poses), 1e-6);
}
/* ************************************************************************* */
TEST(InitializePose3, computePoses3D) {
const string g2oFile = findExampleDataFile("Klaus3");
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr posesInFile;
bool is3D = true;
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
auto I = genericValue(Rot3());
Values orientations;
for (size_t i : {0, 1, 2})
orientations.insert(i, posesInFile->at<Pose3>(i).rotation());
Values poses = initialize::computePoses<Pose3>(orientations, &poseGraph);
EXPECT(assert_equal(*posesInFile, poses, 0.1)); // TODO(frank): very loose !!
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -121,7 +121,7 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap;
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
InitializePose3::createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@ -258,21 +258,21 @@ TEST( InitializePose3, posesWithGivenGuess ) {
}
/* ************************************************************************* */
TEST( InitializePose3, initializePoses )
{
TEST(InitializePose3, initializePoses) {
const string g2oFile = findExampleDataFile("pose3example-grid");
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr expectedValues;
Values::shared_ptr posesInFile;
bool is3D = true;
boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
auto priorModel = noiseModel::Unit::Create(6);
inputGraph->addPrior(0, Pose3(), priorModel);
Values initial = InitializePose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
EXPECT(assert_equal(*posesInFile, initial,
0.1)); // TODO(frank): very loose !!
}
/* ************************************************************************* */
int main() {
TestResult tr;

20
gtsam_unstable/timing/timeShonanAveraging.cpp
View File

@ -34,19 +34,17 @@
using namespace std;
using namespace gtsam;
// string g2oFile = findExampleDataFile("toyExample.g2o");
// save a single line of timing info to an output stream
void saveData(size_t p, double time1, double costP, double cost3, double time2,
double min_eigenvalue, double suBound, std::ostream* os) {
*os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t"
<< time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
*os << static_cast<int>(p) << "\t" << time1 << "\t" << costP << "\t" << cost3
<< "\t" << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
}
void checkR(const Matrix& R) {
Matrix R2 = R.transpose();
Matrix actual_R = R2 * R;
assert_equal(Rot3(),Rot3(actual_R));
assert_equal(Rot3(), Rot3(actual_R));
}
void saveResult(string name, const Values& values) {
@ -82,7 +80,8 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
myfile << i << " ";
myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
Vector quaternion = Rot3(R).quaternion();
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0);
myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
<< " " << quaternion(0);
myfile << "\n";
}
myfile.close();
@ -117,8 +116,7 @@ int main(int argc, char* argv[]) {
if (argc > 1)
g2oFile = argv[argc - 1];
else
g2oFile = string(
"/home/jingwu/git/SESync/data/sphere2500.g2o");
g2oFile = findExampleDataFile("sphere2500");
} catch (const exception& e) {
cerr << e.what() << '\n';
@ -147,14 +145,14 @@ int main(int argc, char* argv[]) {
cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
<< "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
const Values randomRotations = kShonan.initializeRandomly();
for (size_t p = pMin; p < 6; p++) {
for (size_t p = pMin; p <= 7; p++) {
// Randomly initialize at lowest level, initialize by line search after that
const Values initial =
(p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
lambdaMin)
: ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations);
: ShonanAveraging3::LiftTo<Rot3>(pMin, randomRotations);
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
// optimizing
const Values result = kShonan.tryOptimizingAt(p, initial);

1
python/CMakeLists.txt
View File

@ -26,6 +26,7 @@ set(ignore
gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices
gtsam::FactorIndexSet
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::Point2Vector
gtsam::Pose3Vector

10
python/gtsam/__init__.py
View File

@ -8,14 +8,20 @@ def _init():
global Point2 # export function
def Point2(x=0, y=0):
def Point2(x=np.nan, y=np.nan):
"""Shim for the deleted Point2 type."""
if isinstance(x, np.ndarray):
assert x.shape == (2,), "Point2 takes 2-vector"
return x # "copy constructor"
return np.array([x, y], dtype=float)
global Point3 # export function
def Point3(x=0, y=0, z=0):
def Point3(x=np.nan, y=np.nan, z=np.nan):
"""Shim for the deleted Point3 type."""
if isinstance(x, np.ndarray):
assert x.shape == (3,), "Point3 takes 3-vector"
return x # "copy constructor"
return np.array([x, y, z], dtype=float)
# for interactive debugging

125
python/gtsam/examples/ShonanAveragingCLI.py Normal file
View File

@ -0,0 +1,125 @@
"""
GTSAM Copyright 2010-2018, 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
Shonan Rotation Averaging CLI reads a *pose* graph, extracts the
rotation constraints, and runs the Shonan algorithm.
Author: Frank Dellaert
Date: August 2020
"""
# pylint: disable=invalid-name, E1101
import argparse
import matplotlib.pyplot as plt
import numpy as np
import gtsam
from gtsam.utils import plot
def estimate_poses_given_rot(factors: gtsam.BetweenFactorPose3s,
rotations: gtsam.Values,
d: int = 3):
""" Estimate Poses from measurements, given rotations. From SfmProblem in shonan.
Arguments:
factors -- data structure with many BetweenFactorPose3 factors
rotations {Values} -- Estimated rotations
Returns:
Values -- Estimated Poses
"""
I_d = np.eye(d)
def R(j):
return rotations.atRot3(j) if d == 3 else rotations.atRot2(j)
def pose(R, t):
return gtsam.Pose3(R, t) if d == 3 else gtsam.Pose2(R, t)
graph = gtsam.GaussianFactorGraph()
model = gtsam.noiseModel.Unit.Create(d)
# Add a factor anchoring t_0
graph.add(0, I_d, np.zeros((d,)), model)
# Add a factor saying t_j - t_i = Ri*t_ij for all edges (i,j)
for factor in factors:
keys = factor.keys()
i, j, Tij = keys[0], keys[1], factor.measured()
measured = R(i).rotate(Tij.translation())
graph.add(j, I_d, i, -I_d, measured, model)
# Solve linear system
translations = graph.optimize()
# Convert to Values.
result = gtsam.Values()
for j in range(rotations.size()):
tj = translations.at(j)
result.insert(j, pose(R(j), tj))
return result
def run(args):
"""Run Shonan averaging and then recover translations linearly before saving result."""
# Get input file
if args.input_file:
input_file = args.input_file
else:
if args.named_dataset == "":
raise ValueError(
"You must either specify a named dataset or an input file")
input_file = gtsam.findExampleDataFile(args.named_dataset)
if args.dimension == 2:
print("Running Shonan averaging for SO(2) on ", input_file)
shonan = gtsam.ShonanAveraging2(input_file)
if shonan.nrUnknowns() == 0:
raise ValueError("No 2D pose constraints found, try -d 3.")
initial = shonan.initializeRandomly()
rotations, _ = shonan.run(initial, 2, 10)
factors = gtsam.parse2DFactors(input_file)
elif args.dimension == 3:
print("Running Shonan averaging for SO(3) on ", input_file)
shonan = gtsam.ShonanAveraging3(input_file)
if shonan.nrUnknowns() == 0:
raise ValueError("No 3D pose constraints found, try -d 2.")
initial = shonan.initializeRandomly()
rotations, _ = shonan.run(initial, 3, 10)
factors = gtsam.parse3DFactors(input_file)
else:
raise ValueError("Can only run SO(2) or SO(3) averaging")
print("Recovering translations")
poses = estimate_poses_given_rot(factors, rotations, args.dimension)
print("Writing result to ", args.output_file)
gtsam.writeG2o(gtsam.NonlinearFactorGraph(), poses, args.output_file)
plot.plot_trajectory(1, poses, scale=0.2)
plot.set_axes_equal(1)
plt.show()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument('-n', '--named_dataset', type=str, default="pose3example-grid",
help='Find and read frome example dataset file')
parser.add_argument('-i', '--input_file', type=str, default="",
help='Read pose constraints graph from the specified file')
parser.add_argument('-o', '--output_file', type=str, default="shonan.g2o",
help='Write solution to the specified file')
parser.add_argument('-d', '--dimension', type=int, default=3,
help='Optimize over 2D or 3D rotations')
parser.add_argument("-p", "--plot", action="store_true", default=True,
help="Plot result")
run(parser.parse_args())

1
python/gtsam/specializations.h
View File

@ -8,3 +8,4 @@ py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");

29
python/gtsam/tests/test_Point2.py Normal file
View File

@ -0,0 +1,29 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Point2 unit tests.
Author: Frank Dellaert & Fan Jiang
"""
import unittest
import gtsam
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
class TestPoint2(GtsamTestCase):
"""Test selected Point2 methods."""
def test_constructors(self):
"""Test constructors from doubles and vectors."""
expected = gtsam.Point2(1, 2)
actual = gtsam.Point2(np.array([1, 2]))
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":
unittest.main()

29
python/gtsam/tests/test_Point3.py Normal file
View File

@ -0,0 +1,29 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Point3 unit tests.
Author: Frank Dellaert & Fan Jiang
"""
import unittest
import gtsam
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
class TestPoint3(GtsamTestCase):
"""Test selected Point3 methods."""
def test_constructors(self):
"""Test constructors from doubles and vectors."""
expected = gtsam.Point3(1, 2, 3)
actual = gtsam.Point3(np.array([1, 2, 3]))
np.testing.assert_array_equal(actual, expected)
if __name__ == "__main__":
unittest.main()

63
python/gtsam/utils/plot.py
View File

@ -1,9 +1,11 @@
"""Various plotting utlities."""
import numpy as np
# pylint: disable=no-member, invalid-name
import matplotlib.pyplot as plt
import numpy as np
from matplotlib import patches
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import
import gtsam
@ -307,58 +309,47 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None,
def plot_trajectory(fignum, values, scale=1, marginals=None,
title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
"""
Plot a complete 3D trajectory using poses in `values`.
Plot a complete 2D/3D trajectory using poses in `values`.
Args:
fignum (int): Integer representing the figure number to use for plotting.
values (gtsam.Values): Values dict containing the poses.
values (gtsam.Values): Values containing some Pose2 and/or Pose3 values.
scale (float): Value to scale the poses by.
marginals (gtsam.Marginals): Marginalized probability values of the estimation.
Used to plot uncertainty bounds.
title (string): The title of the plot.
axis_labels (iterable[string]): List of axis labels to set.
"""
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
lastKey = None
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
for key in keys:
try:
pose = pose3Values.atPose3(key)
except:
print("Warning: no Pose3 at key: {0}".format(key))
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
except:
print("Warning: no Pose3 at key: {0}".format(lastKey))
pass
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
# Plot 2D poses, if any
poses = gtsam.utilities.allPose2s(values)
for key in poses.keys():
pose = poses.atPose2(key)
if marginals:
covariance = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(key)
else:
covariance = None
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
plot_pose2_on_axes(axes, pose, covariance=covariance,
axis_length=scale)
lastKey = key
# Draw final pose
if lastKey is not None:
try:
lastPose = pose3Values.atPose3(lastKey)
# Then 3D poses, if any
poses = gtsam.utilities.allPose3s(values)
for key in poses.keys():
pose = poses.atPose3(key)
if marginals:
covariance = marginals.marginalCovariance(lastKey)
covariance = marginals.marginalCovariance(key)
else:
covariance = None
fig = plot_pose3(fignum, lastPose, P=covariance,
axis_length=scale, axis_labels=axis_labels)
except:
pass
plot_pose3_on_axes(axes, pose, P=covariance,
axis_length=scale)
fig.suptitle(title)
fig.canvas.set_window_title(title.lower())
@ -383,8 +374,8 @@ def plot_incremental_trajectory(fignum, values, start=0,
fig = plt.figure(fignum)
axes = fig.gca(projection='3d')
pose3Values = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(pose3Values.keys())
poses = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(poses.keys())
for key in keys[start:]:
if values.exists(key):