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 #include <optional>
23 
24 namespace gtsam {
25 
31  protected:
32  struct Circle2 {
33  Circle2(const Point2& p, double r) :
34  center(p), radius(r) {
35  }
37  double radius;
38  };
39 
41 
42  std::vector<double> measurements_;
43  double variance_;
44 
45  public:
46 
47  // Provide access to the Matrix& version of unwhitenedError
49 
52  }
53 
58  explicit SmartRangeFactor(double s) :
59  NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) {
60  }
61 
62  ~SmartRangeFactor() override {
63  }
64 
66  void addRange(Key key, double measuredRange) {
67  if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
68  throw std::invalid_argument(
69  "SmartRangeFactor::addRange: adding duplicate measurement for key.");
70  }
71  keys_.push_back(key);
72  measurements_.push_back(measuredRange);
73  size_t n = keys_.size();
74  // Since we add the errors, the noise variance adds
76  }
77 
78  // Testable
79 
81  void print(const std::string& s = "",
82  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
83  std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
85  }
86 
88  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
89  return false;
90  }
91 
92  // factor interface
93 
99  Point2 triangulate(const Values& x) const {
100  // create n circles corresponding to measured range around each pose
101  std::list<Circle2> circles;
102  size_t n = size();
103  for (size_t j = 0; j < n; j++) {
104  const Pose2& pose = x.at<Pose2>(keys_[j]);
105  circles.push_back(Circle2(pose.translation(), measurements_[j]));
106  }
107 
108  Circle2 circle1 = circles.front();
109  std::optional<Point2> best_fh;
110  std::optional<Circle2> bestCircle2 = std::nullopt; // fixes issue #38
111 
112  // loop over all circles
113  for (const Circle2& it : circles) {
114  // distance between circle centers.
115  double d = distance2(circle1.center, it.center);
116  if (d < 1e-9)
117  continue; // skip circles that are in the same location
118  // Find f and h, the intersection points in normalized circles
119  std::optional<Point2> fh = circleCircleIntersection(
120  circle1.radius / d, it.radius / d);
121  // Check if this pair is better by checking h = fh->y()
122  // if h is large, the intersections are well defined.
123  if (fh && (!best_fh || fh->y() > best_fh->y())) {
124  best_fh = fh;
125  bestCircle2 = it;
126  }
127  }
128 
129  // use best fh to find actual intersection points
130  if (bestCircle2 && best_fh) {
131  auto bestCircleCenter = bestCircle2->center;
132  std::list<Point2> intersections =
133  circleCircleIntersection(circle1.center, bestCircleCenter, best_fh);
134 
135  // pick winner based on other measurements
136  double error1 = 0, error2 = 0;
137  Point2 p1 = intersections.front(), p2 = intersections.back();
138  for (const Circle2& it : circles) {
139  error1 += distance2(it.center, p1);
140  error2 += distance2(it.center, p2);
141  }
142  return (error1 < error2) ? p1 : p2;
143  } else {
144  throw std::runtime_error("triangulate failed");
145  }
146  }
147 
151  Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {
152  size_t n = size();
153  if (n < 3) {
154  if (H) {
155  // set Jacobians to zero for n<3
156  for (size_t j = 0; j < n; j++)
157  (*H)[j] = Matrix::Zero(3, 1);
158  }
159  return Z_1x1;
160  } else {
161  Vector error = Z_1x1;
162 
163  // triangulate to get the optimized point
164  // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ?
165  Point2 optimizedPoint = triangulate(x);
166 
167  // TODO(dellaert): triangulation should be followed by an optimization given poses
168  // now evaluate the errors between predicted and measured range
169  for (size_t j = 0; j < n; j++) {
170  const Pose2& pose = x.at<Pose2>(keys_[j]);
171  if (H)
172  // also calculate 1*3 derivative for each of the n poses
173  error[0] += pose.range(optimizedPoint, (*H)[j]) - measurements_[j];
174  else
175  error[0] += pose.range(optimizedPoint) - measurements_[j];
176  }
177  return error;
178  }
179  }
180 
183  return std::static_pointer_cast<gtsam::NonlinearFactor>(
185  }
186 };
187 } // \namespace gtsam
188 
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Pose2.h
2D Pose
gtsam::SmartRangeFactor::This
SmartRangeFactor This
Definition: SmartRangeFactor.h:40
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
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::SmartRangeFactor::~SmartRangeFactor
~SmartRangeFactor() override
Definition: SmartRangeFactor.h:62
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::SmartRangeFactor::unwhitenedError
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
Definition: SmartRangeFactor.h:151
gtsam::noiseModel::Isotropic::Variance
static shared_ptr Variance(size_t dim, double variance, bool smart=true)
Definition: NoiseModel.cpp:630
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SmartRangeFactor::addRange
void addRange(Key key, double measuredRange)
Add a range measurement to a pose with given key.
Definition: SmartRangeFactor.h:66
gtsam::SmartRangeFactor::triangulate
Point2 triangulate(const Values &x) const
Definition: SmartRangeFactor.h:99
gtsam::SmartRangeFactor::Circle2
Definition: SmartRangeFactor.h:32
gtsam::SmartRangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: SmartRangeFactor.h:182
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::SmartRangeFactor::SmartRangeFactor
SmartRangeFactor()
Definition: SmartRangeFactor.h:51
n
int n
Definition: BiCGSTAB_simple.cpp:1
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Key.h
gtsam::SmartRangeFactor::Circle2::Circle2
Circle2(const Point2 &p, double r)
Definition: SmartRangeFactor.h:33
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
gtsam::KeyFormatter
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
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
gtsam::SmartRangeFactor
Definition: SmartRangeFactor.h:30
gtsam::SmartRangeFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: SmartRangeFactor.h:81
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SmartRangeFactor::variance_
double variance_
variance on noise
Definition: SmartRangeFactor.h:43
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
key
const gtsam::Symbol key('X', 0)
gtsam::SmartRangeFactor::Circle2::center
Point2 center
Definition: SmartRangeFactor.h:36
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
NonlinearFactor.h
Non-linear factor base classes.
gtsam::OptionalMatrixVecType
std::vector< Matrix > * OptionalMatrixVecType
Definition: NonlinearFactor.h:61
gtsam::SmartRangeFactor::Circle2::radius
double radius
Definition: SmartRangeFactor.h:37
gtsam
traits
Definition: SFMdata.h:40
gtsam::SmartRangeFactor::measurements_
std::vector< double > measurements_
Range measurements.
Definition: SmartRangeFactor.h:42
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:88
gtsam::NoiseModelFactor
Definition: NonlinearFactor.h:197
gtsam::Values
Definition: Values.h:65
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::circleCircleIntersection
std::optional< Point2 > circleCircleIntersection(double R_d, double r_d, double tol)
Definition: Point2.cpp:55
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::NoiseModelFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const =0
gtsam::Pose2
Definition: Pose2.h:39
gtsam::SmartRangeFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: SmartRangeFactor.h:88
gtsam::SmartRangeFactor::SmartRangeFactor
SmartRangeFactor(double s)
Definition: SmartRangeFactor.h:58


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:23