gtsam  4.1.0
gtsam
utilities.h
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
18 #pragma once
19 
20 #include <gtsam/inference/Symbol.h>
22 #include <gtsam/linear/Sampler.h>
25 #include <gtsam/nonlinear/Values.h>
26 #include <gtsam/geometry/Point2.h>
27 #include <gtsam/geometry/Point3.h>
28 #include <gtsam/geometry/Pose2.h>
29 #include <gtsam/geometry/Pose3.h>
30 #include <gtsam/geometry/Cal3_S2.h>
32 
33 #include <exception>
34 
35 namespace gtsam {
36 
37 namespace utilities {
38 
39 // Create a KeyList from indices
40 FastList<Key> createKeyList(const Vector& I) {
41  FastList<Key> set;
42  for (int i = 0; i < I.size(); i++)
43  set.push_back(I[i]);
44  return set;
45 }
46 
47 // Create a KeyList from indices using symbol
48 FastList<Key> createKeyList(std::string s, const Vector& I) {
49  FastList<Key> set;
50  char c = s[0];
51  for (int i = 0; i < I.size(); i++)
52  set.push_back(Symbol(c, I[i]));
53  return set;
54 }
55 
56 // Create a KeyVector from indices
57 KeyVector createKeyVector(const Vector& I) {
58  KeyVector set;
59  for (int i = 0; i < I.size(); i++)
60  set.push_back(I[i]);
61  return set;
62 }
63 
64 // Create a KeyVector from indices using symbol
65 KeyVector createKeyVector(std::string s, const Vector& I) {
66  KeyVector set;
67  char c = s[0];
68  for (int i = 0; i < I.size(); i++)
69  set.push_back(Symbol(c, I[i]));
70  return set;
71 }
72 
73 // Create a KeySet from indices
74 KeySet createKeySet(const Vector& I) {
75  KeySet set;
76  for (int i = 0; i < I.size(); i++)
77  set.insert(I[i]);
78  return set;
79 }
80 
81 // Create a KeySet from indices using symbol
82 KeySet createKeySet(std::string s, const Vector& I) {
83  KeySet set;
84  char c = s[0];
85  for (int i = 0; i < I.size(); i++)
86  set.insert(symbol(c, I[i]));
87  return set;
88 }
89 
91 Matrix extractPoint2(const Values& values) {
92  size_t j = 0;
93  Values::ConstFiltered<Point2> points = values.filter<Point2>();
94  Matrix result(points.size(), 2);
95  for(const auto& key_value: points)
96  result.row(j++) = key_value.value;
97  return result;
98 }
99 
101 Matrix extractPoint3(const Values& values) {
102  Values::ConstFiltered<Point3> points = values.filter<Point3>();
103  Matrix result(points.size(), 3);
104  size_t j = 0;
105  for(const auto& key_value: points)
106  result.row(j++) = key_value.value;
107  return result;
108 }
109 
111 Values allPose2s(const Values& values) {
112  return values.filter<Pose2>();
113 }
114 
116 Matrix extractPose2(const Values& values) {
117  Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
118  Matrix result(poses.size(), 3);
119  size_t j = 0;
120  for(const auto& key_value: poses)
121  result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
122  return result;
123 }
124 
126 Values allPose3s(const Values& values) {
127  return values.filter<Pose3>();
128 }
129 
131 Matrix extractPose3(const Values& values) {
132  Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
133  Matrix result(poses.size(), 12);
134  size_t j = 0;
135  for(const auto& key_value: poses) {
136  result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
137  result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
138  result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2);
139  result.row(j).tail(3) = key_value.value.translation();
140  j++;
141  }
142  return result;
143 }
144 
146 void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
147  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
148  sigma);
149  Sampler sampler(model, seed);
150  for(const auto& key_value: values.filter<Point2>()) {
151  values.update<Point2>(key_value.key, key_value.value + Point2(sampler.sample()));
152  }
153 }
154 
156 void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
157  42u) {
158  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
159  Vector3(sigmaT, sigmaT, sigmaR));
160  Sampler sampler(model, seed);
161  for(const auto& key_value: values.filter<Pose2>()) {
162  values.update<Pose2>(key_value.key, key_value.value.retract(sampler.sample()));
163  }
164 }
165 
167 void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
168  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
169  sigma);
170  Sampler sampler(model, seed);
171  for(const auto& key_value: values.filter<Point3>()) {
172  values.update<Point3>(key_value.key, key_value.value + Point3(sampler.sample()));
173  }
174 }
175 
177 void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
178  const Vector& J, const Matrix& Z, double depth) {
179  if (Z.rows() != 2)
180  throw std::invalid_argument("insertBackProjections: Z must be 2*K");
181  if (Z.cols() != J.size())
182  throw std::invalid_argument(
183  "insertBackProjections: J and Z must have same number of entries");
184  for (int k = 0; k < Z.cols(); k++) {
185  Point2 p(Z(0, k), Z(1, k));
186  Point3 P = camera.backproject(p, depth);
187  values.insert(J(k), P);
188  }
189 }
190 
192 void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
193  const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
194  const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
195  if (Z.rows() != 2)
196  throw std::invalid_argument("addMeasurements: Z must be 2*K");
197  if (Z.cols() != J.size())
198  throw std::invalid_argument(
199  "addMeasurements: J and Z must have same number of entries");
200  for (int k = 0; k < Z.cols(); k++) {
201  graph.push_back(
202  boost::make_shared<GenericProjectionFactor<Pose3, Point3> >(
203  Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
204  }
205 }
206 
208 Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
209  const Values& values) {
210  // first count
211  size_t K = 0, k = 0;
212  for(const NonlinearFactor::shared_ptr& f: graph)
213  if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
214  f))
215  ++K;
216  // now fill
217  Matrix errors(2, K);
218  for(const NonlinearFactor::shared_ptr& f: graph) {
219  boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
220  boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
221  f);
222  if (p)
223  errors.col(k++) = p->unwhitenedError(values);
224  }
225  return errors;
226 }
227 
229 Values localToWorld(const Values& local, const Pose2& base,
230  const KeyVector user_keys = KeyVector()) {
231 
232  Values world;
233 
234  // if no keys given, get all keys from local values
235  KeyVector keys(user_keys);
236  if (keys.size()==0)
237  keys = local.keys();
238 
239  // Loop over all keys
240  for(Key key: keys) {
241  try {
242  // if value is a Pose2, compose it with base pose
243  Pose2 pose = local.at<Pose2>(key);
244  world.insert(key, base.compose(pose));
245  } catch (const std::exception& e1) {
246  try {
247  // if value is a Point2, transform it from base pose
248  Point2 point = local.at<Point2>(key);
249  world.insert(key, base.transformFrom(point));
250  } catch (const std::exception& e2) {
251  // if not Pose2 or Point2, do nothing
252  #ifndef NDEBUG
253  std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
254  #endif
255  }
256  }
257  }
258  return world;
259 }
260 
261 } // namespace utilities
262 
269 struct RedirectCout {
271  RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {}
272 
274  std::string str() const {
275  return ssBuffer_.str();
276  }
277 
280  std::cout.rdbuf(coutBuffer_);
281  }
282 
283 private:
284  std::stringstream ssBuffer_;
285  std::streambuf* coutBuffer_;
286 };
287 
288 }
289 
gtsam::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
shared pointer to calibration object
Definition: Cal3_S2.h:39
ProjectionFactor.h
Basic bearing factor from 2D measurement.
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::RedirectCout::str
std::string str() const
return the string
Definition: utilities.h:274
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
Sampler.h
sampling from a NoiseModel
gtsam::Values::filter
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)
Return a filtered view of this Values class, without copying any data.
Definition: Values-inl.h:237
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
A diagonal noise model created by specifying a Vector of sigmas, i.e.
Definition: NoiseModel.cpp:270
Pose3.h
3D Pose
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition: Symbol.h:130
Pose2.h
2D Pose
NonlinearFactor.h
Non-linear factor base classes.
Point3.h
3D Point
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::RedirectCout::~RedirectCout
~RedirectCout()
destructor – redirect stdout buffer to its original buffer
Definition: utilities.h:279
Point2.h
2D Point
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::RedirectCout
For Python str().
Definition: utilities.h:269
Symbol.h
Values.h
A non-templated config holding any types of Manifold-group elements.
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition: NoiseModel.cpp:567
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::RedirectCout::RedirectCout
RedirectCout()
constructor – redirect stdout buffer to a stringstream buffer
Definition: utilities.h:271
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition: make_shared.h:57