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

- add include ClusterTree-inl.h into JunctionTree-inl to avoid the "Clique->addChild(...)" undefined reference issue 

- clean up ISAMLoop and fix comments in vISAMexample
release/4.3a0
Duy-Nguyen Ta 2010-10-22 03:40:47 +00:00
parent f354f8183d
commit 58f23eb6ad
6 changed files with 71 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Makefile.am
View File

@ -18,7 +18,7 @@ noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
#noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
SUBDIRS = vSLAMexample # does not compile....
#SUBDIRS = vSLAMexample # does not compile....
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------

74
examples/vSLAMexample/ISAMLoop-inl.h
View File

@ -8,10 +8,8 @@
#pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
//#include <BayesTree-inl.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/nonlinear/Ordering.h>
//#include <gtsam/inference/IndexTable.h>
#include <boost/foreach.hpp>
#include "ISAMLoop.h"
@ -21,39 +19,32 @@ using namespace gtsam;
/* ************************************************************************* */
template<class Values>
void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialValues) {
// Reorder and relinearize every reorderInterval updates
if(newFactors.size() > 0) {
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize();
reorderCounter_ = 0;
}
factors_.push_back(newFactors);
// Reorder and relinearize every reorderInterval updates
if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
reorder_relinearize();
reorderCounter_ = 0;
}
// BOOST_FOREACH(typename Factors::sharedFactor f, newFactors) {
// f->print("Adding factor: ");
// }
factors_.push_back(newFactors);
// Linearize new factors and insert them
// TODO: optimize for whole config?
linPoint_.insert(initialValues);
// Linearize new factors and insert them
// TODO: optimize for whole config?
linPoint_.insert(initialValues);
// Augment ordering
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
ordering_.tryInsert(key, ordering_.nVars());
}
}
// Augment ordering
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
ordering_.tryInsert(key, ordering_.nVars());
}
}
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
cout << "After linearize: " << endl;
BOOST_FOREACH(GaussianFactorGraph::sharedFactor f, *linearizedNewFactors) {
f->print("Linearized factor: ");
}
cout << "Update ISAM: " << endl;
isam.update(*linearizedNewFactors);
// Update ISAM
isam.update(*linearizedNewFactors);
}
}
@ -61,8 +52,6 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
template<class Values>
void ISAMLoop<Values>::reorder_relinearize() {
//cout << "Reordering " << reorderCounter_;
cout << "Reordering, relinearizing..." << endl;
// Obtain the new linearization point
@ -73,42 +62,19 @@ void ISAMLoop<Values>::reorder_relinearize() {
// Compute an ordering
ordering_ = *factors_.orderingCOLAMD(newLinPoint);
// cout << "Got estimate" << endl;
// newLinPoint.print("newLinPoint");
// factors_.print("factors");
// Create a linear factor graph at the new linearization point
boost::shared_ptr<GaussianFactorGraph> gfg(factors_.linearize(newLinPoint, ordering_));
// Just recreate the whole BayesTree
isam.update(*gfg);
//cout << "Reeliminating..." << endl;
// // Eliminate linear factor graph to a BayesNet with colamd ordering
// Ordering ordering = gfg->getOrdering();
// const BayesNet<GaussianConditional> bn(
// eliminate<GaussianFactor, GaussianConditional>(*gfg, ordering));
//
//// cout << "Rebuilding BayesTree..." << endl;
//
// // Replace the BayesTree with a new one
// isam.clear();
// BOOST_REVERSE_FOREACH(const GaussianISAM::sharedConditional c, bn) {
// isam.insert(c, ordering);
// }
// Update linearization point
linPoint_ = newLinPoint;
// cout << "Done!" << endl;
}
/* ************************************************************************* */
template<class Values>
Values ISAMLoop<Values>::estimate() {
// cout << "ISAMLoop::estimate(): " << endl;
// linPoint_.print("linPoint_");
// isam.print("isam");
if(isam.size() > 0)
return linPoint_.expmap(optimize(isam), ordering_);
else

1
examples/vSLAMexample/ISAMLoop.h
View File

@ -50,7 +50,6 @@ public:
/** Return the current solution estimate */
Values estimate();
Values calculateEstimate() { return estimate(); }
/** Return the current linearization point */
const Values& getLinearizationPoint() { return linPoint_; }

31
examples/vSLAMexample/vISAMexample.cpp
View File

@ -41,8 +41,8 @@ string g_dataFolder;
// Store groundtruth values, read from files
shared_ptrK g_calib;
map<int, Point3> g_landmarks; // map: <landmark_id, landmark_position>
std::vector<Pose3> g_poses; // map: <camera_id, pose>
std::vector<std::vector<Feature2D> > g_measurements; // map: <camera_id, detected_features> -- where: Feature2D: {camera_id, landmark_id, 2d feature_position}
std::vector<Pose3> g_poses; // prior camera poses at each frame
std::vector<std::vector<Feature2D> > g_measurements; // feature sets detected at each frame
// Noise models
SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
@ -52,13 +52,13 @@ SharedGaussian poseSigma(noiseModel::Unit::Create(1));
/* ************************************************************************* */
/**
* Read all data: calibration file, landmarks, poses, and all features measurements
* Data is stored in global variables.
* Data is stored in global variables, which are used later to simulate incremental updates.
*/
void readAllData()
void readAllDataISAM()
{
g_calib = readCalibData(g_dataFolder + CALIB_FILE);
// Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
// Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
// Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
@ -70,9 +70,7 @@ void readAllData()
/* ************************************************************************* */
/**
* Setup vSLAM graph
* by adding and linking 2D features (measurements) detected in each captured image
* with their corresponding landmarks.
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, Pose3& pose,
@ -119,24 +117,23 @@ int main(int argc, char* argv[])
exit(0);
}
g_dataFolder = string(argv[1]);
g_dataFolder += "/";
readAllData();
g_dataFolder = string(argv[1]) + "/";
readAllDataISAM();
ISAMLoop<Values> isam(3);
// Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3;
ISAMLoop<Values> isam(relinearizeInterval);
// At each frame i with new camera pose and new set of measurements associated with it,
// create a graph of new factors and update ISAM
for (size_t i = 0; i<g_measurements.size(); i++)
{
{
shared_ptr<Graph> newFactors;
shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues,
i, g_poses[i],
g_measurements[i], measurementSigma, g_calib);
// cout << "Add prior pose and measurements of camera " << i << endl;
// newFactors->print();
// initialValues->print();
isam.update(*newFactors, *initialValues);
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;

70
inference/ISAM-inl.h
View File

@ -28,51 +28,51 @@ using namespace boost::assign;
namespace gtsam {
using namespace std;
using namespace std;
/** Create an empty Bayes Tree */
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
/** Create an empty Bayes Tree */
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM() : BayesTree<CONDITIONAL>() {}
/** Create a Bayes Tree from a Bayes Net */
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
BayesTree<CONDITIONAL>(bayesNet) {}
/** Create a Bayes Tree from a Bayes Net */
template<class CONDITIONAL>
ISAM<CONDITIONAL>::ISAM(const BayesNet<CONDITIONAL>& bayesNet) :
BayesTree<CONDITIONAL>(bayesNet) {}
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
/* ************************************************************************* */
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update_internal(const FACTORGRAPH& newFactors, Cliques& orphans) {
// Remove the contaminated part of the Bayes tree
BayesNet<CONDITIONAL> bn;
removeTop(newFactors.keys(), bn, orphans);
FACTORGRAPH factors(bn);
// Remove the contaminated part of the Bayes tree
BayesNet<CONDITIONAL> bn;
removeTop(newFactors.keys(), bn, orphans);
FACTORGRAPH factors(bn);
// add the factors themselves
factors.push_back(newFactors);
// add the factors themselves
factors.push_back(newFactors);
// eliminate into a Bayes net
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
// eliminate into a Bayes net
typename BayesNet<CONDITIONAL>::shared_ptr bayesNet = GenericSequentialSolver<typename CONDITIONAL::Factor>(factors).eliminate();
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
this->insert(*rit);
// insert conditionals back in, straight into the topless bayesTree
typename BayesNet<CONDITIONAL>::const_reverse_iterator rit;
for ( rit=bayesNet->rbegin(); rit != bayesNet->rend(); ++rit )
this->insert(*rit);
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
this->insert(orphan);
}
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
this->insert(orphan);
}
}
}
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
Cliques orphans;
this->update_internal(newFactors, orphans);
}
template<class CONDITIONAL>
template<class FACTORGRAPH>
void ISAM<CONDITIONAL>::update(const FACTORGRAPH& newFactors) {
Cliques orphans;
this->update_internal(newFactors, orphans);
}
}
/// namespace gtsam

1
inference/JunctionTree-inl.h
View File

@ -25,6 +25,7 @@
#include <gtsam/inference/inference-inl.h>
#include <gtsam/inference/VariableSlots-inl.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>
#include <gtsam/inference/ClusterTree-inl.h>
#include <boost/foreach.hpp>
#include <boost/pool/pool_alloc.hpp>