Moved TSAMFactors.h from tsam to gtsam
dellaert
parent
0a5690dfb3
commit
05c1e572b6
|
|
@ -0,0 +1,167 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 TSAMFactors.h
|
||||||
|
* @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date May 2014
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Pose2.h>
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* DeltaFactor: relative 2D measurement between Pose2 and Point2
|
||||||
|
*/
|
||||||
|
class DeltaFactor: public NoiseModelFactor2<Pose2, Point2> {
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef DeltaFactor This;
|
||||||
|
typedef NoiseModelFactor2<Pose2, Point2> Base;
|
||||||
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
|
private:
|
||||||
|
Point2 measured_; ///< the measurement
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
DeltaFactor(Key i, Key j, const Point2& measured,
|
||||||
|
const SharedNoiseModel& model) :
|
||||||
|
Base(model, i, j), measured_(measured) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Evaluate measurement error h(x)-z
|
||||||
|
Vector evaluateError(const Pose2& pose, const Point2& point,
|
||||||
|
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||||
|
boost::none) const {
|
||||||
|
Point2 d = pose.transform_to(point, H1, H2);
|
||||||
|
Point2 e = measured_.between(d);
|
||||||
|
return e.vector();
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes
|
||||||
|
*/
|
||||||
|
class DeltaFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> {
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef DeltaFactorBase This;
|
||||||
|
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> Base;
|
||||||
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
|
private:
|
||||||
|
Point2 measured_; ///< the measurement
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured,
|
||||||
|
const SharedNoiseModel& model) :
|
||||||
|
Base(model, b1, i, b2, j), measured_(measured) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Evaluate measurement error h(x)-z
|
||||||
|
Vector evaluateError(const Pose2& base1, const Pose2& pose,
|
||||||
|
const Pose2& base2, const Point2& point, //
|
||||||
|
boost::optional<Matrix&> H1 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H2 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H3 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H4 = boost::none) const {
|
||||||
|
if (H1 || H2 || H3 || H4) {
|
||||||
|
// TODO use fixed-size matrices
|
||||||
|
Matrix D_pose_g_base1, D_pose_g_pose;
|
||||||
|
Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose);
|
||||||
|
Matrix D_point_g_base2, D_point_g_point;
|
||||||
|
Point2 point_g = base2.transform_from(point, D_point_g_base2,
|
||||||
|
D_point_g_point);
|
||||||
|
Matrix D_e_pose_g, D_e_point_g;
|
||||||
|
Point2 d = pose_g.transform_to(point_g, D_e_pose_g, D_e_point_g);
|
||||||
|
if (H1)
|
||||||
|
*H1 = D_e_pose_g * D_pose_g_base1;
|
||||||
|
if (H2)
|
||||||
|
*H2 = D_e_pose_g * D_pose_g_pose;
|
||||||
|
if (H3)
|
||||||
|
*H3 = D_e_point_g * D_point_g_base2;
|
||||||
|
if (H4)
|
||||||
|
*H4 = D_e_point_g * D_point_g_point;
|
||||||
|
return measured_.localCoordinates(d);
|
||||||
|
} else {
|
||||||
|
Pose2 pose_g = base1.compose(pose);
|
||||||
|
Point2 point_g = base2.transform_from(point);
|
||||||
|
Point2 d = pose_g.transform_to(point_g);
|
||||||
|
return measured_.localCoordinates(d);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* OdometryFactorBase: Pose2 odometry, with Basenodes
|
||||||
|
*/
|
||||||
|
class OdometryFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> {
|
||||||
|
|
||||||
|
public:
|
||||||
|
typedef OdometryFactorBase This;
|
||||||
|
typedef NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> Base;
|
||||||
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
|
private:
|
||||||
|
Pose2 measured_; ///< the measurement
|
||||||
|
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured,
|
||||||
|
const SharedNoiseModel& model) :
|
||||||
|
Base(model, b1, i, b2, j), measured_(measured) {
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Evaluate measurement error h(x)-z
|
||||||
|
Vector evaluateError(const Pose2& base1, const Pose2& pose1,
|
||||||
|
const Pose2& base2, const Pose2& pose2, //
|
||||||
|
boost::optional<Matrix&> H1 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H2 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H3 = boost::none, //
|
||||||
|
boost::optional<Matrix&> H4 = boost::none) const {
|
||||||
|
if (H1 || H2 || H3 || H4) {
|
||||||
|
// TODO use fixed-size matrices
|
||||||
|
Matrix D_pose1_g_base1, D_pose1_g_pose1;
|
||||||
|
Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1);
|
||||||
|
Matrix D_pose2_g_base2, D_pose2_g_pose2;
|
||||||
|
Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2);
|
||||||
|
Matrix D_e_pose1_g, D_e_pose2_g;
|
||||||
|
Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g);
|
||||||
|
if (H1)
|
||||||
|
*H1 = D_e_pose1_g * D_pose1_g_base1;
|
||||||
|
if (H2)
|
||||||
|
*H2 = D_e_pose1_g * D_pose1_g_pose1;
|
||||||
|
if (H3)
|
||||||
|
*H3 = D_e_pose2_g * D_pose2_g_base2;
|
||||||
|
if (H4)
|
||||||
|
*H4 = D_e_pose2_g * D_pose2_g_pose2;
|
||||||
|
return measured_.localCoordinates(d);
|
||||||
|
} else {
|
||||||
|
Pose2 pose1_g = base1.compose(pose1);
|
||||||
|
Pose2 pose2_g = base2.compose(pose2);
|
||||||
|
Pose2 d = pose1_g.between(pose2_g);
|
||||||
|
return measured_.localCoordinates(d);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,149 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testTSAMFactors.cpp
|
||||||
|
* @brief Unit tests for TSAM 1 Factors
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date May 2014
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam_unstable/slam/TSAMFactors.h>
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
Key i(1), j(2); // Key for pose and point
|
||||||
|
|
||||||
|
//*************************************************************************
|
||||||
|
TEST( DeltaFactor, all ) {
|
||||||
|
// Create a factor
|
||||||
|
Point2 measurement(1, 1);
|
||||||
|
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||||
|
DeltaFactor factor(i, j, measurement, model);
|
||||||
|
|
||||||
|
// Set the linearization point
|
||||||
|
Pose2 pose(1, 2, 0);
|
||||||
|
Point2 point(4, 11);
|
||||||
|
Vector2 expected(4 - 1 - 1, 11 - 2 - 1);
|
||||||
|
|
||||||
|
// Use the factor to calculate the Jacobians
|
||||||
|
Matrix H1Actual, H2Actual;
|
||||||
|
Vector actual = factor.evaluateError(pose, point, H1Actual, H2Actual);
|
||||||
|
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||||
|
|
||||||
|
// Use numerical derivatives to calculate the Jacobians
|
||||||
|
Matrix H1Expected, H2Expected;
|
||||||
|
H1Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
|
||||||
|
boost::none), pose);
|
||||||
|
H2Expected = numericalDerivative11<LieVector, Point2>(
|
||||||
|
boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
|
||||||
|
boost::none), point);
|
||||||
|
|
||||||
|
// Verify the Jacobians are correct
|
||||||
|
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
//*************************************************************************
|
||||||
|
TEST( DeltaFactorBase, all ) {
|
||||||
|
// Create a factor
|
||||||
|
Key b1(10), b2(20);
|
||||||
|
Point2 measurement(1, 1);
|
||||||
|
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||||
|
DeltaFactorBase factor(b1, i, b2, j, measurement, model);
|
||||||
|
|
||||||
|
// Set the linearization point
|
||||||
|
Pose2 base1, base2(1, 0, 0);
|
||||||
|
Pose2 pose(1, 2, 0);
|
||||||
|
Point2 point(4, 11);
|
||||||
|
Vector2 expected(4 + 1 - 1 - 1, 11 - 2 - 1);
|
||||||
|
|
||||||
|
// Use the factor to calculate the Jacobians
|
||||||
|
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
|
||||||
|
Vector actual = factor.evaluateError(base1, pose, base2, point, H1Actual,
|
||||||
|
H2Actual, H3Actual, H4Actual);
|
||||||
|
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||||
|
|
||||||
|
// Use numerical derivatives to calculate the Jacobians
|
||||||
|
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
|
||||||
|
H1Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
|
||||||
|
point, boost::none, boost::none, boost::none, boost::none), base1);
|
||||||
|
H2Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
|
||||||
|
point, boost::none, boost::none, boost::none, boost::none), pose);
|
||||||
|
H3Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
|
||||||
|
point, boost::none, boost::none, boost::none, boost::none), base2);
|
||||||
|
H4Expected = numericalDerivative11<LieVector, Point2>(
|
||||||
|
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
|
||||||
|
_1, boost::none, boost::none, boost::none, boost::none), point);
|
||||||
|
|
||||||
|
// Verify the Jacobians are correct
|
||||||
|
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
//*************************************************************************
|
||||||
|
TEST( OdometryFactorBase, all ) {
|
||||||
|
// Create a factor
|
||||||
|
Key b1(10), b2(20);
|
||||||
|
Pose2 measurement(1, 1, 0);
|
||||||
|
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||||
|
OdometryFactorBase factor(b1, i, b2, j, measurement, model);
|
||||||
|
|
||||||
|
// Set the linearization pose2
|
||||||
|
Pose2 base1, base2(1, 0, 0);
|
||||||
|
Pose2 pose1(1, 2, 0), pose2(4, 11, 0);
|
||||||
|
Vector3 expected(4 + 1 - 1 - 1, 11 - 2 - 1, 0);
|
||||||
|
|
||||||
|
// Use the factor to calculate the Jacobians
|
||||||
|
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
|
||||||
|
Vector actual = factor.evaluateError(base1, pose1, base2, pose2, H1Actual,
|
||||||
|
H2Actual, H3Actual, H4Actual);
|
||||||
|
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||||
|
|
||||||
|
// Use numerical derivatives to calculate the Jacobians
|
||||||
|
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
|
||||||
|
H1Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
|
||||||
|
pose2, boost::none, boost::none, boost::none, boost::none), base1);
|
||||||
|
H2Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
|
||||||
|
pose2, boost::none, boost::none, boost::none, boost::none), pose1);
|
||||||
|
H3Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
|
||||||
|
pose2, boost::none, boost::none, boost::none, boost::none), base2);
|
||||||
|
H4Expected = numericalDerivative11<LieVector, Pose2>(
|
||||||
|
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
|
||||||
|
base2, _1, boost::none, boost::none, boost::none, boost::none),
|
||||||
|
pose2);
|
||||||
|
|
||||||
|
// Verify the Jacobians are correct
|
||||||
|
EXPECT(assert_equal(H1Expected, H1Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H2Expected, H2Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H3Expected, H3Actual, 1e-9));
|
||||||
|
EXPECT(assert_equal(H4Expected, H4Actual, 1e-9));
|
||||||
|
}
|
||||||
|
|
||||||
|
//*************************************************************************
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
//*************************************************************************
|
||||||
|
|
||||||
Loading…
Reference in New Issue