Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin' into feature/cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@varunagrawal
varunagrawal committed Aug 22, 2021
2 parents 4c9ce4c + 4739f70 commit 713df3c
Show file tree
Hide file tree
Showing 5 changed files with 368 additions and 42 deletions.
64 changes: 64 additions & 0 deletions gtsam/geometry/tests/testUtilities.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */

/*
* @file testUtilities.cpp
* @date Aug 19, 2021
* @author Varun Agrawal
* @brief Tests for the utilities.
*/

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/utilities.h>

using namespace gtsam;
using gtsam::symbol_shorthand::L;
using gtsam::symbol_shorthand::R;
using gtsam::symbol_shorthand::X;

/* ************************************************************************* */
TEST(Utilities, ExtractPoint2) {
Point2 p0(0, 0), p1(1, 0);
Values values;
values.insert<Point2>(L(0), p0);
values.insert<Point2>(L(1), p1);
values.insert<Rot3>(R(0), Rot3());
values.insert<Pose3>(X(0), Pose3());

Matrix all_points = utilities::extractPoint2(values);
EXPECT_LONGS_EQUAL(2, all_points.rows());
}

/* ************************************************************************* */
TEST(Utilities, ExtractPoint3) {
Point3 p0(0, 0, 0), p1(1, 0, 0);
Values values;
values.insert<Point3>(L(0), p0);
values.insert<Point3>(L(1), p1);
values.insert<Rot3>(R(0), Rot3());
values.insert<Pose3>(X(0), Pose3());

Matrix all_points = utilities::extractPoint3(values);
EXPECT_LONGS_EQUAL(2, all_points.rows());
}

/* ************************************************************************* */
int main() {
srand(time(nullptr));
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
19 changes: 7 additions & 12 deletions gtsam/gtsam.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,22 +165,17 @@ gtsam::Values allPose2s(gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPoint2(gtsam::Values& values, double sigma, int seed = 42u);
void perturbPose2(gtsam::Values& values, double sigmaT, double sigmaR,
int seed);
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
int seed = 42u);
void perturbPoint3(gtsam::Values& values, double sigma, int seed = 42u);
void insertBackprojections(gtsam::Values& values,
const gtsam::PinholeCamera<gtsam::Cal3_S2>& c,
Vector J, Matrix Z, double depth);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
Vector J, Matrix Z,
const gtsam::noiseModel::Base* model,
const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i,
Vector J, Matrix Z,
const gtsam::noiseModel::Base* model,
const gtsam::Cal3_S2* K,
const gtsam::Pose3& body_P_sensor);
void insertProjectionFactors(
gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z,
const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K,
const gtsam::Pose3& body_P_sensor = gtsam::Pose3());
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local,
Expand Down
88 changes: 70 additions & 18 deletions gtsam/nonlinear/utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */

/**
* @file matlab.h
* @file utilities.h
* @brief Contains *generic* global functions designed particularly for the matlab interface
* @author Stephen Williams
*/
Expand Down Expand Up @@ -89,21 +89,41 @@ KeySet createKeySet(std::string s, const Vector& I) {

/// Extract all Point2 values into a single matrix [x y]
Matrix extractPoint2(const Values& values) {
Values::ConstFiltered<gtsam::Point2> points = values.filter<gtsam::Point2>();
// Point2 is aliased as a gtsam::Vector in the wrapper
Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();

Matrix result(points.size() + points2.size(), 2);

size_t j = 0;
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
for(const auto& key_value: points)
for (const auto& key_value : points) {
result.row(j++) = key_value.value;
}
for (const auto& key_value : points2) {
if (key_value.value.rows() == 2) {
result.row(j++) = key_value.value;
}
}
return result;
}

/// Extract all Point3 values into a single matrix [x y z]
Matrix extractPoint3(const Values& values) {
Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(), 3);
Values::ConstFiltered<gtsam::Point3> points = values.filter<gtsam::Point3>();
// Point3 is aliased as a gtsam::Vector in the wrapper
Values::ConstFiltered<gtsam::Vector> points2 = values.filter<gtsam::Vector>();

Matrix result(points.size() + points2.size(), 3);

size_t j = 0;
for(const auto& key_value: points)
for (const auto& key_value : points) {
result.row(j++) = key_value.value;
}
for (const auto& key_value : points2) {
if (key_value.value.rows() == 3) {
result.row(j++) = key_value.value;
}
}
return result;
}

Expand Down Expand Up @@ -144,11 +164,18 @@ Matrix extractPose3(const Values& values) {

/// Perturb all Point2 values using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
sigma);
noiseModel::Isotropic::shared_ptr model =
noiseModel::Isotropic::Sigma(2, sigma);
Sampler sampler(model, seed);
for(const auto& key_value: values.filter<Point2>()) {
values.update<Point2>(key_value.key, key_value.value + Point2(sampler.sample()));
for (const auto& key_value : values.filter<Point2>()) {
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()));
}
}
}

Expand All @@ -165,19 +192,34 @@ void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =

/// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
sigma);
noiseModel::Isotropic::shared_ptr model =
noiseModel::Isotropic::Sigma(3, sigma);
Sampler sampler(model, seed);
for(const auto& key_value: values.filter<Point3>()) {
values.update<Point3>(key_value.key, key_value.value + Point3(sampler.sample()));
for (const auto& key_value : values.filter<Point3>()) {
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,
const Vector& J, const Matrix& Z, double depth) {
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())
throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
Expand All @@ -188,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,
const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
Expand Down
Loading

0 comments on commit 713df3c

Please sign in to comment.