From e9a8782a51cc7a960512ab73073f7d5f1e1b5581 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 25 Jun 2013 17:12:01 +0000 Subject: [PATCH 1/7] Switched datasets --- matlab/gtsam_examples/RangeISAMExample_plaza.m | 4 ++-- matlab/gtsam_examples/RangeSLAMExample_plaza.m | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/matlab/gtsam_examples/RangeISAMExample_plaza.m b/matlab/gtsam_examples/RangeISAMExample_plaza.m index 788745a3d..cffe81c30 100644 --- a/matlab/gtsam_examples/RangeISAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeISAMExample_plaza.m @@ -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; diff --git a/matlab/gtsam_examples/RangeSLAMExample_plaza.m b/matlab/gtsam_examples/RangeSLAMExample_plaza.m index 24619b69c..bd643d854 100644 --- a/matlab/gtsam_examples/RangeSLAMExample_plaza.m +++ b/matlab/gtsam_examples/RangeSLAMExample_plaza.m @@ -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(); From 0837eab8f5b4545f873809f2e61fd27b78161c6a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 25 Jun 2013 17:13:02 +0000 Subject: [PATCH 2/7] Scripts to test (still not working so-well) smart range factor --- .../examples/SmartRangeExample_plaza1.cpp | 265 ++++++++++++++++++ .../examples/SmartRangeExample_plaza2.cpp | 35 ++- gtsam_unstable/examples/plotRangeResults.p | 4 +- gtsam_unstable/slam/SmartRangeFactor.h | 23 +- 4 files changed, 302 insertions(+), 25 deletions(-) create mode 100644 gtsam_unstable/examples/SmartRangeExample_plaza1.cpp diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp new file mode 100644 index 000000000..ef56ceafb --- /dev/null +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -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 + +// 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 + +// We want to use iSAM2 to solve the range-SLAM problem incrementally +#include + +// 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 +#include + +// We will use a non-liear solver to batch-inituialize from the first 150 frames +#include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics SLAM problems. +#include +#include +#include +#include + +// Standard headers, added last, so we know headers above work on their own +#include +#include +#include + +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 TimedOdometry; +list readOdometry() { + list 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 RangeTriple; +vector readTriples() { + vector 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 odometry = readOdometry(); +// size_t M = odometry.size(); + + vector 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(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 SmartPtr; + map 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(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 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(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::KeyValuePair& it, result.filter()) + 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::KeyValuePair& it, result.filter()) + os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" + << it.value.theta() << endl; + exit(0); +} + diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 2cf4f3307..f52134c22 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -61,7 +61,8 @@ list readOdometry() { list 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 readTriples() { vector 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 readTriples() { } // main -int main (int argc, char** argv) { +int main(int argc, char** argv) { // load Plaza1 data list 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(0, pose0, priorNoise)); @@ -149,7 +150,9 @@ int main (int argc, char** argv) { boost::tie(t, odometry) = timedOdometry; // add odometry factor - newFactors.add(BetweenFactor(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); + newFactors.add( + BetweenFactor(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 factor(i, symbol('L', j), range,rangeNoise); + RangeFactor 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::KeyValuePair& it, result.filter()) - 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::KeyValuePair& it, result.filter()) - 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); } diff --git a/gtsam_unstable/examples/plotRangeResults.p b/gtsam_unstable/examples/plotRangeResults.p index 085bc48b3..08017729f 100755 --- a/gtsam_unstable/examples/plotRangeResults.p +++ b/gtsam_unstable/examples/plotRangeResults.p @@ -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 \ No newline at end of file +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 \ No newline at end of file diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 87fc319a3..9a8d1fae8 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -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& circles) { + Point2 triangulate(const const Values& x) const { + gttic_(triangulate); + // create n circles corresponding to measured range around each pose + std::list circles; + size_t n = size(); + for (size_t j = 0; j < n; j++) { + const Pose2& pose = x.at(keys_[j]); + circles.push_back(Circle2(pose.translation(), measurements_[j])); + } + Circle2 circle1 = circles.front(); boost::optional best_fh; boost::optional 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 circles; - for (size_t j = 0; j < n; j++) { - const Pose2& pose = x.at(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 From f3aa2fde53e7bfb364098b8691c0a257af2b0adc Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 25 Jun 2013 17:22:45 +0000 Subject: [PATCH 3/7] Fixed compile error --- gtsam_unstable/slam/SmartRangeFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 9a8d1fae8..c2a43b019 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -84,7 +84,7 @@ public: * Triangulate a point from at least three pose-range pairs * Checks for best pair that includes first point */ - Point2 triangulate(const const Values& x) const { + Point2 triangulate(const Values& x) const { gttic_(triangulate); // create n circles corresponding to measured range around each pose std::list circles; From ef5fb525deee147ecc0c05b91085ccc8bda8e465 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 25 Jun 2013 18:30:59 +0000 Subject: [PATCH 4/7] add missing end on saveobj --- wrap/Class.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/wrap/Class.cpp b/wrap/Class.cpp index eadaa1521..3b8c41041 100644 --- a/wrap/Class.cpp +++ b/wrap/Class.cpp @@ -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"; } /* ************************************************************************* */ From e1b24acc96e59e94c7e4bf0d6e9aaeba0bcf4b43 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Tue, 25 Jun 2013 18:36:57 +0000 Subject: [PATCH 5/7] add missing end on saveobj --- wrap/tests/expected/Point3.m | 1 + 1 file changed, 1 insertion(+) diff --git a/wrap/tests/expected/Point3.m b/wrap/tests/expected/Point3.m index 775eca916..94d9c25b8 100644 --- a/wrap/tests/expected/Point3.m +++ b/wrap/tests/expected/Point3.m @@ -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) From 77a1e9a485e71b593cee30b53ee4e0cc0d6fce34 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 29 Jun 2013 02:19:03 +0000 Subject: [PATCH 6/7] Added ISAM2::marginalCovariance function. --- gtsam/nonlinear/ISAM2.cpp | 7 +++++++ gtsam/nonlinear/ISAM2.h | 3 +++ tests/testGaussianISAM2.cpp | 13 +++++++++++++ 3 files changed, 23 insertions(+) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index bc18abfb4..dc764c164 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -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_)); diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 3015a51c8..687723392 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -585,6 +585,9 @@ public: template 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 /// @{ diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f3f26900e..aa169c3a0 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -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);} /* ************************************************************************* */ From 86cf063a5e78d1247c3bcb2eed2ae1f567b252b0 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 1 Jul 2013 13:09:17 +0000 Subject: [PATCH 7/7] Added ISAM2::marginalCovariance to wrapper --- gtsam.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam.h b/gtsam.h index 33d26d939..ee069e0d3 100644 --- a/gtsam.h +++ b/gtsam.h @@ -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;