Add constructor tests and extend tests to Pose2
Milo Knowles
parent
ef2cd5dab5
commit
74b92efd89
|
|
@ -11,6 +11,7 @@
|
|||
|
||||
#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>
|
||||
|
|
@ -19,8 +20,9 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace NM = gtsam::noiseModel;
|
||||
|
||||
// Pose representation is [ Rx Ry Rz Tx Ty Tz ].
|
||||
// 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;
|
||||
|
|
@ -29,30 +31,152 @@ static const int kIndexTy = 4;
|
|||
static const int kIndexTz = 5;
|
||||
|
||||
|
||||
typedef PartialPriorFactor<Pose3> TestPartialPriorFactor;
|
||||
typedef PartialPriorFactor<Pose2> TestPartialPriorFactor2;
|
||||
typedef PartialPriorFactor<Pose3> TestPartialPriorFactor3;
|
||||
typedef std::vector<size_t> Indices;
|
||||
|
||||
/// traits
|
||||
namespace gtsam {
|
||||
template<>
|
||||
struct traits<TestPartialPriorFactor> : public Testable<TestPartialPriorFactor> {
|
||||
};
|
||||
struct traits<TestPartialPriorFactor2> : public Testable<TestPartialPriorFactor2> {};
|
||||
|
||||
template<>
|
||||
struct traits<TestPartialPriorFactor3> : public Testable<TestPartialPriorFactor3> {};
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PartialPriorFactor, JacobianAtIdentity)
|
||||
{
|
||||
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>(
|
||||
boost::bind(&TestPartialPriorFactor2::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, 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>(
|
||||
boost::bind(&TestPartialPriorFactor2::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, 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>(
|
||||
boost::bind(&TestPartialPriorFactor2::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, 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 = noiseModel::Isotropic::Sigma(1, 0.25);
|
||||
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
|
||||
|
||||
TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
||||
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
||||
|
||||
// Create a linearization point at the zero-error point
|
||||
Pose3 pose = Pose3::identity();
|
||||
Pose3 pose = measurement; // Zero-error linearization point.
|
||||
|
||||
// Calculate numerical derivatives.
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
|
||||
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
|
||||
|
||||
// Use the factor to calculate the derivative.
|
||||
Matrix actualH1;
|
||||
|
|
@ -63,19 +187,18 @@ TEST(PartialPriorFactor, JacobianAtIdentity)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PartialPriorFactor, JacobianPartialTranslation) {
|
||||
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 = noiseModel::Isotropic::Sigma(1, 0.25);
|
||||
SharedNoiseModel model = NM::Isotropic::Sigma(1, 0.25);
|
||||
|
||||
TestPartialPriorFactor factor(poseKey, kIndexTy, measurement.translation().y(), model);
|
||||
TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), 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));
|
||||
Pose3 pose = measurement; // Zero-error linearization point.
|
||||
|
||||
// Calculate numerical derivatives.
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
|
||||
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
|
||||
|
||||
// Use the factor to calculate the derivative.
|
||||
Matrix actualH1;
|
||||
|
|
@ -86,20 +209,20 @@ TEST(PartialPriorFactor, JacobianPartialTranslation) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PartialPriorFactor, JacobianFullTranslation) {
|
||||
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 = noiseModel::Isotropic::Sigma(3, 0.25);
|
||||
SharedNoiseModel model = NM::Isotropic::Sigma(3, 0.25);
|
||||
|
||||
std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
|
||||
TestPartialPriorFactor factor(poseKey, translationIndices, measurement.translation(), model);
|
||||
TestPartialPriorFactor3 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));
|
||||
Pose3 pose = measurement; // Zero-error linearization point.
|
||||
|
||||
// Calculate numerical derivatives.
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
|
||||
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
|
||||
|
||||
// Use the factor to calculate the derivative.
|
||||
Matrix actualH1;
|
||||
|
|
@ -110,20 +233,43 @@ TEST(PartialPriorFactor, JacobianFullTranslation) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(PartialPriorFactor, JacobianFullRotation) {
|
||||
TEST(PartialPriorFactor, JacobianTxTz3) {
|
||||
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);
|
||||
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> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
|
||||
TestPartialPriorFactor factor(poseKey, rotationIndices, Rot3::Logmap(measurement.rotation()), model);
|
||||
std::vector<size_t> translationIndices = { kIndexTx, kIndexTz };
|
||||
TestPartialPriorFactor3 factor(poseKey, translationIndices,
|
||||
(Vector(2) << measurement.x(), measurement.z()).finished(), 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));
|
||||
Pose3 pose = measurement; // Zero-error linearization point.
|
||||
|
||||
// Calculate numerical derivatives.
|
||||
Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
|
||||
boost::bind(&TestPartialPriorFactor::evaluateError, &factor, _1, boost::none), pose);
|
||||
boost::bind(&TestPartialPriorFactor3::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, 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>(
|
||||
boost::bind(&TestPartialPriorFactor3::evaluateError, &factor, _1, boost::none), pose);
|
||||
|
||||
// Use the factor to calculate the derivative.
|
||||
Matrix actualH1;
|
||||
|
|
|
|||
Loading…
Reference in New Issue