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