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  }
36  double radius;
37  };
38 
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  ~SmartRangeFactor() override {
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 
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 
179  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
181  }
182 };
183 } // \namespace gtsam
184 
double error(const Values &c) const override
gtsam::NonlinearFactor::shared_ptr clone() const override
Vector3f p1
Vector2 Point2
Definition: Point2.h:27
int n
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition: Point2.cpp:39
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
void addRange(Key key, double measuredRange)
Add a range measurement to a pose with given key.
std::vector< double > measurements_
Range measurements.
const Point2 & translation() const
translation
Definition: Pose2.h:230
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
const SharedNoiseModel & noiseModel() const
access to the noise model
Point2 triangulate(const Values &x) const
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:573
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Eigen::VectorXd Vector
Definition: Vector.h:38
double variance_
variance on noise
const ValueType at(Key j) const
Definition: Values-inl.h:342
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
boost::shared_ptr< This > shared_ptr
std::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
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
traits
Definition: chartTesting.h:28
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
SharedNoiseModel noiseModel_
Non-linear factor base classes.
size_t size() const
Definition: Factor.h:135
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
boost::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
Definition: Point2.cpp:55
float * p
Circle2(const Point2 &p, double r)
static Point3 p2
const G double tol
Definition: Group.h:83
2D Pose
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
std::ptrdiff_t j
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Definition: Pose2.cpp:253


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:44:16