nonlinear/utilities.h
Go to the documentation of this file.
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>
26 #include <gtsam/nonlinear/Values.h>
27 #include <gtsam/geometry/Point2.h>
28 #include <gtsam/geometry/Point3.h>
29 #include <gtsam/geometry/Pose2.h>
30 #include <gtsam/geometry/Pose3.h>
31 #include <gtsam/geometry/Cal3_S2.h>
33 
34 #include <exception>
35 
36 namespace gtsam {
37 
38 namespace utilities {
39 
40 // Create a KeyList from indices
43  for (int i = 0; i < I.size(); i++)
44  set.push_back(I[i]);
45  return set;
46 }
47 
48 // Create a KeyList from indices using symbol
49 FastList<Key> createKeyList(std::string s, const Vector& I) {
51  char c = s[0];
52  for (int i = 0; i < I.size(); i++)
53  set.push_back(Symbol(c, I[i]));
54  return set;
55 }
56 
57 // Create a KeyVector from indices
59  KeyVector set;
60  for (int i = 0; i < I.size(); i++)
61  set.push_back(I[i]);
62  return set;
63 }
64 
65 // Create a KeyVector from indices using symbol
66 KeyVector createKeyVector(std::string s, const Vector& I) {
67  KeyVector set;
68  char c = s[0];
69  for (int i = 0; i < I.size(); i++)
70  set.push_back(Symbol(c, I[i]));
71  return set;
72 }
73 
74 // Create a KeySet from indices
76  KeySet set;
77  for (int i = 0; i < I.size(); i++)
78  set.insert(I[i]);
79  return set;
80 }
81 
82 // Create a KeySet from indices using symbol
83 KeySet createKeySet(std::string s, const Vector& I) {
84  KeySet set;
85  char c = s[0];
86  for (int i = 0; i < I.size(); i++)
87  set.insert(symbol(c, I[i]));
88  return set;
89 }
90 
93  const auto points = values.extract<gtsam::Point2>();
94  // Point2 is aliased as a gtsam::Vector in the wrapper
95  const auto points2 = values.extract<gtsam::Vector>();
96 
97  Matrix result(points.size() + points2.size(), 2);
98 
99  size_t j = 0;
100  for (const auto& key_value : points) {
101  result.row(j++) = key_value.second;
102  }
103  for (const auto& key_value : points2) {
104  if (key_value.second.rows() == 2) {
105  result.row(j++) = key_value.second;
106  }
107  }
108  return result;
109 }
110 
113  const auto points = values.extract<gtsam::Point3>();
114  // Point3 is aliased as a gtsam::Vector in the wrapper
115  const auto points2 = values.extract<gtsam::Vector>();
116 
117  Matrix result(points.size() + points2.size(), 3);
118 
119  size_t j = 0;
120  for (const auto& key_value : points) {
121  result.row(j++) = key_value.second;
122  }
123  for (const auto& key_value : points2) {
124  if (key_value.second.rows() == 3) {
125  result.row(j++) = key_value.second;
126  }
127  }
128  return result;
129 }
130 
133  Values result;
134  for(const auto& key_value: values.extract<Pose2>())
135  result.insert(key_value.first, key_value.second);
136  return result;
137 }
138 
141  const auto poses = values.extract<Pose2>();
142  Matrix result(poses.size(), 3);
143  size_t j = 0;
144  for(const auto& key_value: poses)
145  result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta();
146  return result;
147 }
148 
151  Values result;
152  for(const auto& key_value: values.extract<Pose3>())
153  result.insert(key_value.first, key_value.second);
154  return result;
155 }
156 
159  const auto poses = values.extract<Pose3>();
160  Matrix result(poses.size(), 12);
161  size_t j = 0;
162  for(const auto& key_value: poses) {
163  result.row(j).segment(0, 3) << key_value.second.rotation().matrix().row(0);
164  result.row(j).segment(3, 3) << key_value.second.rotation().matrix().row(1);
165  result.row(j).segment(6, 3) << key_value.second.rotation().matrix().row(2);
166  result.row(j).tail(3) = key_value.second.translation();
167  j++;
168  }
169  return result;
170 }
171 
181  const auto vectors = values.extract<Vector>(Symbol::ChrTest(c));
182  if (vectors.size() == 0) {
183  return Matrix();
184  }
185  auto dim = vectors.begin()->second.size();
186  Matrix result(vectors.size(), dim);
187  Eigen::Index rowi = 0;
188  for (const auto& kv : vectors) {
189  if (kv.second.size() != dim) {
190  throw std::runtime_error(
191  "Tried to extract different-sized vectors into a single matrix");
192  }
193  result.row(rowi) = kv.second;
194  ++rowi;
195  }
196  return result;
197 }
198 
200 void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
203  Sampler sampler(model, seed);
204  for (const auto& key_value : values.extract<Point2>()) {
205  values.update<Point2>(key_value.first,
206  key_value.second + Point2(sampler.sample()));
207  }
208  for (const auto& key_value : values.extract<gtsam::Vector>()) {
209  if (key_value.second.rows() == 2) {
210  values.update<gtsam::Vector>(key_value.first,
211  key_value.second + Point2(sampler.sample()));
212  }
213  }
214 }
215 
217 void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
218  42u) {
220  Vector3(sigmaT, sigmaT, sigmaR));
221  Sampler sampler(model, seed);
222  for(const auto& key_value: values.extract<Pose2>()) {
223  values.update<Pose2>(key_value.first, key_value.second.retract(sampler.sample()));
224  }
225 }
226 
228 void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
231  Sampler sampler(model, seed);
232  for (const auto& key_value : values.extract<Point3>()) {
233  values.update<Point3>(key_value.first,
234  key_value.second + Point3(sampler.sample()));
235  }
236  for (const auto& key_value : values.extract<gtsam::Vector>()) {
237  if (key_value.second.rows() == 3) {
238  values.update<gtsam::Vector>(key_value.first,
239  key_value.second + Point3(sampler.sample()));
240  }
241  }
242 }
243 
254  const Vector& J, const Matrix& Z, double depth) {
255  if (Z.rows() != 2)
256  throw std::invalid_argument("insertBackProjections: Z must be 2*J");
257  if (Z.cols() != J.size())
258  throw std::invalid_argument(
259  "insertBackProjections: J and Z must have same number of entries");
260  for (int k = 0; k < Z.cols(); k++) {
261  Point2 p(Z(0, k), Z(1, k));
262  Point3 P = camera.backproject(p, depth);
263  values.insert(J(k), P);
264  }
265 }
266 
279  const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
280  const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
281  if (Z.rows() != 2)
282  throw std::invalid_argument("addMeasurements: Z must be 2*K");
283  if (Z.cols() != J.size())
284  throw std::invalid_argument(
285  "addMeasurements: J and Z must have same number of entries");
286  for (int k = 0; k < Z.cols(); k++) {
287  graph.push_back(
289  Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
290  }
291 }
292 
295  const Values& values) {
296  // first count
297  size_t K = 0, k = 0;
298  for(const NonlinearFactor::shared_ptr& f: graph)
299  if (std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
300  f))
301  ++K;
302  // now fill
303  Matrix errors(2, K);
304  for(const NonlinearFactor::shared_ptr& f: graph) {
305  std::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
306  std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
307  f);
308  if (p)
309  errors.col(k++) = p->unwhitenedError(values);
310  }
311  return errors;
312 }
313 
315 Values localToWorld(const Values& local, const Pose2& base,
316  const KeyVector user_keys = KeyVector()) {
317 
318  Values world;
319 
320  // if no keys given, get all keys from local values
321  KeyVector keys(user_keys);
322  if (keys.size()==0)
323  keys = local.keys();
324 
325  // Loop over all keys
326  for(Key key: keys) {
327  try {
328  // if value is a Pose2, compose it with base pose
329  Pose2 pose = local.at<Pose2>(key);
330  world.insert(key, base.compose(pose));
331  } catch ([[maybe_unused]] const std::exception& e1) {
332  try {
333  // if value is a Point2, transform it from base pose
334  Point2 point = local.at<Point2>(key);
335  world.insert(key, base.transformFrom(point));
336  } catch ([[maybe_unused]] const std::exception& e2) {
337  // if not Pose2 or Point2, do nothing
338  #ifndef NDEBUG
339  std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
340  #endif
341  }
342  }
343  }
344  return world;
345 }
346 
347 } // namespace utilities
348 
349 }
350 
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::utilities::allPose3s
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:150
Pose2.h
2D Pose
gtsam::utilities::allPose2s
Values allPose2s(const Values &values)
Extract all Pose3 values.
Definition: nonlinear/utilities.h:132
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
set
Definition: pytypes.h:2232
s
RealScalar s
Definition: level1_cplx_impl.h:126
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
keys
const KeyVector keys
Definition: testRegularImplicitSchurFactor.cpp:40
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::utilities::extractPose2
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
Definition: nonlinear/utilities.h:140
gtsam::utilities::createKeyVector
KeyVector createKeyVector(const Vector &I)
Definition: nonlinear/utilities.h:58
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::noiseModel::Diagonal::Sigmas
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
Definition: NoiseModel.cpp:292
Point3.h
3D Point
gtsam::FastSet< Key >
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::utilities::createKeyList
FastList< Key > createKeyList(const Vector &I)
Definition: nonlinear/utilities.h:41
body_P_sensor
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Symbol::ChrTest
static std::function< bool(Key)> ChrTest(unsigned char c)
Definition: Symbol.cpp:66
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
Point2.h
2D Point
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::utilities::localToWorld
Values localToWorld(const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector())
Convert from local to world coordinates.
Definition: nonlinear/utilities.h:315
J
JacobiRotation< float > J
Definition: Jacobi_makeJacobi.cpp:3
gtsam::utilities::extractPoint3
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
Definition: nonlinear/utilities.h:112
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
I
#define I
Definition: main.h:112
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
gtsam::utilities::perturbPose2
void perturbPose2(Values &values, double sigmaT, double sigmaR, int32_t seed=42u)
Perturb all Pose2 values using normally distributed noise.
Definition: nonlinear/utilities.h:217
gtsam::GenericProjectionFactor
Definition: ProjectionFactor.h:40
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
Symbol.h
VectorValues.h
Factor Graph Values.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::FastList
Definition: FastList.h:43
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::utilities::insertBackprojections
void insertBackprojections(Values &values, const PinholeCamera< Cal3_S2 > &camera, const Vector &J, const Matrix &Z, double depth)
Insert a number of initial point values by backprojecting.
Definition: nonlinear/utilities.h:253
gtsam::utilities::extractVectors
Matrix extractVectors(const Values &values, char c)
Definition: nonlinear/utilities.h:180
key
const gtsam::Symbol key('X', 0)
set
void set(Container &c, Position position, const Value &value)
Definition: stdlist_overload.cpp:37
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
NonlinearFactor.h
Non-linear factor base classes.
gtsam::utilities::perturbPoint2
void perturbPoint2(Values &values, double sigma, int32_t seed=42u)
Perturb all Point2 values using normally distributed noise.
Definition: nonlinear/utilities.h:200
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition: Sampler.cpp:62
gtsam::utilities::reprojectionErrors
Matrix reprojectionErrors(const NonlinearFactorGraph &graph, const Values &values)
Calculate the errors of all projection factors in a graph.
Definition: nonlinear/utilities.h:294
gtsam::utilities::extractPose3
Matrix extractPose3(const Values &values)
Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z].
Definition: nonlinear/utilities.h:158
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
gtsam::utilities::createKeySet
KeySet createKeySet(const Vector &I)
Definition: nonlinear/utilities.h:75
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:156
gtsam::utilities::extractPoint2
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
Definition: nonlinear/utilities.h:92
P
static double P[]
Definition: ellpe.c:68
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
int32_t
signed int int32_t
Definition: ms_stdint.h:82
gtsam::utilities::insertProjectionFactors
void insertProjectionFactors(NonlinearFactorGraph &graph, Key i, const Vector &J, const Matrix &Z, const SharedNoiseModel &model, const Cal3_S2::shared_ptr K, const Pose3 &body_P_sensor=Pose3())
Insert multiple projection factors for a single pose key.
Definition: nonlinear/utilities.h:278
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:625
gtsam::noiseModel::Isotropic::shared_ptr
std::shared_ptr< Isotropic > shared_ptr
Definition: NoiseModel.h:559
camera
static const CalibratedCamera camera(kDefaultPose)
Sampler.h
sampling from a NoiseModel
depth
static double depth
Definition: testSphericalCamera.cpp:64
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Z
#define Z
Definition: icosphere.cpp:21
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Sampler
Definition: Sampler.h:31
Values.h
A non-templated config holding any types of Manifold-group elements.
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Symbol
Definition: inference/Symbol.h:37
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
gtsam::utilities::perturbPoint3
void perturbPoint3(Values &values, double sigma, int32_t seed=42u)
Perturb all Point3 values using normally distributed noise.
Definition: nonlinear/utilities.h:228


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:18:23