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

Merge pull request #851 from borglab/fix/extract-point2 

Fix utilities and add tests
release/4.3a0
Varun Agrawal 2021-08-20 10:41:51 -07:00 committed by GitHub
parent 4ea2b2ee9a a7ff8e0600
commit 52161785cf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 337 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
gtsam/geometry/tests/testUtilities.cpp Normal file
View File

@ -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
gtsam/gtsam.i
View File

@ -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,

84
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
* @author Stephen Williams
*/
@ -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;
}
@ -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()));
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()));
}
}
}
@ -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()));
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");
@ -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()) {

196
python/gtsam/tests/test_Utilities.py Normal file
View File

@ -0,0 +1,196 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Utilities unit tests.
Author: Varun Agrawal
"""
# pylint: disable=invalid-name, E1101, E0611
import unittest
import numpy as np
import gtsam
from gtsam.utils.test_case import GtsamTestCase
class TestUtilites(GtsamTestCase):
"""Test various GTSAM utilities."""
def test_createKeyList(self):
"""Test createKeyList."""
I = [0, 1, 2]
kl = gtsam.utilities.createKeyList(I)
self.assertEqual(kl.size(), 3)
kl = gtsam.utilities.createKeyList("s", I)
self.assertEqual(kl.size(), 3)
def test_createKeyVector(self):
"""Test createKeyVector."""
I = [0, 1, 2]
kl = gtsam.utilities.createKeyVector(I)
self.assertEqual(len(kl), 3)
kl = gtsam.utilities.createKeyVector("s", I)
self.assertEqual(len(kl), 3)
def test_createKeySet(self):
"""Test createKeySet."""
I = [0, 1, 2]
kl = gtsam.utilities.createKeySet(I)
self.assertEqual(kl.size(), 3)
kl = gtsam.utilities.createKeySet("s", I)
self.assertEqual(kl.size(), 3)
def test_extractPoint2(self):
"""Test extractPoint2."""
initial = gtsam.Values()
point2 = gtsam.Point2(0.0, 0.1)
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, point2)
np.testing.assert_equal(gtsam.utilities.extractPoint2(initial),
point2.reshape(-1, 2))
def test_extractPoint3(self):
"""Test extractPoint3."""
initial = gtsam.Values()
point3 = gtsam.Point3(0.0, 0.1, 0.0)
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, point3)
np.testing.assert_equal(gtsam.utilities.extractPoint3(initial),
point3.reshape(-1, 3))
def test_allPose2s(self):
"""Test allPose2s."""
initial = gtsam.Values()
initial.insert(0, gtsam.Pose2())
initial.insert(1, gtsam.Pose2(1, 1, 1))
initial.insert(2, gtsam.Point2(1, 1))
initial.insert(3, gtsam.Point3(1, 2, 3))
result = gtsam.utilities.allPose2s(initial)
self.assertEqual(result.size(), 2)
def test_extractPose2(self):
"""Test extractPose2."""
initial = gtsam.Values()
pose2 = np.asarray((0.0, 0.1, 0.1))
initial.insert(1, gtsam.Pose2(*pose2))
initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0))
np.testing.assert_allclose(gtsam.utilities.extractPose2(initial),
pose2.reshape(-1, 3))
def test_allPose3s(self):
"""Test allPose3s."""
initial = gtsam.Values()
initial.insert(0, gtsam.Pose3())
initial.insert(2, gtsam.Point2(1, 1))
initial.insert(1, gtsam.Pose3())
initial.insert(3, gtsam.Point3(1, 2, 3))
result = gtsam.utilities.allPose3s(initial)
self.assertEqual(result.size(), 2)
def test_extractPose3(self):
"""Test extractPose3."""
initial = gtsam.Values()
pose3 = np.asarray([1., 0., 0., 0., 1., 0., 0., 0., 1., 0., 0., 0.])
initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1))
initial.insert(2, gtsam.Pose3())
np.testing.assert_allclose(gtsam.utilities.extractPose3(initial),
pose3.reshape(-1, 12))
def test_perturbPoint2(self):
"""Test perturbPoint2."""
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
values.insert(1, gtsam.Point2(1, 1))
gtsam.utilities.perturbPoint2(values, 1.0)
self.assertTrue(
not np.allclose(values.atPoint2(1), gtsam.Point2(1, 1)))
def test_perturbPose2(self):
"""Test perturbPose2."""
values = gtsam.Values()
values.insert(0, gtsam.Pose2())
values.insert(1, gtsam.Point2(1, 1))
gtsam.utilities.perturbPose2(values, 1, 1)
self.assertTrue(values.atPose2(0) != gtsam.Pose2())
def test_perturbPoint3(self):
"""Test perturbPoint3."""
values = gtsam.Values()
point3 = gtsam.Point3(0, 0, 0)
values.insert(0, gtsam.Pose2())
values.insert(1, point3)
gtsam.utilities.perturbPoint3(values, 1)
self.assertTrue(not np.allclose(values.atPoint3(1), point3))
def test_insertBackprojections(self):
"""Test insertBackprojections."""
values = gtsam.Values()
cam = gtsam.PinholeCameraCal3_S2()
gtsam.utilities.insertBackprojections(
values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]),
10)
np.testing.assert_allclose(values.atPoint3(0),
gtsam.Point3(200, 200, 10))
def test_insertProjectionFactors(self):
"""Test insertProjectionFactors."""
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2())
self.assertEqual(graph.size(), 2)
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(),
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
self.assertEqual(graph.size(), 2)
def test_reprojectionErrors(self):
"""Test reprojectionErrors."""
pixels = np.asarray([[20, 30], [20, 30]])
I = [1, 2]
K = gtsam.Cal3_S2()
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K)
gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
errors = gtsam.utilities.reprojectionErrors(graph, values)
np.testing.assert_allclose(errors, np.zeros((2, 2)))
def test_localToWorld(self):
"""Test localToWorld."""
local = gtsam.Values()
local.insert(0, gtsam.Point2(10, 10))
local.insert(1, gtsam.Pose2(6, 11, 0.0))
base = gtsam.Pose2(1, 0, 0)
world = gtsam.utilities.localToWorld(local, base)
expected_point2 = gtsam.Point2(11, 10)
expected_pose2 = gtsam.Pose2(7, 11, 0)
np.testing.assert_allclose(world.atPoint2(0), expected_point2)
np.testing.assert_allclose(
world.atPose2(1).matrix(), expected_pose2.matrix())
user_keys = [1]
world = gtsam.utilities.localToWorld(local, base, user_keys)
np.testing.assert_allclose(
world.atPose2(1).matrix(), expected_pose2.matrix())
# Raise error since 0 is not in user_keys
self.assertRaises(RuntimeError, world.atPoint2, 0)
if __name__ == "__main__":
unittest.main()