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

Merge pull request #64 from borglab/fix/38_SmartRangeExamples-2 

Fix/38 smart range examples 2
release/4.3a0
Frank Dellaert 2019-06-13 00:10:09 -04:00 committed by GitHub
parent 39e08c9a4e 5017f3e774
commit 28235955d0
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 54 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/RangeISAMExample_plaza2.cpp
View File

@ -108,7 +108,7 @@ int main (int argc, char** argv) {
// Set Noise parameters // Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI); Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2); Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
double sigmaR = 100; // range standard deviation double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
@ -157,7 +157,7 @@ int main (int argc, char** argv) {
boost::tie(t, odometry) = timedOdometry; boost::tie(t, odometry) = timedOdometry;
// add odometry factor // add odometry factor
newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas))); newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry, odoNoise));
// predict pose and add as initial estimate // predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry); Pose2 predictedPose = lastPose.compose(odometry);

20
gtsam/slam/SmartFactorBase.h
View File

@ -118,14 +118,17 @@ public:
} }
/** /**
* Add a new measurement and pose key * Add a new measurement and pose/camera key
* @param measured_i is the 2m dimensional projection of a single landmark * @param measured is the 2m dimensional projection of a single landmark
* @param poseKey is the index corresponding to the camera observing the landmark * @param key is the index corresponding to the camera observing the landmark
* @param sharedNoiseModel is the measurement noise
*/ */
void add(const Z& measured_i, const Key& cameraKey_i) { void add(const Z& measured, const Key& key) {
this->measured_.push_back(measured_i); if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
this->keys_.push_back(cameraKey_i); throw std::invalid_argument(
"SmartFactorBase::add: adding duplicate measurement for key.");
}
this->measured_.push_back(measured);
this->keys_.push_back(key);
} }
/** /**
@ -133,8 +136,7 @@ public:
*/ */
void add(ZVector& measurements, KeyVector& cameraKeys) { void add(ZVector& measurements, KeyVector& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i)); this->add(measurements.at(i), cameraKeys.at(i));
this->keys_.push_back(cameraKeys.at(i));
} }
} }

47
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file SmartRangeExample_plaza2.cpp * @file SmartRangeExample_plaza1.cpp
* @brief A 2D Range SLAM example * @brief A 2D Range SLAM example
* @date June 20, 2013 * @date June 20, 2013
* @author FRank Dellaert * @author FRank Dellaert
@ -42,6 +42,9 @@
#include <gtsam/sam/RangeFactor.h> #include <gtsam/sam/RangeFactor.h>
#include <gtsam_unstable/slam/SmartRangeFactor.h> #include <gtsam_unstable/slam/SmartRangeFactor.h>
// To find data files, we can use `findExampleDataFile`, declared here:
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own // Standard headers, added last, so we know headers above work on their own
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
@ -59,10 +62,9 @@ namespace NM = gtsam::noiseModel;
typedef pair<double, Pose2> TimedOdometry; typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() { list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList; list<TimedOdometry> odometryList;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); string drFile = findExampleDataFile("Plaza1_DR.txt");
if (!is) ifstream is(drFile);
throw runtime_error( if (!is) throw runtime_error("Plaza1_DR.txt file not found");
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
while (is) { while (is) {
double t, distance_traveled, delta_heading; double t, distance_traveled, delta_heading;
@ -79,10 +81,9 @@ list<TimedOdometry> readOdometry() {
typedef boost::tuple<double, size_t, double> RangeTriple; typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() { vector<RangeTriple> readTriples() {
vector<RangeTriple> triples; vector<RangeTriple> triples;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); string tdFile = findExampleDataFile("Plaza1_TD.txt");
if (!is) ifstream is(tdFile);
throw runtime_error( if (!is) throw runtime_error("Plaza1_TD.txt file not found");
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
while (is) { while (is) {
double t, sender, receiver, range; double t, sender, receiver, range;
@ -98,8 +99,6 @@ int main(int argc, char** argv) {
// load Plaza1 data // load Plaza1 data
list<TimedOdometry> odometry = readOdometry(); list<TimedOdometry> odometry = readOdometry();
// size_t M = odometry.size();
vector<RangeTriple> triples = readTriples(); vector<RangeTriple> triples = readTriples();
size_t K = triples.size(); size_t K = triples.size();
@ -112,11 +111,11 @@ int main(int argc, char** argv) {
// Set Noise parameters // Set Noise parameters
Vector priorSigmas = Vector3(1, 1, M_PI); Vector priorSigmas = Vector3(1, 1, M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2); Vector odoSigmas = Vector3(0.05, 0.01, 0.1);
auto odoNoise = NM::Diagonal::Sigmas(odoSigmas);
double sigmaR = 100; // range standard deviation double sigmaR = 100; // range standard deviation
const NM::Base::shared_ptr // all same type const NM::Base::shared_ptr // all same type
priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
rangeNoise = robust ? tukey : gaussian; rangeNoise = robust ? tukey : gaussian;
@ -130,10 +129,8 @@ int main(int argc, char** argv) {
NonlinearFactorGraph newFactors; NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise)); newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
ofstream os2( ofstream os2("rangeResultLM.txt");
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); ofstream os3("rangeResultSR.txt");
ofstream os3(
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");
// initialize points (Gaussian) // initialize points (Gaussian)
Values initial; Values initial;
@ -170,11 +167,11 @@ int main(int argc, char** argv) {
double t; double t;
Pose2 odometry; Pose2 odometry;
boost::tie(t, odometry) = timedOdometry; boost::tie(t, odometry) = timedOdometry;
printf("step %d, time = %g\n",(int)i,t);
// add odometry factor // add odometry factor
newFactors.push_back( newFactors.push_back(
BetweenFactor<Pose2>(i - 1, i, odometry, BetweenFactor<Pose2>(i - 1, i, odometry, odoNoise));
NM::Diagonal::Sigmas(odoSigmas)));
// predict pose and add as initial estimate // predict pose and add as initial estimate
Pose2 predictedPose = lastPose.compose(odometry); Pose2 predictedPose = lastPose.compose(odometry);
@ -188,8 +185,13 @@ int main(int argc, char** argv) {
double range = boost::get<2>(triples[k]); double range = boost::get<2>(triples[k]);
if (i > start) { if (i > start) {
if (smart && totalCount < minK) { if (smart && totalCount < minK) {
smartFactors[j]->addRange(i, range); try {
printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl; smartFactors[j]->addRange(i, range);
printf("adding range %g for %d",range,(int)j);
} catch (const invalid_argument& e) {
printf("warning: omitting duplicate range %g for %d",range,(int)j);
}
cout << endl;
} }
else { else {
RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
@ -254,8 +256,7 @@ int main(int argc, char** argv) {
// Write result to file // Write result to file
Values result = isam.calculateEstimate(); Values result = isam.calculateEstimate();
ofstream os( ofstream os("rangeResult.txt");
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>()) for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl; << it.value.theta() << endl;

23
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -41,6 +41,9 @@
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/RangeFactor.h> #include <gtsam/sam/RangeFactor.h>
// To find data files, we can use `findExampleDataFile`, declared here:
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own // Standard headers, added last, so we know headers above work on their own
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
@ -58,10 +61,9 @@ namespace NM = gtsam::noiseModel;
typedef pair<double, Pose2> TimedOdometry; typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() { list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList; list<TimedOdometry> odometryList;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt"); string drFile = findExampleDataFile("Plaza2_DR.txt");
if (!is) ifstream is(drFile);
throw runtime_error( if (!is) throw runtime_error("Plaza2_DR.txt file not found");
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_DR.txt file not found");
while (is) { while (is) {
double t, distance_traveled, delta_heading; double t, distance_traveled, delta_heading;
@ -78,10 +80,9 @@ list<TimedOdometry> readOdometry() {
typedef boost::tuple<double, size_t, double> RangeTriple; typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() { vector<RangeTriple> readTriples() {
vector<RangeTriple> triples; vector<RangeTriple> triples;
ifstream is("/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt"); string tdFile = findExampleDataFile("Plaza2_TD.txt");
if (!is) ifstream is(tdFile);
throw runtime_error( if (!is) throw runtime_error("Plaza2_TD.txt file not found");
"/Users/dellaert/borg/gtsam/examples/Data/Plaza1_TD.txt file not found");
while (is) { while (is) {
double t, sender, receiver, range; double t, sender, receiver, range;
@ -201,13 +202,11 @@ int main(int argc, char** argv) {
// Write result to file // Write result to file
Values result = isam.calculateEstimate(); Values result = isam.calculateEstimate();
ofstream os2( ofstream os2("rangeResultLM.txt");
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>()) for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
<< endl; << endl;
ofstream os( ofstream os("rangeResult.txt");
"/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>()) for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
<< it.value.theta() << endl; << it.value.theta() << endl;

9
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -13,7 +13,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <list> #include <list>
#include <map> #include <map>
@ -60,6 +59,10 @@ class SmartRangeFactor: public NoiseModelFactor {
/// Add a range measurement to a pose with given key. /// Add a range measurement to a pose with given key.
void addRange(Key key, double measuredRange) { void addRange(Key key, double measuredRange) {
if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
throw std::invalid_argument(
"SmartRangeFactor::addRange: adding duplicate measurement for key.");
}
keys_.push_back(key); keys_.push_back(key);
measurements_.push_back(measuredRange); measurements_.push_back(measuredRange);
size_t n = keys_.size(); size_t n = keys_.size();
@ -89,7 +92,6 @@ class SmartRangeFactor: public NoiseModelFactor {
* Raise runtime_error if not well defined. * Raise runtime_error if not well defined.
*/ */
Point2 triangulate(const Values& x) const { Point2 triangulate(const Values& x) const {
// gttic_(triangulate);
// create n circles corresponding to measured range around each pose // create n circles corresponding to measured range around each pose
std::list<Circle2> circles; std::list<Circle2> circles;
size_t n = size(); size_t n = size();
@ -100,7 +102,7 @@ class SmartRangeFactor: public NoiseModelFactor {
Circle2 circle1 = circles.front(); Circle2 circle1 = circles.front();
boost::optional<Point2> best_fh; boost::optional<Point2> best_fh;
boost::optional<Circle2> bestCircle2; auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38
// loop over all circles // loop over all circles
for (const Circle2& it : circles) { for (const Circle2& it : circles) {
@ -136,7 +138,6 @@ class SmartRangeFactor: public NoiseModelFactor {
} else { } else {
throw std::runtime_error("triangulate failed"); throw std::runtime_error("triangulate failed");
} }
// gttoc_(triangulate);
} }
/** /**

2
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -46,7 +46,7 @@ TEST( SmartRangeFactor, constructor ) {
TEST( SmartRangeFactor, addRange ) { TEST( SmartRangeFactor, addRange ) {
SmartRangeFactor f(sigma); SmartRangeFactor f(sigma);
f.addRange(1, 10); f.addRange(1, 10);
f.addRange(1, 12); f.addRange(2, 12);
LONGS_EQUAL(2, f.size()) LONGS_EQUAL(2, f.size())
} }
/* ************************************************************************* */ /* ************************************************************************* */