12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

updated docs

release/4.3a0
Varun Agrawal 2021-08-20 00:55:10 -04:00
parent e056a3393c
commit d074dbedf5
1 changed files with 44 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
gtsam/nonlinear/utilities.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file matlab.h * @file utilities.h
* @brief Contains *generic* global functions designed particularly for the matlab interface * @brief Contains *generic* global functions designed particularly for the matlab interface
* @author Stephen Williams * @author Stephen Williams
*/ */
@ -164,11 +164,18 @@ Matrix extractPose3(const Values& values) {
/// Perturb all Point2 values using normally distributed noise /// Perturb all Point2 values using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, noiseModel::Isotropic::shared_ptr model =
sigma); noiseModel::Isotropic::Sigma(2, sigma);
Sampler sampler(model, seed); Sampler sampler(model, seed);
for (const auto& key_value : values.filter<Point2>()) { for (const auto& key_value : values.filter<Point2>()) {
values.update<Point2>(key_value.key, key_value.value + Point2(sampler.sample())); values.update<Point2>(key_value.key,
key_value.value + Point2(sampler.sample()));
}
for (const auto& key_value : values.filter<gtsam::Vector>()) {
if (key_value.value.rows() == 2) {
values.update<gtsam::Vector>(key_value.key,
key_value.value + Point2(sampler.sample()));
}
} }
} }
@ -185,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
/// Perturb all Point3 values using normally distributed noise /// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3, noiseModel::Isotropic::shared_ptr model =
sigma); noiseModel::Isotropic::Sigma(3, sigma);
Sampler sampler(model, seed); Sampler sampler(model, seed);
for (const auto& key_value : values.filter<Point3>()) { for (const auto& key_value : values.filter<Point3>()) {
values.update<Point3>(key_value.key, key_value.value + Point3(sampler.sample())); values.update<Point3>(key_value.key,
key_value.value + Point3(sampler.sample()));
}
for (const auto& key_value : values.filter<gtsam::Vector>()) {
if (key_value.value.rows() == 3) {
values.update<gtsam::Vector>(key_value.key,
key_value.value + Point3(sampler.sample()));
}
} }
} }
/// Insert a number of initial point values by backprojecting /**
* @brief Insert a number of initial point values by backprojecting
*
* @param values The values dict to insert the backprojections to.
* @param camera The camera model.
* @param J Vector of key indices.
* @param Z 2*J matrix of pixel values.
* @param depth Initial depth value.
*/
void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera, void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
const Vector& J, const Matrix& Z, double depth) { const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*K"); throw std::invalid_argument("insertBackProjections: Z must be 2*J");
if (Z.cols() != J.size()) if (Z.cols() != J.size())
throw std::invalid_argument( throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries"); "insertBackProjections: J and Z must have same number of entries");
@ -208,7 +230,17 @@ void insertBackprojections(Values& values, const PinholeCamera<Cal3_S2>& camera,
} }
} }
/// Insert multiple projection factors for a single pose key /**
* @brief Insert multiple projection factors for a single pose key
*
* @param graph The nonlinear factor graph to add the factors to.
* @param i Camera key.
* @param J Vector of key indices.
* @param Z 2*J matrix of pixel values.
* @param model Factor noise model.
* @param K Calibration matrix.
* @param body_P_sensor Pose of the camera sensor in the body frame.
*/
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
const Vector& J, const Matrix& Z, const SharedNoiseModel& model, const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {