41FastList<Key> createKeyList(
const Vector& I) {
43 for (
int i = 0; i < I.size(); i++)
49FastList<Key> createKeyList(std::string s,
const Vector& I) {
52 for (
int i = 0; i < I.size(); i++)
53 set.push_back(Symbol(c, I[i]));
58KeyVector createKeyVector(
const Vector& I) {
60 for (
int i = 0; i < I.size(); i++)
66KeyVector createKeyVector(std::string s,
const Vector& I) {
69 for (
int i = 0; i < I.size(); i++)
70 set.push_back(Symbol(c, I[i]));
75KeySet createKeySet(
const Vector& I) {
77 for (
int i = 0; i < I.size(); i++)
83KeySet createKeySet(std::string s,
const Vector& I) {
86 for (
int i = 0; i < I.size(); i++)
87 set.insert(
symbol(c, I[i]));
95 const auto points2 = values.
extract<gtsam::Vector>();
97 Matrix result(points.size() + points2.size(), 2);
100 for (
const auto& key_value : points) {
101 result.row(j++) = key_value.second;
103 for (
const auto& key_value : points2) {
104 if (key_value.second.rows() == 2) {
105 result.row(j++) = key_value.second;
115 const auto points2 = values.
extract<gtsam::Vector>();
117 Matrix result(points.size() + points2.size(), 3);
120 for (
const auto& key_value : points) {
121 result.row(j++) = key_value.second;
123 for (
const auto& key_value : points2) {
124 if (key_value.second.rows() == 3) {
125 result.row(j++) = key_value.second;
135 result.insert(key_value.first, key_value.second);
142 Matrix result(poses.size(), 3);
144 for(
const auto& key_value: poses)
145 result.row(j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta();
153 result.insert(key_value.first, key_value.second);
160 Matrix result(poses.size(), 12);
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();
182 if (vectors.size() == 0) {
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");
193 result.row(rowi) = kv.second;
201 noiseModel::Isotropic::shared_ptr model =
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,
220 Vector3(sigmaT, sigmaT, sigmaR));
223 values.
update<
Pose2>(key_value.first, key_value.second.retract(sampler.
sample()));
229 noiseModel::Isotropic::shared_ptr model =
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,
254 const Vector& J,
const Matrix& Z,
double depth) {
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));
280 const Cal3_S2::shared_ptr K,
const Pose3& body_P_sensor =
Pose3()) {
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++) {
289 Point2(Z(0, k), Z(1, k)), model, i,
Key(J(k)), K, body_P_sensor));
298 for(
const NonlinearFactor::shared_ptr& f: graph)
304 for(
const NonlinearFactor::shared_ptr& f: graph) {
305 boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
306 boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
309 errors.col(k++) = p->unwhitenedError(values);
330 world.
insert(key, base.compose(pose));
331 }
catch (
const std::exception& e1) {
336 }
catch (
const std::exception& e2) {
339 std::cerr <<
"Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
The most common 5DOF 3D->2D calibration.
Base class for all pinhole cameras.
sampling from a NoiseModel
Non-linear factor base classes.
Factor Graph consisting of non-linear factors.
A non-templated config holding any types of Manifold-group elements.
Reprojection of a LANDMARK to a 2D point.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
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
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
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
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
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
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
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
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
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
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
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
Vector sample() const
sample from distribution
Definition Sampler.cpp:59
Definition NonlinearFactorGraph.h:55
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
void update(Key j, const Value &val)
single element change of existing element
Definition Values.cpp:180
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
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
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
KeyVector keys() const
Returns a vector of keys in the config.
Definition Values.cpp:230
Non-linear factor for a constraint derived from a 2D measurement.
Definition ProjectionFactor.h:40
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
Definition utilities.h:92
Values localToWorld(const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector())
Convert from local to world coordinates.
Definition utilities.h:315
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
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
Definition utilities.h:112
Matrix reprojectionErrors(const NonlinearFactorGraph &graph, const Values &values)
Calculate the errors of all projection factors in a graph.
Definition utilities.h:294
void perturbPoint2(Values &values, double sigma, int32_t seed=42u)
Perturb all Point2 values using normally distributed noise.
Definition utilities.h:200
void perturbPose2(Values &values, double sigmaT, double sigmaR, int32_t seed=42u)
Perturb all Pose2 values using normally distributed noise.
Definition utilities.h:217
Values allPose2s(const Values &values)
Extract all Pose3 values.
Definition utilities.h:132
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
Values allPose3s(const Values &values)
Extract all Pose3 values.
Definition utilities.h:150
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
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
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
Definition utilities.h:140
void perturbPoint3(Values &values, double sigma, int32_t seed=42u)
Perturb all Point3 values using normally distributed noise.
Definition utilities.h:228