Go to the documentation of this file.
43 for (
int i = 0;
i <
I.size();
i++)
52 for (
int i = 0;
i <
I.size();
i++)
60 for (
int i = 0;
i <
I.size();
i++)
69 for (
int i = 0;
i <
I.size();
i++)
77 for (
int i = 0;
i <
I.size();
i++)
86 for (
int i = 0;
i <
I.size();
i++)
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;
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;
134 for(
const auto& key_value:
values.extract<
Pose2>())
135 result.insert(key_value.first, key_value.second);
144 for(
const auto& key_value: poses)
145 result.row(
j++) << key_value.second.x(), key_value.second.y(), key_value.second.theta();
152 for(
const auto& key_value:
values.extract<
Pose3>())
153 result.insert(key_value.first, key_value.second);
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();
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;
204 for (
const auto& key_value :
values.extract<
Point2>()) {
209 if (key_value.second.rows() == 2) {
220 Vector3(sigmaT, sigmaT, sigmaR));
222 for(
const auto& key_value:
values.extract<
Pose2>()) {
223 values.update<
Pose2>(key_value.first, key_value.second.retract(sampler.
sample()));
232 for (
const auto& key_value :
values.extract<
Point3>()) {
237 if (key_value.second.rows() == 3) {
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++) {
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++) {
305 std::shared_ptr<const GenericProjectionFactor<Pose3, Point3> >
p =
306 std::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
309 errors.col(k++) =
p->unwhitenedError(
values);
331 }
catch ([[maybe_unused]]
const std::exception& e1) {
336 }
catch ([[maybe_unused]]
const std::exception& e2) {
339 std::cerr <<
"Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
std::shared_ptr< This > shared_ptr
Values allPose3s(const Values &values)
Extract all Pose3 values.
Values allPose2s(const Values &values)
Extract all Pose3 values.
Reprojection of a LANDMARK to a 2D point.
Annotation indicating that a class derives from another given type.
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
The most common 5DOF 3D->2D calibration.
Matrix extractPose2(const Values &values)
Extract all Pose2 values into a single matrix [x y theta].
KeyVector createKeyVector(const Vector &I)
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
FastList< Key > createKeyList(const Vector &I)
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
static std::function< bool(Key)> ChrTest(unsigned char c)
static const double sigma
Values localToWorld(const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector())
Convert from local to world coordinates.
JacobiRotation< float > J
Matrix extractPoint3(const Values &values)
Extract all Point3 values into a single matrix [x y z].
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
void perturbPose2(Values &values, double sigmaT, double sigmaR, int32_t seed=42u)
Perturb all Pose2 values using normally distributed noise.
Base class for all pinhole cameras.
Key symbol(unsigned char c, std::uint64_t j)
noiseModel::Base::shared_ptr SharedNoiseModel
std::shared_ptr< Cal3_S2 > shared_ptr
noiseModel::Diagonal::shared_ptr model
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.
Matrix extractVectors(const Values &values, char c)
const gtsam::Symbol key('X', 0)
void set(Container &c, Position position, const Value &value)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
void perturbPoint2(Values &values, double sigma, int32_t seed=42u)
Perturb all Point2 values using normally distributed noise.
Vector sample() const
sample from distribution
Matrix reprojectionErrors(const NonlinearFactorGraph &graph, const Values &values)
Calculate the errors of all projection factors in a graph.
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].
Factor Graph consisting of non-linear factors.
KeySet createKeySet(const Vector &I)
std::shared_ptr< Diagonal > shared_ptr
void insert(Key j, const Value &val)
Matrix extractPoint2(const Values &values)
Extract all Point2 values into a single matrix [x y].
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.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
std::shared_ptr< Isotropic > shared_ptr
static const CalibratedCamera camera(kDefaultPose)
sampling from a NoiseModel
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
A non-templated config holding any types of Manifold-group elements.
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
void perturbPoint3(Values &values, double sigma, int32_t seed=42u)
Perturb all Point3 values using normally distributed noise.
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:40