Loading...
Searching...
No Matches
utilities.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <exception>
namespace gtsam {
namespace utilities {
// Create a KeyList from indices
FastList<Key> createKeyList(const Vector& I) {
FastList<Key> set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyList from indices using symbol
FastList<Key> createKeyList(std::string s, const Vector& I) {
FastList<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeyVector from indices
KeyVector createKeyVector(const Vector& I) {
KeyVector set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyVector from indices using symbol
KeyVector createKeyVector(std::string s, const Vector& I) {
KeyVector set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeySet from indices
KeySet createKeySet(const Vector& I) {
KeySet set;
for (int i = 0; i < I.size(); i++)
set.insert(I[i]);
return set;
}
// Create a KeySet from indices using symbol
KeySet createKeySet(std::string s, const Vector& I) {
KeySet set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));
return set;
}
Matrix extractPoint2(const Values& values) {
const auto points = values.extract<gtsam::Point2>();
// Point2 is aliased as a gtsam::Vector in the wrapper
const auto points2 = values.extract<gtsam::Vector>();
Matrix result(points.size() + points2.size(), 2);
size_t j = 0;
for (const auto& key_value : points) {
result.row(j++) = key_value.second;
}
for (const auto& key_value : points2) {
if (key_value.second.rows() == 2) {
result.row(j++) = key_value.second;
}
}
return result;
}
Matrix extractPoint3(const Values& values) {
const auto points = values.extract<gtsam::Point3>();
// Point3 is aliased as a gtsam::Vector in the wrapper
const auto points2 = values.extract<gtsam::Vector>();
Matrix result(points.size() + points2.size(), 3);
size_t j = 0;
for (const auto& key_value : points) {
result.row(j++) = key_value.second;
}
for (const auto& key_value : points2) {
if (key_value.second.rows() == 3) {
result.row(j++) = key_value.second;
}
}
return result;
}
Values allPose2s(const Values& values) {
Values result;
for(const auto& key_value: values.extract<Pose2>())
result.insert(key_value.first, key_value.second);
return result;
}
Matrix extractPose2(const Values& values) {
const auto poses = values.extract<Pose2>();
Matrix result(poses.size(), 3);
size_t j = 0;
for(const auto& key_value: poses)
result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta();
return result;
}
Values allPose3s(const Values& values) {
Values result;
for(const auto& key_value: values.extract<Pose3>())
result.insert(key_value.first, key_value.second);
return result;
}
Matrix extractPose3(const Values& values) {
const auto poses = values.extract<Pose3>();
Matrix result(poses.size(), 12);
size_t j = 0;
for(const auto& key_value: poses) {
result.row(j).segment(0, 3) << key_value.second.rotation().matrix().row(0);
result.row(j).segment(3, 3) << key_value.second.rotation().matrix().row(1);
result.row(j).segment(6, 3) << key_value.second.rotation().matrix().row(2);
result.row(j).tail(3) = key_value.second.translation();
j++;
}
return result;
}
Matrix extractVectors(const Values& values, char c) {
const auto vectors = values.extract<Vector>(Symbol::ChrTest(c));
if (vectors.size() == 0) {
return Matrix();
}
auto dim = vectors.begin()->second.size();
Matrix result(vectors.size(), dim);
Eigen::Index rowi = 0;
for (const auto& kv : vectors) {
if (kv.second.size() != dim) {
throw std::runtime_error(
"Tried to extract different-sized vectors into a single matrix");
}
result.row(rowi) = kv.second;
++rowi;
}
return result;
}
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model =
noiseModel::Isotropic::Sigma(2, sigma);
Sampler sampler(model, seed);
for (const auto& key_value : values.extract<Point2>()) {
values.update<Point2>(key_value.first,
key_value.second + Point2(sampler.sample()));
}
for (const auto& key_value : values.extract<gtsam::Vector>()) {
if (key_value.second.rows() == 2) {
values.update<gtsam::Vector>(key_value.first,
key_value.second + Point2(sampler.sample()));
}
}
}
void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
42u) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed);
for(const auto& key_value: values.extract<Pose2>()) {
values.update<Pose2>(key_value.first, key_value.second.retract(sampler.sample()));
}
}
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model =
noiseModel::Isotropic::Sigma(3, sigma);
Sampler sampler(model, seed);
for (const auto& key_value : values.extract<Point3>()) {
values.update<Point3>(key_value.first,
key_value.second + Point3(sampler.sample()));
}
for (const auto& key_value : values.extract<gtsam::Vector>()) {
if (key_value.second.rows() == 3) {
values.update<gtsam::Vector>(key_value.first,
key_value.second + Point3(sampler.sample()));
}
}
}
void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*J");
if (Z.cols() != J.size())
throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
Point2 p(Z(0, k), Z(1, k));
Point3 P = camera.backproject(p, depth);
values.insert(J(k), P);
}
}
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()) {
if (Z.rows() != 2)
throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size())
throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> >(
Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
}
}
Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
const Values& values) {
// first count
size_t K = 0, k = 0;
for(const NonlinearFactor::shared_ptr& f: graph)
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f))
++K;
// now fill
Matrix errors(2, K);
for(const NonlinearFactor::shared_ptr& f: graph) {
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f);
if (p)
errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
Values localToWorld(const Values& local, const Pose2& base,
const KeyVector user_keys = KeyVector()) {
Values world;
// if no keys given, get all keys from local values
KeyVector keys(user_keys);
if (keys.size()==0)
keys = local.keys();
// Loop over all keys
for(Key key: keys) {
try {
// if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key);
world.insert(key, base.compose(pose));
} catch (const std::exception& e1) {
try {
// if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key);
world.insert(key, base.transformFrom(point));
} catch (const std::exception& e2) {
// if not Pose2 or Point2, do nothing
#ifndef NDEBUG
std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
#endif
}
}
}
return world;
}
} // namespace utilities
}
Point2.h
2D Point
Point3.h
3D Point
Pose2.h
2D Pose
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
PinholeCamera.h
Base class for all pinhole cameras.
Pose3.h
3D Pose
VectorValues.h
Factor Graph Values.
Sampler.h
sampling from a NoiseModel
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
NonlinearFactor.h
Non-linear factor base classes.
Values.h
A non-templated config holding any types of Manifold-group elements.
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
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:135
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::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:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholeBaseK::backproject
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK > Dresult_dcal=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition PinholePose.h:139
gtsam::Pose2
A 2D pose (Point2,Rot2).
Definition Pose2.h:36
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition Pose2.cpp:226
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
gtsam::Symbol::ChrTest
static std::function< bool(Key)> ChrTest(unsigned char c)
Return a filter function that returns true when evaluated on a Key whose character (when converted to...
Definition Symbol.cpp:65
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:273
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:597
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition Sampler.cpp:59
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::update
void update(Key j, const Value &val)
single element change of existing element
Definition Values.cpp:180
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
gtsam::Values::keys
KeyVector keys() const
Returns a vector of keys in the config.
Definition Values.cpp:230
gtsam::GenericProjectionFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition ProjectionFactor.h:40
gtsam::utilities::extractPoint2
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
Definition utilities.h:92
gtsam::utilities::localToWorld
Values localToWorld(const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector())
Convert from local to world coordinates.
Definition utilities.h:315
gtsam::utilities::extractVectors
Matrix extractVectors(const Values &values, char c)
Extract all Vector values with a given symbol character into an mxn matrix, where m is the number of ...
Definition utilities.h:180
gtsam::utilities::extractPoint3
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
Definition utilities.h:112
gtsam::utilities::reprojectionErrors
Matrix reprojectionErrors(const NonlinearFactorGraph &graph, const Values &values)
Calculate the errors of all projection factors in a graph.
Definition utilities.h:294
gtsam::utilities::perturbPoint2
void perturbPoint2(Values &values, double sigma, int32_t seed=42u)
Perturb all Point2 values using normally distributed noise.
Definition utilities.h:200
gtsam::utilities::perturbPose2
void perturbPose2(Values &values, double sigmaT, double sigmaR, int32_t seed=42u)
Perturb all Pose2 values using normally distributed noise.
Definition utilities.h:217
gtsam::utilities::allPose2s
Values allPose2s(const Values &values)
Extract all Pose3 values.
Definition utilities.h:132
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 utilities.h:278
gtsam::utilities::allPose3s
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition utilities.h:150
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 utilities.h:253
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 utilities.h:158
gtsam::utilities::extractPose2
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
Definition utilities.h:140
gtsam::utilities::perturbPoint3
void perturbPoint3(Values &values, double sigma, int32_t seed=42u)
Perturb all Point3 values using normally distributed noise.
Definition utilities.h:228
Symbol.h