Functions | |
Values | allPose2s (const Values &values) |
Extract all Pose3 values. More... | |
Values | allPose3s (const Values &values) |
Extract all Pose3 values. More... | |
FastList< Key > | createKeyList (const Vector &I) |
FastList< Key > | createKeyList (std::string s, const Vector &I) |
KeySet | createKeySet (const Vector &I) |
KeySet | createKeySet (std::string s, const Vector &I) |
KeyVector | createKeyVector (const Vector &I) |
KeyVector | createKeyVector (std::string s, const Vector &I) |
Matrix | extractPoint2 (const Values &values) |
Extract all Point2 values into a single matrix [x y]. More... | |
Matrix | extractPoint3 (const Values &values) |
Extract all Point3 values into a single matrix [x y z]. More... | |
Matrix | extractPose2 (const Values &values) |
Extract all Pose2 values into a single matrix [x y theta]. More... | |
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]. More... | |
Matrix | extractVectors (const Values &values, char c) |
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. More... | |
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. More... | |
Values | localToWorld (const Values &local, const Pose2 &base, const KeyVector user_keys=KeyVector()) |
Convert from local to world coordinates. More... | |
void | perturbPoint2 (Values &values, double sigma, int32_t seed=42u) |
Perturb all Point2 values using normally distributed noise. More... | |
void | perturbPoint3 (Values &values, double sigma, int32_t seed=42u) |
Perturb all Point3 values using normally distributed noise. More... | |
void | perturbPose2 (Values &values, double sigmaT, double sigmaR, int32_t seed=42u) |
Perturb all Pose2 values using normally distributed noise. More... | |
Matrix | reprojectionErrors (const NonlinearFactorGraph &graph, const Values &values) |
Calculate the errors of all projection factors in a graph. More... | |
Extract all Pose3 values.
Definition at line 132 of file nonlinear/utilities.h.
Extract all Pose3 values.
Definition at line 150 of file nonlinear/utilities.h.
Definition at line 41 of file nonlinear/utilities.h.
Definition at line 49 of file nonlinear/utilities.h.
Definition at line 75 of file nonlinear/utilities.h.
Definition at line 83 of file nonlinear/utilities.h.
Definition at line 58 of file nonlinear/utilities.h.
Definition at line 66 of file nonlinear/utilities.h.
Extract all Point2 values into a single matrix [x y].
Definition at line 92 of file nonlinear/utilities.h.
Extract all Point3 values into a single matrix [x y z].
Definition at line 112 of file nonlinear/utilities.h.
Extract all Pose2 values into a single matrix [x y theta].
Definition at line 140 of file nonlinear/utilities.h.
Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z].
Definition at line 158 of file nonlinear/utilities.h.
Extract all Vector values with a given symbol character into an mxn matrix, where m is the number of symbols that match the character and n is the dimension of the variables. If not all variables have dimension n, then a runtime error will be thrown. The order of returned values are sorted by the symbol. For example, calling extractVector(values, 'x'), where values contains 200 variables x1, x2, ..., x200 of type Vector each 5-dimensional, will return a 200x5 matrix with row i containing xi.
Definition at line 180 of file nonlinear/utilities.h.
void gtsam::utilities::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.
values | The values dict to insert the backprojections to. |
camera | The camera model. |
J | Vector of key indices. |
Z | 2*J matrix of pixel values. |
depth | Initial depth value. |
Definition at line 253 of file nonlinear/utilities.h.
void gtsam::utilities::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.
graph | The nonlinear factor graph to add the factors to. |
i | Camera key. |
J | Vector of key indices. |
Z | 2*J matrix of pixel values. |
model | Factor noise model. |
K | Calibration matrix. |
body_P_sensor | Pose of the camera sensor in the body frame. |
Definition at line 278 of file nonlinear/utilities.h.
Values gtsam::utilities::localToWorld | ( | const Values & | local, |
const Pose2 & | base, | ||
const KeyVector | user_keys = KeyVector() |
||
) |
Convert from local to world coordinates.
Definition at line 315 of file nonlinear/utilities.h.
Perturb all Point2 values using normally distributed noise.
Definition at line 200 of file nonlinear/utilities.h.
Perturb all Point3 values using normally distributed noise.
Definition at line 228 of file nonlinear/utilities.h.
void gtsam::utilities::perturbPose2 | ( | Values & | values, |
double | sigmaT, | ||
double | sigmaR, | ||
int32_t | seed = 42u |
||
) |
Perturb all Pose2 values using normally distributed noise.
Definition at line 217 of file nonlinear/utilities.h.
Matrix gtsam::utilities::reprojectionErrors | ( | const NonlinearFactorGraph & | graph, |
const Values & | values | ||
) |
Calculate the errors of all projection factors in a graph.
Definition at line 294 of file nonlinear/utilities.h.