115 lines
4.2 KiB
C++
115 lines
4.2 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/Pose3.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
#include <gtsam/base/TestableAssertions.h>
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
|
|
|
|
// Pose 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<Pose3> TestPartialPriorFactor;
|
|
|
|
/// traits
|
|
namespace gtsam {
|
|
template<>
|
|
struct traits<TestPartialPriorFactor> : public Testable<TestPartialPriorFactor> {
|
|
};
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(PartialPriorFactor, JacobianPartialTranslation) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = noiseModel::Isotropic::Sigma(1, 0.25);
|
|
|
|
TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().x(), model);
|
|
|
|
// Create a linearization point at the zero-error point
|
|
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), 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, JacobianFullTranslation) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
|
|
|
std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
|
|
TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model);
|
|
|
|
// Create a linearization point at the zero-error point
|
|
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), 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, JacobianFullRotation) {
|
|
Key poseKey(1);
|
|
Pose3 measurement(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
|
|
|
|
std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
|
|
TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model);
|
|
|
|
// Create a linearization point at the zero-error point
|
|
Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0));
|
|
|
|
// Calculate numerical derivatives.
|
|
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
|
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), 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); }
|
|
/* ************************************************************************* */
|