131 lines
3.3 KiB
C++
131 lines
3.3 KiB
C++
/**
|
|
* @file SmartRangeFactor.h
|
|
*
|
|
* @brief A smart factor for range-only SLAM that does initialization and marginalization
|
|
*
|
|
* @date Sep 10, 2012
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam_unstable/base/dllexport.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/nonlinear/Key.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <boost/foreach.hpp>
|
|
#include <map>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Smart factor for range SLAM
|
|
* @addtogroup SLAM
|
|
*/
|
|
class GTSAM_UNSTABLE_EXPORT SmartRangeFactor: public NoiseModelFactor {
|
|
protected:
|
|
|
|
struct Circle2 {
|
|
Circle2(const Point2& p, double r) :
|
|
center(p), radius(r) {
|
|
}
|
|
Point2 center;
|
|
double radius;
|
|
};
|
|
|
|
/// Range measurements
|
|
std::vector<double> measurements_;
|
|
|
|
public:
|
|
|
|
/** Default constructor: don't use directly */
|
|
SmartRangeFactor() {
|
|
}
|
|
|
|
/** standard binary constructor */
|
|
SmartRangeFactor(const SharedNoiseModel& model) {
|
|
}
|
|
|
|
virtual ~SmartRangeFactor() {
|
|
}
|
|
|
|
/// Add a range measurement to a pose with given key.
|
|
void addRange(Key key, double measuredRange) {
|
|
keys_.push_back(key);
|
|
measurements_.push_back(measuredRange);
|
|
}
|
|
|
|
// Testable
|
|
|
|
/** print */
|
|
virtual void print(const std::string& s = "",
|
|
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
|
}
|
|
|
|
/** Check if two factors are equal */
|
|
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
|
|
return false;
|
|
}
|
|
|
|
// factor interface
|
|
|
|
/**
|
|
* 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) {
|
|
Circle2 circle1 = circles.front();
|
|
boost::optional<Point2> best_fh;
|
|
boost::optional<Circle2> best_circle;
|
|
BOOST_FOREACH(const Circle2& it, circles) {
|
|
// distance between circle centers.
|
|
double d = circle1.center.dist(it.center);
|
|
if (d < 1e-9)
|
|
continue;
|
|
boost::optional<Point2> fh = Point2::CircleCircleIntersection(
|
|
circle1.radius / d, it.radius / d);
|
|
if (fh && (!best_fh || fh->y() > best_fh->y())) {
|
|
best_fh = fh;
|
|
best_circle = it;
|
|
}
|
|
}
|
|
std::list<Point2> solutions = Point2::CircleCircleIntersection(
|
|
circle1.center, best_circle->center, best_fh);
|
|
// TODO, pick winner based on other measurement
|
|
return solutions.front();
|
|
}
|
|
|
|
/**
|
|
* Error function *without* the NoiseModel, \f$ z-h(x) \f$.
|
|
*/
|
|
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);
|
|
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];
|
|
}
|
|
}
|
|
return errors;
|
|
}
|
|
|
|
};
|
|
|
|
} // \namespace gtsam
|
|
|