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

Merged from branch 'trunk'

release/4.3a0
Richard Roberts 2013-07-01 13:11:15 +00:00
parent b1fdf32a8c 86cf063a5e
commit d7767188b3
12 changed files with 333 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam.h
View File

@ -2047,6 +2047,7 @@ virtual class ISAM2 : gtsam::ISAM2BayesTree {
gtsam::Values getLinearizationPoint() const;
gtsam::Values calculateEstimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::Values calculateBestEstimate() const;
gtsam::VectorValues getDelta() const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;

7
gtsam/nonlinear/ISAM2.cpp
View File

@ -1018,6 +1018,13 @@ Values ISAM2::calculateEstimate() const {
return ret;
}
/* ************************************************************************* */
Matrix ISAM2::marginalCovariance(Index key) const {
return marginalFactor(ordering_[key],
params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
->information().inverse();
}
/* ************************************************************************* */
Values ISAM2::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));

3
gtsam/nonlinear/ISAM2.h
View File

@ -585,6 +585,9 @@ public:
template<class VALUE>
VALUE calculateEstimate(Key key) const;
/** Return marginal on any variable as a covariance matrix */
GTSAM_EXPORT Matrix marginalCovariance(Index key) const;
/// @name Public members for non-typical usage
/// @{

265
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp Normal file
View File

@ -0,0 +1,265 @@
/* ----------------------------------------------------------------------------
* 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 SmartRangeExample_plaza2.cpp
* @brief A 2D Range SLAM example
* @date June 20, 2013
* @author FRank Dellaert
*/
// Both relative poses and recovered trajectory poses will be stored as Pose2 objects
#include <gtsam/geometry/Pose2.h>
// Each variable in the system (poses and landmarks) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use Symbols
#include <gtsam/nonlinear/Symbol.h>
// We want to use iSAM2 to solve the range-SLAM problem incrementally
#include <gtsam/nonlinear/ISAM2.h>
// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
// and initial guesses for any new variables used in the added factors
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
// We will use a non-liear solver to batch-inituialize from the first 150 frames
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam_unstable/slam/SmartRangeFactor.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
namespace NM = gtsam::noiseModel;
// data available at http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/
// Datafile format (from http://www.frc.ri.cmu.edu/projects/emergencyresponse/RangeData/log.html)
// load the odometry
// DR: Odometry Input (delta distance traveled and delta heading change)
// Time (sec) Delta Dist. Trav. (m) Delta Heading (rad)
typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
if (!is)
throw runtime_error(
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
while (is) {
double t, distance_traveled, delta_heading;
is >> t >> distance_traveled >> delta_heading;
odometryList.push_back(
TimedOdometry(t, Pose2(distance_traveled, 0, delta_heading)));
}
is.clear(); /* clears the end-of-file and error flags */
return odometryList;
}
// load the ranges from TD
// Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() {
vector<RangeTriple> triples;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
if (!is)
throw runtime_error(
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
while (is) {
double t, sender, receiver, range;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}
is.clear(); /* clears the end-of-file and error flags */
return triples;
}
// main
int main(int argc, char** argv) {
// load Plaza1 data
list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples();
size_t K = triples.size();
// parameters
size_t start = 220, end=3000;
size_t minK = 100; // first batch of smart factors
size_t incK = 50; // minimum number of range measurements to process after
bool robust = true;
bool smart = true;
// Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian;
// Initialize iSAM
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
ofstream os2(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
ofstream os3(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
// initialize points (Gaussian)
Values initial;
if (!smart) {
initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
}
Values landmarkEstimates = initial; // copy landmarks
initial.insert(0, pose0);
// initialize smart range factors
size_t ids[] = { 1, 6, 0, 5 };
typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
map<size_t, SmartPtr> smartFactors;
if (smart) {
BOOST_FOREACH(size_t jj,ids) {
smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
newFactors.add(smartFactors[jj]);
}
}
// set some loop variables
size_t i = 1; // step counter
size_t k = 0; // range measurement counter
Pose2 lastPose = pose0;
size_t countK = 0, totalCount=0;
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.add(
BetweenFactor<Pose2>(i - 1, i, odometry,
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
lastPose = predictedPose;
initial.insert(i, predictedPose);
landmarkEstimates.insert(i, predictedPose);
// Check if there are range factors to be added
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
if (i > start) {
if (smart && totalCount < minK) {
smartFactors[j]->addRange(i, range);
printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
}
else {
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
rangeNoise);
// Throw out obvious outliers based on current landmark estimates
Vector error = factor.unwhitenedError(landmarkEstimates);
if (k <= 200 || fabs(error[0]) < 5)
newFactors.add(factor);
}
totalCount += 1;
}
k = k + 1;
countK = countK + 1;
}
// Check whether to update iSAM 2
if (k >= minK && countK >= incK) {
gttic_(update);
isam.update(newFactors, initial);
gttoc_(update);
gttic_(calculateEstimate);
Values result = isam.calculateEstimate();
gttoc_(calculateEstimate);
lastPose = result.at<Pose2>(i);
bool hasLandmarks = result.exists(symbol('L', ids[0]));
if (hasLandmarks) {
// update landmark estimates
landmarkEstimates = Values();
BOOST_FOREACH(size_t jj,ids)
landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
}
newFactors = NonlinearFactorGraph();
initial = Values();
if (smart && !hasLandmarks) {
cout << "initialize from smart landmarks" << endl;
BOOST_FOREACH(size_t jj,ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
initial.insert(symbol('L', jj), landmark);
landmarkEstimates.insert(symbol('L', jj), landmark);
}
}
countK = 0;
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
if (smart) {
BOOST_FOREACH(size_t jj,ids) {
Point2 landmark = smartFactors[jj]->triangulate(result);
os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
<< endl;
}
}
}
i += 1;
if (i>end) break;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
gttoc_(iSAM);
// Print timings
tictoc_print_();
// Write result to file
Values result = isam.calculateEstimate();
ofstream os(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);
}

35
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -61,7 +61,8 @@ list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt");
if (!is)
throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
throw runtime_error(
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
while (is) {
double t, distance_traveled, delta_heading;
@ -80,7 +81,8 @@ vector<RangeTriple> readTriples() {
vector<RangeTriple> triples;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt");
if (!is)
throw runtime_error("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
throw runtime_error(
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
while (is) {
double t, sender, receiver, range;
@ -92,7 +94,7 @@ vector<RangeTriple> readTriples() {
}
// main
int main (int argc, char** argv) {
int main(int argc, char** argv) {
// load Plaza1 data
list<TimedOdometry> odometry = readOdometry();
@ -106,7 +108,7 @@ int main (int argc, char** argv) {
bool robust = false;
// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector priorSigmas = Vector3(0.01, 0.01, 0.01);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type
@ -120,8 +122,7 @@ int main (int argc, char** argv) {
ISAM2 isam;
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.add(PriorFactor<Pose2>(0, pose0, priorNoise));
@ -149,7 +150,9 @@ int main (int argc, char** argv) {
boost::tie(t, odometry) = timedOdometry;
// add odometry factor
newFactors.add(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));
newFactors.add(
BetweenFactor<Pose2>(i - 1, i, odometry,
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry);
@ -161,10 +164,10 @@ int main (int argc, char** argv) {
while (k < K && t >= boost::get<0>(triples[k])) {
size_t j = boost::get<1>(triples[k]);
double range = boost::get<2>(triples[k]);
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,rangeNoise);
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise);
// Throw out obvious outliers based on current landmark estimates
Vector error=factor.unwhitenedError(landmarkEstimates);
if (k<=200 || fabs(error[0])<5)
Vector error = factor.unwhitenedError(landmarkEstimates);
if (k <= 200 || fabs(error[0]) < 5)
newFactors.add(factor);
k = k + 1;
countK = countK + 1;
@ -199,12 +202,16 @@ int main (int argc, char** argv) {
// Write result to file
Values result = isam.calculateEstimate();
ofstream os2("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
ofstream os2(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& it, result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl;
ofstream os("/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl;
ofstream os(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl;
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl;
exit(0);
}

4
gtsam_unstable/examples/plotRangeResults.p
View File

@ -2,5 +2,5 @@
reset
#set terminal pdf
set title "Smart range SLAM result"
set size ratio 1
plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles
set size ratio -1
plot "rangeResult.txt" using 2:3 with lines, "rangeResultLM.txt" using 2:3:4 with circles, "rangeResultSR.txt" using 2:3:4 with circles

23
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -69,6 +69,8 @@ public:
/** print */
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
NoiseModelFactor::print(s);
}
/** Check if two factors are equal */
@ -82,7 +84,16 @@ public:
* Triangulate a point from at least three pose-range pairs
* Checks for best pair that includes first point
*/
static Point2 triangulate(const std::list<Circle2>& circles) {
Point2 triangulate(const Values& x) const {
gttic_(triangulate);
// create n circles corresponding to measured range around each pose
std::list<Circle2> circles;
size_t n = size();
for (size_t j = 0; j < n; j++) {
const Pose2& pose = x.at<Pose2>(keys_[j]);
circles.push_back(Circle2(pose.translation(), measurements_[j]));
}
Circle2 circle1 = circles.front();
boost::optional<Point2> best_fh;
boost::optional<Circle2> best_circle;
@ -116,6 +127,7 @@ public:
error2 += it.center.dist(p2);
}
return (error1 < error2) ? p1 : p2;
gttoc_(triangulate);
}
/**
@ -133,16 +145,9 @@ public:
} else {
Vector error = zero(1);
// create n circles corresponding to measured range around each pose
std::list<Circle2> circles;
for (size_t j = 0; j < n; j++) {
const Pose2& pose = x.at<Pose2>(keys_[j]);
circles.push_back(Circle2(pose.translation(), measurements_[j]));
}
// triangulate to get the optimized point
// TODO: Should we have a (better?) variant that does this in relative coordinates ?
Point2 optimizedPoint = triangulate(circles);
Point2 optimizedPoint = triangulate(x);
// TODO: triangulation should be followed by an optimization given poses
// now evaluate the errors between predicted and measured range

4
matlab/gtsam_examples/RangeISAMExample_plaza.m
View File

@ -27,7 +27,7 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m)
% TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
if false % switch between data files
if true % switch between data files
datafile = findExampleDataFile('Plaza1_.mat');
headingOffset=0;
minK=200; % minimum number of range measurements to process initially
@ -65,7 +65,7 @@ isam = ISAM2;
%% Add prior on first pose
pose0 = Pose2(GT(1,2),GT(1,3),headingOffset+GT(1,4));
newFactors = NonlinearFactorGraph;
if ~addRange | ~useGroundTruth
if ~addRange || ~useGroundTruth
newFactors.add(PriorFactorPose2(0,pose0,noiseModels.prior));
end
initial = Values;

6
matlab/gtsam_examples/RangeSLAMExample_plaza.m
View File

@ -27,7 +27,7 @@ import gtsam.*
% Time (sec) X_pose (m) Y_pose (m)
% TD
% Time (sec) Sender / Antenna ID Receiver Node ID Range (m)
datafile = findExampleDataFile('Plaza1_.mat');
datafile = findExampleDataFile('Plaza2_.mat');
load(datafile)
M=size(DR,1);
K=size(TD,1);
@ -42,7 +42,7 @@ base = noiseModel.mEstimator.Tukey(5);
noiseModels.range = noiseModel.Robust(base,noiseModel.Isotropic.Sigma(1, sigmaR));
%% Add prior on first pose
pose0 = Pose2(GT(1,2),GT(1,3),GT(1,4));
pose0 = Pose2(GT(1,2),GT(1,3),pi+GT(1,4));
graph = NonlinearFactorGraph;
graph.add(PriorFactorPose2(0,pose0,noiseModels.prior));
initial = Values;
@ -84,7 +84,7 @@ for i=1:M
end
toc
%% GRaph was built, optimize !
%% Graph was built, optimize !
tic
batchOptimizer = LevenbergMarquardtOptimizer(graph, initial);
result = batchOptimizer.optimize();

13
tests/testGaussianISAM2.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
@ -1027,6 +1028,18 @@ TEST_UNSAFE(ISAM2, marginalizeLeaves5)
EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys));
}
/* ************************************************************************* */
TEST(ISAM2, marginalCovariance)
{
// Create isam2
ISAM2 isam = createSlamlikeISAM2();
// Check marginal
Matrix expected = Marginals(isam.getFactorsUnsafe(), isam.getLinearizationPoint()).marginalCovariance(5);
Matrix actual = isam.marginalCovariance(5);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

1
wrap/Class.cpp
View File

@ -478,6 +478,7 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi
proxyFile.oss << " function sobj = saveobj(obj)\n";
proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n";
proxyFile.oss << " sobj = obj.string_serialize();\n";
proxyFile.oss << " end\n";
}
/* ************************************************************************* */

1
wrap/tests/expected/Point3.m
View File

@ -66,6 +66,7 @@ classdef Point3 < handle
function sobj = saveobj(obj)
% SAVEOBJ Saves the object to a matlab-readable format
sobj = obj.string_serialize();
end
end
methods(Static = true)