1/**
2 * @file SmartRangeFactor.h
3 *
4 * @brief A smart factor for range-only SLAM that does initialization and marginalization
5 *
6 * @date Sep 10, 2012
7 * @author Alex Cunningham
8 */
9
10#pragma once
11
12#include <gtsam_unstable/dllexport.h>
13#include <gtsam/nonlinear/NonlinearFactor.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
24namespace gtsam {
25
26/**
27 * Smart factor for range SLAM
28 * @ingroup slam
29 */
30class SmartRangeFactor: public NoiseModelFactor {
31 protected:
32 struct Circle2 {
33 Circle2(const Point2& p, double r) :
34 center(p), radius(r) {
35 }
36 Point2 center;
37 double radius;
38 };
39
40 typedef SmartRangeFactor This;
41
42 std::vector<double> measurements_; ///< Range measurements
43 double variance_; ///< variance on noise
44
45 public:
46
47 // Provide access to the Matrix& version of unwhitenedError
48 using NoiseModelFactor::unwhitenedError;
49
50 /** Default constructor: don't use directly */
51 SmartRangeFactor() {
52 }
53
54 /**
55 * Constructor
56 * @param s standard deviation of range measurement noise
57 */
58 explicit SmartRangeFactor(double s) :
59 NoiseModelFactor(noiseModel::Isotropic::Sigma(dim: 1, sigma: s)), variance_(s * s) {
60 }
61
62 ~SmartRangeFactor() override {
63 }
64
65 /// Add a range measurement to a pose with given key.
66 void addRange(Key key, double measuredRange) {
67 if(std::find(first: keys_.begin(), last: keys_.end(), val: key) != keys_.end()) {
68 throw std::invalid_argument(
69 "SmartRangeFactor::addRange: adding duplicate measurement for key.");
70 }
71 keys_.push_back(x: key);
72 measurements_.push_back(x: measuredRange);
73 size_t n = keys_.size();
74 // Since we add the errors, the noise variance adds
75 noiseModel_ = noiseModel::Isotropic::Variance(dim: 1, variance: n * variance_);
76 }
77
78 // Testable
79
80 /** print */
81 void print(const std::string& s = "",
82 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
83 std::cout << s << "SmartRangeFactor with " << size() << " measurements\n";
84 NoiseModelFactor::print(s);
85 }
86
87 /** Check if two factors are equal */
88 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
89 return false;
90 }
91
92 // factor interface
93
94 /**
95 * Triangulate a point from at least three pose-range pairs
96 * Checks for best pair that includes first point
97 * Raise runtime_error if not well defined.
98 */
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>(j: keys_[j]);
105 circles.push_back(x: 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(p1: circle1.center, q: 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 R_d: circle1.radius / d, r_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(c1: circle1.center, c2: bestCircleCenter, fh: 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(p1: it.center, q: p1);
140 error2 += distance2(p1: it.center, q: p2);
141 }
142 return (error1 < error2) ? p1 : p2;
143 } else {
144 throw std::runtime_error("triangulate failed");
145 }
146 }
147
148 /**
149 * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
150 */
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(rows: 3, cols: 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>(j: keys_[j]);
171 if (H)
172 // also calculate 1*3 derivative for each of the n poses
173 error[0] += pose.range(point: optimizedPoint, H1: (*H)[j]) - measurements_[j];
174 else
175 error[0] += pose.range(point: optimizedPoint) - measurements_[j];
176 }
177 return error;
178 }
179 }
180
181 /// @return a deep copy of this factor
182 gtsam::NonlinearFactor::shared_ptr clone() const override {
183 return std::static_pointer_cast<gtsam::NonlinearFactor>(
184 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
185 }
186};
187} // \namespace gtsam
188
189