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

Merge branch 'trunk'

release/4.3a0
Richard Roberts 2013-06-24 19:30:24 +00:00
parent 5d258855ca f603624248
commit bad9373aec
5 changed files with 152 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/TestableAssertions.h
View File

@ -91,7 +91,7 @@ bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual,
if(match) {
size_t i = 0;
BOOST_FOREACH(const V& a, expected) {
if (!assert_equal(a, expected[i++], tol)) {
if (!assert_equal(a, actual[i++], tol)) {
match = false;
break;
}

2
gtsam/geometry/Cal3_S2.h
View File

@ -87,7 +87,7 @@ namespace gtsam {
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
/// focal length y
inline double fy() const { return fy_;}
/// skew

19
gtsam/linear/JacobianFactor.cpp
View File

@ -140,17 +140,26 @@ namespace gtsam {
GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())),
model_(model), Ab_(matrix_)
{
// get number of measurements and variables involved in this factor
size_t m = b.size(), n = terms.size();
if(model->dim() != (size_t) b.size())
if(model->dim() != (size_t) m)
throw InvalidNoiseModel(b.size(), model->dim());
size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google.
for(size_t j=0; j<terms.size(); ++j)
// Get the dimensions of each variable and copy to "dims" array, add 1 for RHS
size_t* dims = (size_t*)alloca(sizeof(size_t)*(n+1)); // FIXME: alloca is bad, just ask Google.
for(size_t j=0; j<n; ++j)
dims[j] = terms[j].second.cols();
dims[terms.size()] = 1;
dims[n] = 1;
// Frank is mystified why this is done this way, rather than just creating Ab_
Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+terms.size()+1, b.size()));
for(size_t j=0; j<terms.size(); ++j)
// Now copy the Jacobian matrices from the terms matrix
for(size_t j=0; j<n; ++j) {
assert(terms[j].second.rows()==m);
Ab_(j) = terms[j].second;
}
getb() = b;
assertInvariants();
}

94
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -33,8 +33,10 @@ protected:
double radius;
};
/// Range measurements
std::vector<double> measurements_;
typedef SmartRangeFactor This;
std::vector<double> measurements_; ///< Range measurements
double variance_; ///< variance on noise
public:
@ -42,8 +44,12 @@ public:
SmartRangeFactor() {
}
/** standard binary constructor */
SmartRangeFactor(const SharedNoiseModel& model) {
/**
* Constructor
* @param s standard deviation of range measurement noise
*/
SmartRangeFactor(double s) :
NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) {
}
virtual ~SmartRangeFactor() {
@ -53,6 +59,9 @@ public:
void addRange(Key key, double measuredRange) {
keys_.push_back(key);
measurements_.push_back(measuredRange);
size_t n = keys_.size();
// Since we add the errors, the noise variance adds
noiseModel_ = noiseModel::Isotropic::Variance(1, n * variance_);
}
// Testable
@ -77,22 +86,36 @@ public:
Circle2 circle1 = circles.front();
boost::optional<Point2> best_fh;
boost::optional<Circle2> best_circle;
// loop over all circles
BOOST_FOREACH(const Circle2& it, circles) {
// distance between circle centers.
double d = circle1.center.dist(it.center);
if (d < 1e-9)
continue;
continue; // skip circles that are in the same location
// Find f and h, the intersection points in normalized circles
boost::optional<Point2> fh = Point2::CircleCircleIntersection(
circle1.radius / d, it.radius / d);
// Check if this pair is better by checking h = fh->y()
// if h is large, the intersections are well defined.
if (fh && (!best_fh || fh->y() > best_fh->y())) {
best_fh = fh;
best_circle = it;
}
}
std::list<Point2> solutions = Point2::CircleCircleIntersection(
// use best fh to find actual intersection points
std::list<Point2> intersections = Point2::CircleCircleIntersection(
circle1.center, best_circle->center, best_fh);
// TODO, pick winner based on other measurement
return solutions.front();
// pick winner based on other measurements
double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back();
BOOST_FOREACH(const Circle2& it, circles) {
error1 += it.center.dist(p1);
error2 += it.center.dist(p2);
}
return (error1 < error2) ? p1 : p2;
}
/**
@ -100,28 +123,45 @@ public:
*/
virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const {
size_t K = size();
Vector errors = zero(K);
if (K >= 3) {
std::list<Circle2> circles;
for (size_t i = 0; i < K; i++) {
const Pose2& pose = x.at<Pose2>(keys_[i]);
circles.push_back(Circle2(pose.translation(), measurements_[i]));
}
Point2 optimizedPoint = triangulate(circles);
size_t n = size();
if (n < 3) {
if (H)
*H = std::vector<Matrix>();
for (size_t i = 0; i < K; i++) {
const Pose2& pose = x.at<Pose2>(keys_[i]);
if (H) {
Matrix Hi;
errors[i] = pose.range(optimizedPoint, Hi) - measurements_[i];
H->push_back(Hi);
} else
errors[i] = pose.range(optimizedPoint) - measurements_[i];
// set Jacobians to zero for n<3
for (size_t j = 0; j < n; j++)
(*H)[j] = zeros(3, 1);
return zero(1);
} 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);
// TODO: triangulation should be followed by an optimization given poses
// now evaluate the errors between predicted and measured range
for (size_t j = 0; j < n; j++) {
const Pose2& pose = x.at<Pose2>(keys_[j]);
if (H)
// also calculate 1*3 derivative for each of the n poses
error[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j];
else
error[0] += pose.range(optimizedPoint) - measurements_[j];
}
return error;
}
return errors;
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
};

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

@ -17,67 +17,117 @@
*/
#include <gtsam_unstable/slam/SmartRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
const noiseModel::Base::shared_ptr gaussian = noiseModel::Isotropic::Sigma(1,
2.0);
static const double sigma = 2.0;
// Test situation:
static const Point2 p(0, 10);
static const Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
static const double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(
p);
/* ************************************************************************* */
TEST( SmartRangeFactor, constructor ) {
SmartRangeFactor f1;
LONGS_EQUAL(0, f1.size())
SmartRangeFactor f2(gaussian);
SmartRangeFactor f2(sigma);
LONGS_EQUAL(0, f2.size())
}
/* ************************************************************************* */
TEST( SmartRangeFactor, addRange ) {
SmartRangeFactor f(gaussian);
SmartRangeFactor f(sigma);
f.addRange(1, 10);
f.addRange(1, 12);
LONGS_EQUAL(2, f.size())
}
/* ************************************************************************* */
TEST( SmartRangeFactor, unwhitenedError ) {
// Test situation:
Point2 p(0, 10);
Pose2 pose1(0, 0, 0), pose2(5, 0, 0), pose3(5, 5, 0);
double r1 = pose1.range(p), r2 = pose2.range(p), r3 = pose3.range(p);
TEST( SmartRangeFactor, scenario ) {
DOUBLES_EQUAL(10, r1, 1e-9);
DOUBLES_EQUAL(sqrt(100+25), r2, 1e-9);
DOUBLES_EQUAL(sqrt(50), r3, 1e-9);
}
/* ************************************************************************* */
TEST( SmartRangeFactor, unwhitenedError ) {
Values values; // all correct
values.insert(1, pose1);
values.insert(2, pose2);
values.insert(3, pose3);
SmartRangeFactor f(gaussian);
SmartRangeFactor f(sigma);
f.addRange(1, r1);
// Check Jacobian for n==1
vector<Matrix> H1(1);
f.unwhitenedError(values, H1); // with H now !
CHECK(assert_equal(zeros(3,1), H1.front()));
// Whenever there are two ranges or less, error should be zero
Vector actual1 = f.unwhitenedError(values);
EXPECT(assert_equal(Vector_(1,0.0), actual1));
f.addRange(2, r2);
Vector actual2 = f.unwhitenedError(values);
EXPECT(assert_equal(Vector2(0,0), actual2));
EXPECT(assert_equal(Vector_(1,0.0), actual2));
f.addRange(3, r3);
vector<Matrix> H;
Vector actual3 = f.unwhitenedError(values,H);
EXPECT(assert_equal(Vector3(0,0,0), actual3));
vector<Matrix> H(3);
Vector actual3 = f.unwhitenedError(values);
EXPECT_LONGS_EQUAL(3, f.keys().size());
EXPECT(assert_equal(Vector_(1,0.0), actual3));
// Check keys and Jacobian
EXPECT_LONGS_EQUAL(3,f.keys().size());
EXPECT(assert_equal(Matrix_(1,3,0.0,-1.0,0.0), H.front()));
EXPECT(assert_equal(Matrix_(1,3,sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
Vector actual4 = f.unwhitenedError(values, H); // with H now !
EXPECT(assert_equal(Vector_(1,0.0), actual4));
CHECK(assert_equal(Matrix_(1,3, 0.0,-1.0,0.0), H.front()));
CHECK(assert_equal(Matrix_(1,3, sqrt(2)/2,-sqrt(2)/2,0.0), H.back()));
// Test clone
NonlinearFactor::shared_ptr clone = f.clone();
EXPECT_LONGS_EQUAL(3, clone->keys().size());
}
/* ************************************************************************* */
TEST( SmartRangeFactor, optimization ) {
SmartRangeFactor f(sigma);
f.addRange(1, r1);
f.addRange(2, r2);
f.addRange(3, r3);
// Create initial value for optimization
Values initial;
initial.insert(1, pose1);
initial.insert(2, pose2);
initial.insert(3, Pose2(5, 6, 0)); // does not satisfy range measurement
Vector actual5 = f.unwhitenedError(initial);
EXPECT(assert_equal(Vector_(1,sqrt(25+16)-sqrt(50)), actual5));
// Create Factor graph
NonlinearFactorGraph graph;
graph.add(f);
const noiseModel::Base::shared_ptr //
priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
graph.add(PriorFactor<Pose2>(1, pose1, priorNoise));
graph.add(PriorFactor<Pose2>(2, pose2, priorNoise));
// Try optimizing
LevenbergMarquardtParams params;
// params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();
EXPECT(assert_equal(initial.at<Pose2>(1), result.at<Pose2>(1)));
EXPECT(assert_equal(initial.at<Pose2>(2), result.at<Pose2>(2)));
// only the third pose will be changed, converges on following:
EXPECT(assert_equal(Pose2(5.52159, 5.582727, 0), result.at<Pose2>(3),1e-5));
}
/* ************************************************************************* */