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

fix extractPoint2/3 and added C++ tests

release/4.3a0
Varun Agrawal 2021-08-19 23:32:34 -04:00
parent 4ea2b2ee9a
commit 910aceae74
2 changed files with 90 additions and 6 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);
}
/* ************************************************************************* */

32
gtsam/nonlinear/utilities.h
View File

@ -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;
}