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

Merge pull request #1107 from borglab/fix/91_single_test_exe

release/4.3a0
Varun Agrawal 2022-02-17 00:12:25 -05:00 committed by GitHub
parent 07a7802469 59fbdf9cff
commit 2b78b96670
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
38 changed files with 699 additions and 576 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.github/scripts/unix.sh vendored
View File

@ -71,6 +71,7 @@ function configure()
-DGTSAM_USE_SYSTEM_EIGEN=${GTSAM_USE_SYSTEM_EIGEN:-OFF} \
-DGTSAM_USE_SYSTEM_METIS=${GTSAM_USE_SYSTEM_METIS:-OFF} \
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
-DGTSAM_SINGLE_TEST_EXE=ON \
-DBOOST_ROOT=$BOOST_ROOT \
-DBoost_NO_SYSTEM_PATHS=ON \
-DBoost_ARCHITECTURE=-x64

2
examples/Data/randomGrid3D.xml
View File

@ -7,7 +7,7 @@
<count>32</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>

2
examples/Data/toy3D.xml
View File

@ -7,7 +7,7 @@
<count>2</count>
<item_version>1</item_version>
<item class_id="3" tracking_level="0" version="1">
<px class_id="4" class_name="JacobianFactor" tracking_level="1" version="0" object_id="_0">
<px class_id="4" class_name="gtsam::JacobianFactor" tracking_level="1" version="0" object_id="_0">
<Base class_id="5" tracking_level="0" version="0">
<Base class_id="6" tracking_level="0" version="0">
<keys_>

1
gtsam/base/FastSet.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <boost/version.hpp>
#if BOOST_VERSION >= 107400
#include <boost/serialization/library_version_type.hpp>
#endif

2
gtsam/base/serializationTestHelpers.h
View File

@ -42,7 +42,7 @@ T create() {
}
// Creates or empties a folder in the build folder and returns the relative path
boost::filesystem::path resetFilesystem(
inline boost::filesystem::path resetFilesystem(
boost::filesystem::path folder = "actual") {
boost::filesystem::remove_all(folder);
boost::filesystem::create_directory(folder);

3
gtsam/basis/tests/testChebyshev.cpp
View File

@ -25,9 +25,10 @@
using namespace std;
using namespace gtsam;
namespace {
auto model = noiseModel::Unit::Create(1);
const size_t N = 3;
} // namespace
//******************************************************************************
TEST(Chebyshev, Chebyshev1) {

2
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -27,9 +27,11 @@ using namespace std;
using namespace gtsam;
using namespace boost::placeholders;
namespace {
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
const size_t N = 32;
} // namespace
//******************************************************************************
TEST(Chebyshev2, Point) {

22
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -17,17 +17,19 @@
* @date Jan 30, 2012
*/
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>
// #define DT_DEBUG_MEMORY
// #define DT_NO_PRUNING
#define DISABLE_DOT
#include <gtsam/discrete/DecisionTree-inl.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/Signature.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
using namespace std;
using namespace gtsam;
@ -148,9 +150,9 @@ TEST(DecisionTree, example) {
DOT(notb);
// Check supplying empty trees yields an exception
CHECK_EXCEPTION(apply(empty, &Ring::id), std::runtime_error);
CHECK_EXCEPTION(apply(empty, a, &Ring::mul), std::runtime_error);
CHECK_EXCEPTION(apply(a, empty, &Ring::mul), std::runtime_error);
CHECK_EXCEPTION(gtsam::apply(empty, &Ring::id), std::runtime_error);
CHECK_EXCEPTION(gtsam::apply(empty, a, &Ring::mul), std::runtime_error);
CHECK_EXCEPTION(gtsam::apply(a, empty, &Ring::mul), std::runtime_error);
// apply, two nodes, in natural order
DT anotb = apply(a, notb, &Ring::mul);

2
gtsam/discrete/tests/testDecisionTreeFactor.cpp
View File

@ -107,7 +107,7 @@ TEST(DecisionTreeFactor, enumerate) {
}
/* ************************************************************************* */
TEST(DiscreteFactorGraph, DotWithNames) {
TEST(DecisionTreeFactor, DotWithNames) {
DiscreteKey A(12, 3), B(5, 2);
DecisionTreeFactor f(A & B, "1 2 3 4 5 6");
auto formatter = [](Key key) { return key == 12 ? "A" : "B"; };

8
gtsam/geometry/PinholeCamera.h
View File

@ -230,13 +230,15 @@ public:
Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
// We just call 3-derivative version in Base
if (Dcamera){
Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal;
Point2 pi = Base::project(pw, Dcamera ? &Dpose : 0, Dpoint,
Dcamera ? &Dcal : 0);
if (Dcamera)
const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
*Dcamera << Dpose, Dcal;
return pi;
} else {
return Base::project(pw, boost::none, Dpoint, boost::none);
}
}
/// project a 3D point from world coordinates into the image

2
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -160,7 +160,7 @@ TEST(Cal3Bundler, retract) {
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
TEST(Cal3Bundler, Print) {
Cal3Bundler cal(1, 2, 3, 4, 5);
std::stringstream os;
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()

11
gtsam/geometry/tests/testPose2.cpp
View File

@ -796,13 +796,14 @@ TEST(Pose2, align_4) {
}
//******************************************************************************
namespace {
Pose2 id;
Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
} // namespace
//******************************************************************************
TEST(Pose2, Invariants) {
Pose2 id;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
@ -812,24 +813,18 @@ TEST(Pose2 , Invariants) {
EXPECT(check_manifold_invariants(id, T1));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T1));
}
//******************************************************************************
TEST(Pose2, LieGroupDerivatives) {
Pose2 id;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
}
//******************************************************************************
TEST(Pose2, ChartDerivatives) {
Pose2 id;
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);
CHECK_CHART_DERIVATIVES(T2, id);

14
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -80,12 +80,6 @@ TEST(Quaternion , Compose) {
EXPECT(traits<Q>::Equals(expected, actual));
}
//******************************************************************************
Vector3 Q_z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, Q_z_axis));
Q R1(Eigen::AngleAxisd(1, Q_z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
//******************************************************************************
TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
@ -107,6 +101,14 @@ TEST(Quaternion , Inverse) {
EXPECT(traits<Q>::Equals(expected, actual));
}
//******************************************************************************
namespace {
Vector3 Q_z_axis(0, 0, 1);
Q id(Eigen::AngleAxisd(0, Q_z_axis));
Q R1(Eigen::AngleAxisd(1, Q_z_axis));
Q R2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
} // namespace
//******************************************************************************
TEST(Quaternion, Invariants) {
EXPECT(check_group_invariants(id, id));

11
gtsam/geometry/tests/testRot2.cpp
View File

@ -156,13 +156,14 @@ TEST( Rot2, relativeBearing )
}
//******************************************************************************
namespace {
Rot2 id;
Rot2 T1(0.1);
Rot2 T2(0.2);
} // namespace
//******************************************************************************
TEST(Rot2, Invariants) {
Rot2 id;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
@ -172,24 +173,18 @@ TEST(Rot2 , Invariants) {
EXPECT(check_manifold_invariants(id, T1));
EXPECT(check_manifold_invariants(T2, id));
EXPECT(check_manifold_invariants(T2, T1));
}
//******************************************************************************
TEST(Rot2, LieGroupDerivatives) {
Rot2 id;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
CHECK_LIE_GROUP_DERIVATIVES(T2, T1);
}
//******************************************************************************
TEST(Rot2, ChartDerivatives) {
Rot2 id;
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);
CHECK_CHART_DERIVATIVES(T2, id);

8
gtsam/geometry/tests/testRot3.cpp
View File

@ -640,13 +640,14 @@ TEST( Rot3, slerp)
}
//******************************************************************************
namespace {
Rot3 id;
Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2));
} // namespace
//******************************************************************************
TEST(Rot3, Invariants) {
Rot3 id;
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, T1));
EXPECT(check_group_invariants(T2, id));
@ -662,8 +663,6 @@ TEST(Rot3 , Invariants) {
//******************************************************************************
TEST(Rot3, LieGroupDerivatives) {
Rot3 id;
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, T2);
CHECK_LIE_GROUP_DERIVATIVES(T2, id);
@ -673,7 +672,6 @@ TEST(Rot3 , LieGroupDerivatives) {
//******************************************************************************
TEST(Rot3, ChartDerivatives) {
Rot3 id;
if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) {
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, T2);

6
gtsam/geometry/tests/testSO3.cpp
View File

@ -67,10 +67,12 @@ TEST(SO3, ClosestTo) {
}
//******************************************************************************
namespace {
SO3 id;
Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
} // namespace
/* ************************************************************************* */
TEST(SO3, ChordalMean) {
@ -79,16 +81,16 @@ TEST(SO3, ChordalMean) {
}
//******************************************************************************
TEST(SO3, Hat) {
// Check that Hat specialization is equal to dynamic version
TEST(SO3, Hat) {
EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
TEST(SO3, Vee) {
// Check that Hat specialization is equal to dynamic version
TEST(SO3, Vee) {
auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));

25
gtsam/geometry/tests/testSO4.cpp
View File

@ -48,6 +48,14 @@ TEST(SO4, Concept) {
}
//******************************************************************************
TEST(SO4, Random) {
std::mt19937 rng(42);
auto Q = SO4::Random(rng);
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
}
//******************************************************************************
namespace {
SO4 id;
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
@ -55,13 +63,8 @@ Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
SO4 Q2 = SO4::Expmap(v2);
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
SO4 Q3 = SO4::Expmap(v3);
} // namespace
//******************************************************************************
TEST(SO4, Random) {
std::mt19937 rng(42);
auto Q = SO4::Random(rng);
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
}
//******************************************************************************
TEST(SO4, Expmap) {
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
@ -84,16 +87,16 @@ TEST(SO4, Expmap) {
}
//******************************************************************************
TEST(SO4, Hat) {
// Check that Hat specialization is equal to dynamic version
TEST(SO4, Hat) {
EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
TEST(SO4, Vee) {
// Check that Hat specialization is equal to dynamic version
TEST(SO4, Vee) {
auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
@ -116,8 +119,8 @@ TEST(SO4, Retract) {
}
//******************************************************************************
TEST(SO4, Local) {
// Check that Cayley is identical to dynamic version
TEST(SO4, Local) {
EXPECT(
assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
EXPECT(
@ -166,9 +169,7 @@ TEST(SO4, vec) {
Matrix actualH;
const Vector16 actual = Q2.vec(actualH);
EXPECT(assert_equal(expected, actual));
std::function<Vector16(const SO4&)> f = [](const SO4& Q) {
return Q.vec();
};
std::function<Vector16(const SO4&)> f = [](const SO4& Q) { return Q.vec(); };
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
EXPECT(assert_equal(numericalH, actualH));
}

2
gtsam/navigation/tests/imuFactorTesting.h
View File

@ -28,6 +28,7 @@ using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
namespace {
static const Vector3 kZero = Z_3x1;
typedef imuBias::ConstantBias Bias;
static const Bias kZeroBiasHat, kZeroBias;
@ -43,6 +44,7 @@ static const Vector3 kGravityAlongNavZDown(0, 0, kGravity);
auto radians = [](double t) { return t * M_PI / 180; };
static const double kGyroSigma = radians(0.5) / 60; // 0.5 degree ARW
static const double kAccelSigma = 0.1 / 60; // 10 cm VRW
}
namespace testing {

29
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -19,8 +19,6 @@
#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
@ -63,22 +61,6 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Rot3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, CopyAndMove) {
Unit3 nDown(0, 0, -1);
@ -129,17 +111,6 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Pose3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, CopyAndMove) {
Unit3 nDown(0, 0, -1);

3
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -31,7 +31,7 @@ using namespace std;
using namespace gtsam;
using namespace GeographicLib;
// *************************************************************************
namespace {
// Convert from Mag to ENU
// ENU Origin is where the plane was in hold next to runway
// const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
@ -55,6 +55,7 @@ double s(scale * nM.norm());
Unit3 dir(nM);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
} // namespace
using boost::none;

7
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -20,7 +20,7 @@
using namespace std::placeholders;
using namespace gtsam;
// *****************************************************************************
namespace {
// Magnetic field in the nav frame (NED), with units of nT.
Point3 nM(22653.29982, -1956.83010, 44202.47862);
@ -51,8 +51,9 @@ SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
// Make up a rotation and offset of the sensor in the body frame.
Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
// *****************************************************************************
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)),
Point3(-0.1, 0.2, 0.3));
} // namespace
// *****************************************************************************
TEST(MagPoseFactor, Constructors) {

49
gtsam/navigation/tests/testImuFactorSerialization.cpp → gtsam/navigation/tests/testSerializationNavigation.cpp
View File

@ -10,17 +10,19 @@
* -------------------------------------------------------------------------- */
/**
* @file testImuFactor.cpp
* @brief Unit test for ImuFactor
* @file testSerializationNavigation.cpp
* @brief serialization tests for navigation
* @author Luca Carlone
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
* @date February 2022
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
@ -30,17 +32,13 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
"gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
"gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
"gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
template <typename P>
P getPreintegratedMeasurements() {
@ -69,6 +67,7 @@ P getPreintegratedMeasurements() {
return pim;
}
/* ************************************************************************* */
TEST(ImuFactor, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
@ -83,6 +82,7 @@ TEST(ImuFactor, serialization) {
EXPECT(equalsBinary<ImuFactor>(factor));
}
/* ************************************************************************* */
TEST(ImuFactor2, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
@ -93,6 +93,7 @@ TEST(ImuFactor2, serialization) {
EXPECT(equalsBinary<ImuFactor2>(factor));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, Serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
@ -107,6 +108,28 @@ TEST(CombinedImuFactor, Serialization) {
EXPECT(equalsBinary<CombinedImuFactor>(factor));
}
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Rot3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Pose3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

33
gtsam/nonlinear/factorTesting.h
View File

@ -21,6 +21,8 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <string>
#include <vector>
namespace gtsam {
@ -34,13 +36,14 @@ namespace gtsam {
* This is fixable but expensive, and does not matter in practice as most factors will sit near
* zero errors anyway. However, it means that below will only be exact for the correct measurement.
*/
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) {
inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values,
double delta = 1e-5) {
// We will fill a vector of key/Jacobians pairs (a map would sort)
std::vector<std::pair<Key, Matrix> > jacobians;
// Get size
const Eigen::VectorXd e = factor.whitenedError(values);
const Vector e = factor.whitenedError(values);
const size_t rows = e.size();
// Loop over all variables
@ -51,18 +54,18 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
Vector dx = Vector::Zero(cols);
dx(col) = delta;
dX[key] = dx;
Values eval_values = values.retract(dX);
const Eigen::VectorXd left = factor.whitenedError(eval_values);
const Vector left = factor.whitenedError(eval_values);
dx(col) = -delta;
dX[key] = dx;
eval_values = values.retract(dX);
const Eigen::VectorXd right = factor.whitenedError(eval_values);
const Vector right = factor.whitenedError(eval_values);
J.col(col) = (left - right) * one_over_2delta;
}
jacobians.push_back(std::make_pair(key, J));
jacobians.emplace_back(key, J);
}
// Next step...return JacobianFactor
@ -71,15 +74,15 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
namespace internal {
// CPPUnitLite-style test for linearization of a factor
bool testFactorJacobians(const std::string& name_,
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
inline bool testFactorJacobians(const std::string& name_,
const NoiseModelFactor& factor,
const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
auto actual =
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
if (!actual) return false;
@ -89,17 +92,19 @@ bool testFactorJacobians(const std::string& name_,
// if not equal, test individual jacobians:
if (!equal) {
for (size_t i = 0; i < actual->size(); i++) {
bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
bool i_good =
assert_equal((Matrix)(expected.getA(expected.begin() + i)),
(Matrix)(actual->getA(actual->begin() + i)), tolerance);
if (!i_good) {
std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
std::cout << "Mismatch in Jacobian " << i + 1
<< " (base 1), as shown above" << std::endl;
}
}
}
return equal;
}
}
} // namespace internal
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.

3
gtsam/nonlinear/tests/testCallRecord.cpp
View File

@ -153,7 +153,7 @@ TEST(CallRecord, virtualReverseAdDispatching) {
}
{
const int Rows = 6;
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>(), NJM);
record.CallRecord::reverseAD2(Eigen::Matrix<double, Rows, Cols>::Zero(), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Rows, Cols))));
record.CallRecord::reverseAD2(DynRowMat(Rows, Cols), NJM);
EXPECT((assert_equal(record.cc, CallConfig(Eigen::Dynamic, Cols, Rows, Cols))));
@ -168,4 +168,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

32
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -21,14 +21,13 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
namespace {
Key poseKey(1);
Key pointKey(2);
@ -41,43 +40,18 @@ typedef BearingFactor<Pose3, Point3> BearingFactor3D;
Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
/* ************************************************************************* */
TEST(BearingFactor, Serialization2D) {
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(BearingFactor, 2D) {
// Serialize the factor
std::string serialized = serializeXML(factor2D);
// And de-serialize it
BearingFactor2D factor;
deserializeXML(serialized, factor);
// Set the linearization point
Values values;
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(BearingFactor, Serialization3D) {
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
}
/* ************************************************************************* */

32
gtsam/sam/tests/testBearingRangeFactor.cpp
View File

@ -21,14 +21,13 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
namespace {
Key poseKey(1);
Key pointKey(2);
@ -40,43 +39,18 @@ typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
BearingRangeFactor3D factor3D(poseKey, pointKey,
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization2D) {
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(BearingRangeFactor, 2D) {
// Serialize the factor
std::string serialized = serializeXML(factor2D);
// And de-serialize it
BearingRangeFactor2D factor;
deserializeXML(serialized, factor);
// Set the linearization point
Values values;
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization3D) {
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
}
/* ************************************************************************* */

58
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
@ -32,42 +31,40 @@ using namespace std::placeholders;
using namespace std;
using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(1));
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
// Keys are deliberately *not* in sorted order to test that case.
namespace {
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(1));
constexpr Key poseKey(2);
constexpr Key pointKey(1);
constexpr double measurement(10.0);
/* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorError3D(const Pose3& pose, const Point3& point,
const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
const RangeFactorWithTransform2D& factor) {
return factor.evaluateError(pose, point);
}
/* ************************************************************************* */
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
const RangeFactorWithTransform3D& factor) {
return factor.evaluateError(pose, point);
}
} // namespace
/* ************************************************************************* */
TEST( RangeFactor, Constructor) {
@ -75,27 +72,6 @@ TEST( RangeFactor, Constructor) {
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
}
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
/* ************************************************************************* */
TEST(RangeFactor, Serialization2D) {
RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(RangeFactor, Serialization3D) {
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
TEST( RangeFactor, ConstructorWithTransform) {
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
@ -142,28 +118,6 @@ TEST( RangeFactor, EqualsWithTransform ) {
body_P_sensor_3D);
CHECK(assert_equal(factor3D_1, factor3D_2));
}
/* ************************************************************************* */
TEST( RangeFactor, EqualsAfterDeserializing) {
// Check that the same results are obtained after deserializing:
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
body_P_sensor_3D), factor3D_2;
// check with Equal() trait:
gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
CHECK(assert_equal(factor3D_1, factor3D_2));
const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
const Point3 point(-2.0, 11.0, 1.0);
const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
const Vector error_1 = factor3D_1.unwhitenedError(values);
const Vector error_2 = factor3D_2.unwhitenedError(values);
CHECK(assert_equal(error_1, error_2));
}
/* ************************************************************************* */
TEST( RangeFactor, Error2D ) {
// Create a factor
@ -421,7 +375,7 @@ struct Range<Vector4, Vector4> {
// derivatives not implemented
}
};
}
} // namespace gtsam
TEST(RangeFactor, NonGTSAM) {
// Create a factor

140
gtsam/sam/tests/testSerializationSam.cpp Normal file
View File

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationSam.cpp
* @brief All serialization test in this directory
* @author Frank Dellaert
* @date February 2022
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/sam/BearingFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
using namespace std;
using namespace gtsam;
namespace {
Key poseKey(1);
Key pointKey(2);
constexpr double rangeMmeasurement(10.0);
} // namespace
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
/* ************************************************************************* */
TEST(SerializationSam, BearingFactor2D) {
using BearingFactor2D = BearingFactor<Pose2, Point2>;
double measurement2D(10.0);
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(SerializationSam, BearingFactor3D) {
using BearingFactor3D = BearingFactor<Pose3, Point3>;
Unit3 measurement3D =
Pose3().bearing(Point3(1, 0, 0)); // has to match values!
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
namespace {
static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1));
}
TEST(SerializationSam, RangeFactor2D) {
using RangeFactor2D = RangeFactor<Pose2, Point2>;
RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(SerializationSam, RangeFactor3D) {
using RangeFactor3D = RangeFactor<Pose3, Point3>;
RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
TEST(RangeFactor, EqualsAfterDeserializing) {
// Check that the same results are obtained after deserializing:
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform<Pose3, Point3> factor3D_1(
poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D),
factor3D_2;
// check with Equal() trait:
gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
CHECK(assert_equal(factor3D_1, factor3D_2));
const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
const Point3 point(-2.0, 11.0, 1.0);
const Values values = {{poseKey, genericValue(pose)},
{pointKey, genericValue(point)}};
const Vector error_1 = factor3D_1.unwhitenedError(values);
const Vector error_2 = factor3D_2.unwhitenedError(values);
CHECK(assert_equal(error_1, error_2));
}
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization2D) {
using BearingRangeFactor2D = BearingRangeFactor<Pose2, Point2>;
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization3D) {
using BearingRangeFactor3D = BearingRangeFactor<Pose3, Point3>;
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
BearingRangeFactor3D factor3D(poseKey, pointKey,
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

52
gtsam/slam/tests/PinholeFactor.h Normal file
View File

@ -0,0 +1,52 @@
/* ----------------------------------------------------------------------------
* 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 PinholeFactor.h
* @brief helper class for tests
* @author Frank Dellaert
* @date February 2022
*/
#pragma once
namespace gtsam {
template <typename T>
struct traits;
}
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/SmartFactorBase.h>
namespace gtsam {
class PinholeFactor : public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
}
};
/// traits
template <>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
} // namespace gtsam

88
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -29,6 +29,7 @@
using namespace std;
using namespace gtsam;
namespace {
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
@ -48,23 +49,24 @@ static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static double fov = 60; // degrees
static size_t w = 640, h = 480;
}
/* ************************************************************************* */
// default Cal3_S2 cameras
namespace vanilla {
typedef PinholeCamera<Cal3_S2> Camera;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3_S2 K(fov, w, h);
static Cal3_S2 K2(1500, 1200, 0, w, h);
Camera level_camera(level_pose, K2);
Camera level_camera_right(pose_right, K2);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Camera cam1(level_pose, K2);
Camera cam2(pose_right, K2);
Camera cam3(pose_above, K2);
static const Cal3_S2 K(fov, w, h);
static const Cal3_S2 K2(1500, 1200, 0, w, h);
static const Camera level_camera(level_pose, K2);
static const Camera level_camera_right(pose_right, K2);
static const Point2 level_uv = level_camera.project(landmark1);
static const Point2 level_uv_right = level_camera_right.project(landmark1);
static const Camera cam1(level_pose, K2);
static const Camera cam2(pose_right, K2);
static const Camera cam3(pose_above, K2);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
SmartProjectionParams params;
static const SmartProjectionParams params;
} // namespace vanilla
/* ************************************************************************* */
@ -74,12 +76,12 @@ typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK);
Camera cam1(level_pose, sharedK);
Camera cam2(pose_right, sharedK);
Camera cam3(pose_above, sharedK);
static const Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
static const Camera level_camera(level_pose, sharedK);
static const Camera level_camera_right(pose_right, sharedK);
static const Camera cam1(level_pose, sharedK);
static const Camera cam2(pose_right, sharedK);
static const Camera cam3(pose_above, sharedK);
} // namespace vanillaPose
/* ************************************************************************* */
@ -89,12 +91,12 @@ typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
Camera level_camera(level_pose, sharedK2);
Camera level_camera_right(pose_right, sharedK2);
Camera cam1(level_pose, sharedK2);
Camera cam2(pose_right, sharedK2);
Camera cam3(pose_above, sharedK2);
static const Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
static const Camera level_camera(level_pose, sharedK2);
static const Camera level_camera_right(pose_right, sharedK2);
static const Camera cam1(level_pose, sharedK2);
static const Camera cam2(pose_right, sharedK2);
static const Camera cam3(pose_above, sharedK2);
} // namespace vanillaPose2
/* *************************************************************************/
@ -103,15 +105,15 @@ namespace bundler {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
Camera level_camera(level_pose, K);
Camera level_camera_right(pose_right, K);
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
Pose3 pose1 = level_pose;
Camera cam1(level_pose, K);
Camera cam2(pose_right, K);
Camera cam3(pose_above, K);
static const Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
static const Camera level_camera(level_pose, K);
static const Camera level_camera_right(pose_right, K);
static const Point2 level_uv = level_camera.project(landmark1);
static const Point2 level_uv_right = level_camera_right.project(landmark1);
static const Pose3 pose1 = level_pose;
static const Camera cam1(level_pose, K);
static const Camera cam2(pose_right, K);
static const Camera cam3(pose_above, K);
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
} // namespace bundler
@ -122,14 +124,14 @@ typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
static const boost::shared_ptr<Cal3Bundler> sharedBundlerK(new Cal3Bundler(500, 1e-3,
1e-3, 1000,
2000));
Camera level_camera(level_pose, sharedBundlerK);
Camera level_camera_right(pose_right, sharedBundlerK);
Camera cam1(level_pose, sharedBundlerK);
Camera cam2(pose_right, sharedBundlerK);
Camera cam3(pose_above, sharedBundlerK);
static const Camera level_camera(level_pose, sharedBundlerK);
static const Camera level_camera_right(pose_right, sharedBundlerK);
static const Camera cam1(level_pose, sharedBundlerK);
static const Camera cam2(pose_right, sharedBundlerK);
static const Camera cam3(pose_above, sharedBundlerK);
} // namespace bundlerPose
/* ************************************************************************* */
@ -138,12 +140,12 @@ namespace sphericalCamera {
typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static EmptyCal::shared_ptr emptyK(new EmptyCal());
Camera level_camera(level_pose);
Camera level_camera_right(pose_right);
Camera cam1(level_pose);
Camera cam2(pose_right);
Camera cam3(pose_above);
static const EmptyCal::shared_ptr emptyK(new EmptyCal());
static const Camera level_camera(level_pose);
static const Camera level_camera_right(pose_right);
static const Camera cam1(level_pose);
static const Camera cam2(pose_right);
static const Camera cam3(pose_above);
} // namespace sphericalCamera
/* *************************************************************************/

105
gtsam/slam/tests/testSerializationInSlam.cpp Normal file
View File

@ -0,0 +1,105 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationSlam.cpp
* @brief all serialization tests in this directory
* @author Frank Dellaert
* @date February 2022
*/
#include "smartFactorScenarios.h"
#include "PinholeFactor.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
namespace {
static const double rankTol = 1.0;
static const double sigma = 0.1;
static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma));
} // namespace
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
/* ************************************************************************* */
TEST(SmartFactorBase, serialize) {
using namespace serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
TEST(SerializationSlam, SmartProjectionFactor) {
using namespace vanilla;
using namespace serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
TEST(SerializationSlam, SmartProjectionPoseFactor) {
using namespace vanillaPose;
using namespace serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
TEST(SerializationSlam, SmartProjectionPoseFactor2) {
using namespace vanillaPose;
using namespace serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
Pose3 bts;
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
KeyVector key_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

75
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -16,43 +16,25 @@
* @date Feb 2015
*/
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/slam/SmartFactorBase.h>
using namespace std;
#include "PinholeFactor.h"
namespace {
using namespace gtsam;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
} // namespace
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
using namespace std;
namespace gtsam {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
}
};
/// traits
template<>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
}
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f = PinholeFactor(unit2);
f.add(Point2(0, 0), 1);
@ -83,7 +65,8 @@ TEST(SmartFactorBase, PinholeWithSensor) {
Vector expectedError = Vector::Zero(2);
Matrix29 expectedFs;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1,
0, 0, 0, 0;
Matrix23 expectedE;
expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
@ -94,20 +77,13 @@ TEST(SmartFactorBase, PinholeWithSensor) {
EXPECT(assert_equal(expectedE, E));
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
class StereoFactor : public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
double error(const Values& values) const override {
return 0.0;
}
StereoFactor(const SharedNoiseModel& sharedNoiseModel)
: Base(sharedNoiseModel) {}
double error(const Values& values) const override { return 0.0; }
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@ -117,7 +93,6 @@ public:
/// traits
template <>
struct traits<StereoFactor> : public Testable<StereoFactor> {};
}
TEST(SmartFactorBase, Stereo) {
StereoFactor f(unit3);
@ -125,24 +100,7 @@ TEST(SmartFactorBase, Stereo) {
f.add(StereoPoint2(), 2);
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartFactorBase, serialize) {
using namespace gtsam::serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
} // namespace gtsam
/* ************************************************************************* */
int main() {
@ -150,4 +108,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

32
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -22,18 +22,19 @@
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
using namespace boost::assign;
namespace {
static const bool isDebugTest = false;
static const Symbol l1('l', 1), l2('l', 2), l3('l', 3);
static const Key c1 = 1, c2 = 2, c3 = 3;
static const Point2 measurement1(323.0, 240.0);
static const double rankTol = 1.0;
}
template<class CALIBRATION>
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
@ -70,8 +71,9 @@ TEST(SmartProjectionFactor, Constructor) {
/* ************************************************************************* */
TEST(SmartProjectionFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, params);
auto myParams = params;
myParams.setRankTolerance(rankTol);
SmartFactor factor1(unit2, myParams);
}
/* ************************************************************************* */
@ -84,8 +86,9 @@ TEST(SmartProjectionFactor, Constructor3) {
/* ************************************************************************* */
TEST(SmartProjectionFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, params);
auto myParams = params;
myParams.setRankTolerance(rankTol);
SmartFactor factor1(unit2, myParams);
factor1.add(measurement1, c1);
}
@ -810,25 +813,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartProjectionFactor, serialize) {
using namespace vanilla;
using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

44
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -32,6 +31,7 @@
using namespace boost::assign;
using namespace std::placeholders;
namespace {
static const double rankTol = 1.0;
// Create a noise model for the pixel error
static const double sigma = 0.1;
@ -51,6 +51,7 @@ static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
@ -1332,47 +1333,6 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.at<Pose3>(x3)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
TEST(SmartProjectionPoseFactor, serialize) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
TEST(SmartProjectionPoseFactor, serialize2) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
Pose3 bts;
SmartFactor factor(model, sharedK, bts, params);
// insert some measurments
KeyVector key_view;
Point2Vector meas_view;
key_view.push_back(Symbol('x', 1));
meas_view.push_back(Point2(10, 10));
factor.add(meas_view, key_view);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

19
gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp
View File

@ -31,12 +31,13 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
namespace {
// make a realistic calibration matrix
static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2(
new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
b));
static SmartStereoProjectionParams params;
@ -45,8 +46,8 @@ static SmartStereoProjectionParams params;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data
static Symbol x1('X', 1);
@ -59,16 +60,19 @@ static Symbol body_P_cam3_key('P', 3);
static Key poseKey1(x1);
static Key poseExtrinsicKey1(body_P_cam1_key);
static Key poseExtrinsicKey2(body_P_cam2_key);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static StereoPoint2 measurement2(350.0, 200.0, 240.0); //potentially use more reasonable measurement value?
static StereoPoint2 measurement1(
323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
static StereoPoint2 measurement2(
350.0, 200.0, 240.0); // potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
const StereoCamera& cam2,
const StereoCamera& cam3,
Point3 landmark) {
vector<StereoPoint2> measurements_cam;
StereoPoint2 cam1_uv1 = cam1.project(landmark);
@ -82,6 +86,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
}
LevenbergMarquardtParams lm_params;
} // namespace
/* ************************************************************************* */
TEST( SmartStereoProjectionFactorPP, params) {

17
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -32,13 +32,13 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
namespace {
// make a realistic calibration matrix
static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2(
new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,
b));
static SmartStereoProjectionParams params;
@ -47,8 +47,8 @@ static SmartStereoProjectionParams params;
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;
using symbol_shorthand::X;
// tests data
static Symbol x1('X', 1);
@ -56,15 +56,17 @@ static Symbol x2('X', 2);
static Symbol x3('X', 3);
static Key poseKey1(x1);
static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
static StereoPoint2 measurement1(
323.0, 300.0, 240.0); // potentially use more reasonable measurement value?
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
static double missing_uR = std::numeric_limits<double>::quiet_NaN();
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
const StereoCamera& cam2,
const StereoCamera& cam3,
Point3 landmark) {
vector<StereoPoint2> measurements_cam;
StereoPoint2 cam1_uv1 = cam1.project(landmark);
@ -78,6 +80,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
}
LevenbergMarquardtParams lm_params;
} // namespace
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, params) {

90
tests/testSerializationSLAM.cpp → tests/testSerializationSlam.cpp
View File

@ -19,16 +19,16 @@
#include <CppUnitLite/TestHarness.h>
#include <tests/smallExample.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/geometry/Point3.h>
@ -44,8 +44,16 @@
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/serialization/export.hpp>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
@ -592,6 +600,78 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsBinary<GenericStereoFactor3D>(genericStereoFactor3D));
}
/* ************************************************************************* */
// Read from XML file
namespace {
static GaussianFactorGraph read(const string& name) {
auto inputFile = findExampleDataFile(name);
ifstream is(inputFile);
if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
boost::archive::xml_iarchive in_archive(is);
GaussianFactorGraph Ab;
in_archive >> boost::serialization::make_nvp("graph", Ab);
return Ab;
}
} // namespace
/* ************************************************************************* */
// Read from XML file
TEST(SubgraphSolver, Solves) {
using gtsam::example::planarGraph;
// Create preconditioner
SubgraphPreconditioner system;
// We test on three different graphs
const auto Ab1 = planarGraph(3).first;
const auto Ab2 = read("toy3D");
const auto Ab3 = read("randomGrid3D");
// For all graphs, test solve and solveTranspose
for (const auto& Ab : {Ab1, Ab2, Ab3}) {
// Call build, a non-const method needed to make solve work :-(
KeyInfo keyInfo(Ab);
std::map<Key, Vector> lambda;
system.build(Ab, keyInfo, lambda);
// Create a perturbed (non-zero) RHS
const auto xbar = system.Rc1().optimize(); // merely for use in zero below
auto values_y = VectorValues::Zero(xbar);
auto it = values_y.begin();
it->second.setConstant(100);
++it;
it->second.setConstant(-100);
// Solve the VectorValues way
auto values_x = system.Rc1().backSubstitute(values_y);
// Solve the matrix way, this really just checks BN::backSubstitute
// This only works with Rc1 ordering, not with keyInfo !
// TODO(frank): why does this not work with an arbitrary ordering?
const auto ord = system.Rc1().ordering();
const Matrix R1 = system.Rc1().matrix(ord).first;
auto ord_y = values_y.vector(ord);
auto vector_x = R1.inverse() * ord_y;
EXPECT(assert_equal(vector_x, values_x.vector(ord)));
// Test that 'solve' does implement x = R^{-1} y
// We do this by asserting it gives same answer as backSubstitute
// Only works with keyInfo ordering:
const auto ordering = keyInfo.ordering();
auto vector_y = values_y.vector(ordering);
const size_t N = R1.cols();
Vector solve_x = Vector::Zero(N);
system.solve(vector_y, solve_x);
EXPECT(assert_equal(values_x.vector(ordering), solve_x));
// Test that transposeSolve does implement x = R^{-T} y
// We do this by asserting it gives same answer as backSubstituteTranspose
auto values_x2 = system.Rc1().backSubstituteTranspose(values_y);
Vector solveT_x = Vector::Zero(N);
system.transposeSolve(vector_y, solveT_x);
EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

71
tests/testSubgraphPreconditioner.cpp
View File

@ -29,10 +29,8 @@
#include <CppUnitLite/TestHarness.h>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/serialization/export.hpp>
#include <boost/tuple/tuple.hpp>
using namespace boost::assign;
@ -197,75 +195,6 @@ TEST(SubgraphPreconditioner, system) {
EXPECT(assert_equal(expected_g, vec(g)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor")
// Read from XML file
static GaussianFactorGraph read(const string& name) {
auto inputFile = findExampleDataFile(name);
ifstream is(inputFile);
if (!is.is_open()) throw runtime_error("Cannot find file " + inputFile);
boost::archive::xml_iarchive in_archive(is);
GaussianFactorGraph Ab;
in_archive >> boost::serialization::make_nvp("graph", Ab);
return Ab;
}
TEST(SubgraphSolver, Solves) {
// Create preconditioner
SubgraphPreconditioner system;
// We test on three different graphs
const auto Ab1 = planarGraph(3).first;
const auto Ab2 = read("toy3D");
const auto Ab3 = read("randomGrid3D");
// For all graphs, test solve and solveTranspose
for (const auto& Ab : {Ab1, Ab2, Ab3}) {
// Call build, a non-const method needed to make solve work :-(
KeyInfo keyInfo(Ab);
std::map<Key, Vector> lambda;
system.build(Ab, keyInfo, lambda);
// Create a perturbed (non-zero) RHS
const auto xbar = system.Rc1().optimize(); // merely for use in zero below
auto values_y = VectorValues::Zero(xbar);
auto it = values_y.begin();
it->second.setConstant(100);
++it;
it->second.setConstant(-100);
// Solve the VectorValues way
auto values_x = system.Rc1().backSubstitute(values_y);
// Solve the matrix way, this really just checks BN::backSubstitute
// This only works with Rc1 ordering, not with keyInfo !
// TODO(frank): why does this not work with an arbitrary ordering?
const auto ord = system.Rc1().ordering();
const Matrix R1 = system.Rc1().matrix(ord).first;
auto ord_y = values_y.vector(ord);
auto vector_x = R1.inverse() * ord_y;
EXPECT(assert_equal(vector_x, values_x.vector(ord)));
// Test that 'solve' does implement x = R^{-1} y
// We do this by asserting it gives same answer as backSubstitute
// Only works with keyInfo ordering:
const auto ordering = keyInfo.ordering();
auto vector_y = values_y.vector(ordering);
const size_t N = R1.cols();
Vector solve_x = Vector::Zero(N);
system.solve(vector_y, solve_x);
EXPECT(assert_equal(values_x.vector(ordering), solve_x));
// Test that transposeSolve does implement x = R^{-T} y
// We do this by asserting it gives same answer as backSubstituteTranspose
auto values_x2 = system.Rc1().backSubstituteTranspose(values_y);
Vector solveT_x = Vector::Zero(N);
system.transposeSolve(vector_y, solveT_x);
EXPECT(assert_equal(values_x2.vector(ordering), solveT_x));
}
}
/* ************************************************************************* */
TEST(SubgraphPreconditioner, conjugateGradients) {
// Build a planar graph