gtsam  4.1.0
gtsam
SmartRangeFactor.h
Go to the documentation of this file.
1 
10 #pragma once
11 
12 #include <gtsam_unstable/dllexport.h>
14 #include <gtsam/inference/Key.h>
15 #include <gtsam/geometry/Pose2.h>
16 
17 #include <list>
18 #include <map>
19 #include <stdexcept>
20 #include <string>
21 #include <vector>
22 
23 namespace gtsam {
24 
30  protected:
31  struct Circle2 {
32  Circle2(const Point2& p, double r) :
33  center(p), radius(r) {
34  }
35  Point2 center;
36  double radius;
37  };
38 
39  typedef SmartRangeFactor This;
40 
41  std::vector<double> measurements_;
42  double variance_;
43 
44  public:
47  }
48 
53  explicit SmartRangeFactor(double s) :
54  NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) {
55  }
56 
57  virtual ~SmartRangeFactor() {
58  }
59 
61  void addRange(Key key, double measuredRange) {
62  if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
63  throw std::invalid_argument(
64  "SmartRangeFactor::addRange: adding duplicate measurement for key.");
65  }
66  keys_.push_back(key);
67  measurements_.push_back(measuredRange);
68  size_t n = keys_.size();
69  // Since we add the errors, the noise variance adds
70  noiseModel_ = noiseModel::Isotropic::Variance(1, n * variance_);
71  }
72 
73  // Testable
74 
76  void print(const std::string& s = "",
77  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
78  std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
80  }
81 
83  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
84  return false;
85  }
86 
87  // factor interface
88 
94  Point2 triangulate(const Values& x) const {
95  // create n circles corresponding to measured range around each pose
96  std::list<Circle2> circles;
97  size_t n = size();
98  for (size_t j = 0; j < n; j++) {
99  const Pose2& pose = x.at<Pose2>(keys_[j]);
100  circles.push_back(Circle2(pose.translation(), measurements_[j]));
101  }
102 
103  Circle2 circle1 = circles.front();
104  boost::optional<Point2> best_fh;
105  auto bestCircle2 = boost::make_optional(false, circle1); // fixes issue #38
106 
107  // loop over all circles
108  for (const Circle2& it : circles) {
109  // distance between circle centers.
110  double d = distance2(circle1.center, it.center);
111  if (d < 1e-9)
112  continue; // skip circles that are in the same location
113  // Find f and h, the intersection points in normalized circles
114  boost::optional<Point2> fh = circleCircleIntersection(
115  circle1.radius / d, it.radius / d);
116  // Check if this pair is better by checking h = fh->y()
117  // if h is large, the intersections are well defined.
118  if (fh && (!best_fh || fh->y() > best_fh->y())) {
119  best_fh = fh;
120  bestCircle2 = it;
121  }
122  }
123 
124  // use best fh to find actual intersection points
125  if (bestCircle2 && best_fh) {
126  auto bestCircleCenter = bestCircle2->center;
127  std::list<Point2> intersections =
128  circleCircleIntersection(circle1.center, bestCircleCenter, best_fh);
129 
130  // pick winner based on other measurements
131  double error1 = 0, error2 = 0;
132  Point2 p1 = intersections.front(), p2 = intersections.back();
133  for (const Circle2& it : circles) {
134  error1 += distance2(it.center, p1);
135  error2 += distance2(it.center, p2);
136  }
137  return (error1 < error2) ? p1 : p2;
138  } else {
139  throw std::runtime_error("triangulate failed");
140  }
141  }
142 
146  Vector unwhitenedError(const Values& x,
147  boost::optional<std::vector<Matrix>&> H = boost::none) const override {
148  size_t n = size();
149  if (n < 3) {
150  if (H) {
151  // set Jacobians to zero for n<3
152  for (size_t j = 0; j < n; j++)
153  (*H)[j] = Matrix::Zero(3, 1);
154  }
155  return Z_1x1;
156  } else {
157  Vector error = Z_1x1;
158 
159  // triangulate to get the optimized point
160  // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ?
161  Point2 optimizedPoint = triangulate(x);
162 
163  // TODO(dellaert): triangulation should be followed by an optimization given poses
164  // now evaluate the errors between predicted and measured range
165  for (size_t j = 0; j < n; j++) {
166  const Pose2& pose = x.at<Pose2>(keys_[j]);
167  if (H)
168  // also calculate 1*3 derivative for each of the n poses
169  error[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j];
170  else
171  error[0] += pose.range(optimizedPoint) - measurements_[j];
172  }
173  return error;
174  }
175  }
176 
178  gtsam::NonlinearFactor::shared_ptr clone() const override {
179  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
180  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
181  }
182 };
183 } // \namespace gtsam
184 
gtsam::SmartRangeFactor::Circle2
Definition: SmartRangeFactor.h:31
gtsam::SmartRangeFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: SmartRangeFactor.h:76
gtsam::SmartRangeFactor::measurements_
std::vector< double > measurements_
Range measurements.
Definition: SmartRangeFactor.h:41
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition: Pose2.h:230
gtsam::SmartRangeFactor
Definition: SmartRangeFactor.h:29
gtsam::noiseModel::Isotropic::Variance
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
An isotropic noise model created by specifying a variance = sigma^2.
Definition: NoiseModel.cpp:573
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam::Pose2::range
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate range to a landmark.
Definition: Pose2.cpp:247
gtsam::Pose2
Definition: Pose2.h:36
gtsam::SmartRangeFactor::addRange
void addRange(Key key, double measuredRange)
Add a range measurement to a pose with given key.
Definition: SmartRangeFactor.h:61
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::SmartRangeFactor::SmartRangeFactor
SmartRangeFactor(double s)
Constructor.
Definition: SmartRangeFactor.h:53
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
Pose2.h
2D Pose
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::SmartRangeFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: SmartRangeFactor.h:83
Key.h
gtsam::SmartRangeFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition: SmartRangeFactor.h:146
gtsam::SmartRangeFactor::triangulate
Point2 triangulate(const Values &x) const
Triangulate a point from at least three pose-range pairs Checks for best pair that includes first poi...
Definition: SmartRangeFactor.h:94
gtsam::KeyFormatter
boost::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::SmartRangeFactor::variance_
double variance_
variance on noise
Definition: SmartRangeFactor.h:42
gtsam::SmartRangeFactor::SmartRangeFactor
SmartRangeFactor()
Default constructor: don't use directly.
Definition: SmartRangeFactor.h:46
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::SmartRangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: SmartRangeFactor.h:178
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition: NonlinearFactor.cpp:119