287 lines
11 KiB
C++
287 lines
11 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
#include <gtsam_unstable/slam/PartialPriorFactor.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/base/TestableAssertions.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
using namespace std::placeholders;
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
namespace NM = gtsam::noiseModel;
|
|
|
|
// Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ].
|
|
static const int kIndexRx = 0;
|
|
static const int kIndexRy = 1;
|
|
static const int kIndexRz = 2;
|
|
static const int kIndexTx = 3;
|
|
static const int kIndexTy = 4;
|
|
static const int kIndexTz = 5;
|
|
|
|
typedef PartialPriorFactor<Pose2> TestPartialPriorFactor2;
|
|
typedef PartialPriorFactor<Pose3> TestPartialPriorFactor3;
|
|
typedef std::vector<size_t> Indices;
|
|
|
|
/// traits
|
|
namespace gtsam {
|
|
template<>
|
|
struct traits<TestPartialPriorFactor2> : public Testable<TestPartialPriorFactor2> {};
|
|
|
|
template<>
|
|
struct traits<TestPartialPriorFactor3> : public Testable<TestPartialPriorFactor3> {};
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, Constructors2) {
|
|
Key poseKey(1);
|
|
Pose2 measurement(-13.1, 3.14, -0.73);
|
|
|
|
// Prior on x component of translation.
|
|
TestPartialPriorFactor2 factor1(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25));
|
|
CHECK(assert_equal(1, factor1.prior().rows()));
|
|
CHECK(assert_equal(measurement.x(), factor1.prior()(0)));
|
|
CHECK(assert_container_equality<Indices>({ 0 }, factor1.indices()));
|
|
|
|
// Prior on full translation vector.
|
|
const Indices t_indices = { 0, 1 };
|
|
TestPartialPriorFactor2 factor2(poseKey, t_indices, measurement.translation(), NM::Isotropic::Sigma(2, 0.25));
|
|
CHECK(assert_equal(2, factor2.prior().rows()));
|
|
CHECK(assert_equal(measurement.translation(), factor2.prior()));
|
|
CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
|
|
|
|
// Prior on theta.
|
|
TestPartialPriorFactor2 factor3(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.1));
|
|
CHECK(assert_equal(1, factor3.prior().rows()));
|
|
CHECK(assert_equal(measurement.theta(), factor3.prior()(0)));
|
|
CHECK(assert_container_equality<Indices>({ 2 }, factor3.indices()));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianPartialTranslation2) {
|
|
Key poseKey(1);
|
|
Pose2 measurement(-13.1, 3.14, -0.73);
|
|
|
|
// Prior on x component of translation.
|
|
TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(1, 0.25));
|
|
|
|
Pose2 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
|
|
[&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianFullTranslation2) {
|
|
Key poseKey(1);
|
|
Pose2 measurement(-6.0, 3.5, 0.123);
|
|
|
|
// Prior on x component of translation.
|
|
TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
|
|
NM::Isotropic::Sigma(2, 0.25));
|
|
|
|
Pose2 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
|
|
[&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianTheta) {
|
|
Key poseKey(1);
|
|
Pose2 measurement(-1.0, 0.4, -2.5);
|
|
|
|
// Prior on x component of translation.
|
|
TestPartialPriorFactor2 factor(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(1, 0.25));
|
|
|
|
Pose2 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
|
|
[&factor](const Pose2& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, Constructors3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
|
|
|
|
// Single component of translation.
|
|
TestPartialPriorFactor3 factor1(poseKey, kIndexTy, measurement.y(),
|
|
NM::Isotropic::Sigma(1, 0.25));
|
|
CHECK(assert_equal(1, factor1.prior().rows()));
|
|
CHECK(assert_equal(measurement.y(), factor1.prior()(0)));
|
|
CHECK(assert_container_equality<Indices>({ kIndexTy }, factor1.indices()));
|
|
|
|
// Full translation vector.
|
|
const Indices t_indices = { kIndexTx, kIndexTy, kIndexTz };
|
|
TestPartialPriorFactor3 factor2(poseKey, t_indices, measurement.translation(),
|
|
NM::Isotropic::Sigma(3, 0.25));
|
|
CHECK(assert_equal(3, factor2.prior().rows()));
|
|
CHECK(assert_equal(measurement.translation(), factor2.prior()));
|
|
CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
|
|
|
|
// Full tangent vector of rotation.
|
|
const Indices r_indices = { kIndexRx, kIndexRy, kIndexRz };
|
|
TestPartialPriorFactor3 factor3(poseKey, r_indices, Rot3::Logmap(measurement.rotation()),
|
|
NM::Isotropic::Sigma(3, 0.1));
|
|
CHECK(assert_equal(3, factor3.prior().rows()));
|
|
CHECK(assert_equal(Rot3::Logmap(measurement.rotation()), factor3.prior()));
|
|
CHECK(assert_container_equality<Indices>(r_indices, factor3.indices()));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianAtIdentity3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement = Pose3::Identity();
|
|
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
|
|
|
|
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
|
|
|
Pose3 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianPartialTranslation3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
|
|
|
|
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
|
|
|
Pose3 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianFullTranslation3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25);
|
|
|
|
std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
|
|
TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model);
|
|
|
|
// Create a linearization point at the zero-error point
|
|
Pose3 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianTxTz3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(-0.17, 0.567, M_PI), Point3(10.0, -2.3, 3.14));
|
|
SharedNoiseModel model = NM::Isotropic::Sigma(2, 0.25);
|
|
|
|
std::vector<size_t> translationIndices = { kIndexTx, kIndexTz };
|
|
TestPartialPriorFactor3 factor(poseKey, translationIndices,
|
|
(Vector(2) << measurement.x(), measurement.z()).finished(), model);
|
|
|
|
Pose3 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianFullRotation3) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25);
|
|
|
|
std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
|
|
TestPartialPriorFactor3 factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model);
|
|
|
|
Pose3 pose = measurement; // Zero-error linearization point.
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
[&factor](const Pose3& p) { return factor.evaluateError(p); }, pose);
|
|
|
|
// Use the factor to calculate the derivative.
|
|
Matrix actualH1;
|
|
factor.evaluateError(pose, actualH1);
|
|
|
|
// Verify we get the expected error.
|
|
CHECK(assert_equal(expectedH1, actualH1, 1e-5));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|