Moved project specific factors into a different project.

release/4.3a0
krunalchande 2014-11-19 11:59:08 -05:00
parent d49396c1d2
commit 708d114b3c
9 changed files with 0 additions and 817 deletions

23
gtsam.h
View File

@ -2101,15 +2101,6 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/DistanceFactor.h>
template<T = {gtsam::Point2, gtsam::Point3}>
virtual class DistanceFactor : gtsam::NoiseModelFactor {
DistanceFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
@ -2139,20 +2130,6 @@ typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimple
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/DroneDynamicsFactor.h>
virtual class DroneDynamicsFactor : gtsam::NoiseModelFactor {
DroneDynamicsFactor(size_t key1, size_t key2, const gtsam::LieVector& measured, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/DroneDynamicsVelXYFactor.h>
virtual class DroneDynamicsVelXYFactor : gtsam::NoiseModelFactor {
DroneDynamicsVelXYFactor(size_t key1, size_t key2, size_t key3, Vector motors, Vector acc, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/CageFactor.h>
virtual class CageFactor : gtsam::NoiseModelFactor {
CageFactor(size_t key1, const gtsam::Pose3& pose, double cageBoundary, const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>

View File

@ -1,98 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 CageFactor.h
* @author Krunal Chande
* @date November 10, 2014
*/
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to constrain position based on size of the accessible area
*/
class CageFactor: public NoiseModelFactor1<Pose3> {
private:
Pose3 pose_;
double cageBoundary_;
typedef CageFactor This;
typedef NoiseModelFactor1<Pose3> Base;
public:
CageFactor() {} /* Default Constructor*/
CageFactor(Key poseKey, const Pose3& pose, double cageBoundary, const SharedNoiseModel& model):
Base(model, poseKey), pose_(pose), cageBoundary_(cageBoundary){}
virtual ~CageFactor(){}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** h(x) - z */
Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const {
double distance = pose.translation().dist(Point3(0,0,0));
if(distance > cageBoundary_){
double distance = pose.range(Point3(0,0,0), H);
return (gtsam::Vector(1) << distance - cageBoundary_);
} else {
if(H) *H = gtsam::zeros(1, Pose3::Dim());
return (gtsam::Vector(1) << 0.0);
}
// Point3 p2;
// double x = pose.x(), y = pose.y(), z = pose.z();
// if (x < 0) x = -x;
// if (y < 0) y = -y;
// if (z < 0) z = -z;
// double errorX = 100/(x-cageBoundary_), errorY = 100/(y-cageBoundary_), errorZ = 100/(z-cageBoundary_);
// if (H) *H = pose.translation().distance(p2, H);
// return (Vector(3)<<errorX, errorY, errorZ);
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
;
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "Cage Factor, Cage Boundary = " << cageBoundary_ << " Pose: " << pose_ << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(cageBoundary_);
ar & BOOST_SERIALIZATION_NVP(pose_);
}
}; // end CageFactor
} // end namespace

View File

@ -1,88 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 DistanceFactor.h
* @author Duy-Nguyen Ta
* @date Sep 26, 2014
*
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Factor to constrain known measured distance between two points
*/
template<class POINT>
class DistanceFactor: public NoiseModelFactor2<POINT, POINT> {
double measured_; /// measured distance
typedef NoiseModelFactor2<POINT, POINT> Base;
typedef DistanceFactor<POINT> This;
public:
/// Default constructor
DistanceFactor() {
}
/// Constructor with keys and known measured distance
DistanceFactor(Key p1, Key p2, double measured, const SharedNoiseModel& model) :
Base(model, p1, p2), measured_(measured) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/// h(x)-z
Vector evaluateError(const POINT& p1, const POINT& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
double distance = p1.distance(p2, H1, H2);
return (Vector(1) << distance - measured_);
}
/** return the measured */
double measured() const {
return measured_;
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "DistanceFactor, distance = " << measured_ << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
} /* namespace gtsam */

View File

@ -1,114 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 DroneDynamicsFactor.h
* @author Duy-Nguyen Ta
* @date Sep 29, 2014
*/
// Implementation is incorrect use DroneDynamicsVelXYFactor instead.
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
class DroneDynamicsFactor: public NoiseModelFactor2<Pose3, LieVector> {
private:
LieVector measured_; /** body velocity measured from raw acc and motor inputs*/
typedef DroneDynamicsFactor This;
typedef NoiseModelFactor2<Pose3, LieVector> Base;
public:
DroneDynamicsFactor() {} /* Default constructor */
DroneDynamicsFactor(Key poseKey, Key velKey, const LieVector& measured,
const SharedNoiseModel& model) :
Base(model, poseKey, velKey), measured_(measured) {
}
virtual ~DroneDynamicsFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const LieVector& vel,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
// error = v - wRb*measured
Rot3 wRb = pose.rotation();
Vector3 error;
if (H1 || H2) {
*H2 = eye(3);
*H1 = zeros(3,6);
Matrix H1Rot;
error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - measured_.vector();
(*H1).block(0,0,3,3) = H1Rot;
}
else {
error = wRb.unrotate(Point3(vel.vector())).vector() - measured_.vector();
}
return error;
}
/** return the measured */
LieVector measured() const {
return measured_;
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
;
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "DroneDynamicsFactor, measured = " << measured_.vector().transpose() << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // DroneDynamicsFactor
} // namespace gtsam

View File

@ -1,124 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 DroneDynamicsFactor.h
* @author Duy-Nguyen Ta
* @date Oct 1, 2014
*/
#pragma once
#include <boost/lexical_cast.hpp>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
class DroneDynamicsVelXYFactor: public NoiseModelFactor3<Pose3, LieVector, LieVector> {
private:
Vector motors_; /** motor inputs */
Vector acc_; /** raw acc */
Matrix M_;
typedef DroneDynamicsVelXYFactor This;
typedef NoiseModelFactor3<Pose3, LieVector, LieVector> Base;
public:
DroneDynamicsVelXYFactor() {} /* Default constructor */
DroneDynamicsVelXYFactor(Key poseKey, Key velKey, Key cKey, const Vector& motors, const Vector& acc,
const SharedNoiseModel& model) :
Base(model, poseKey, velKey, cKey), motors_(motors), acc_(acc), M_(computeM(motors, acc)) {
}
virtual ~DroneDynamicsVelXYFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// M = [sum(sqrt(m))ax 1 0 0; 0 0 sum(sqrt(m))ay 1; 0 0 0 0]
Matrix computeM(const Vector& motors, const Vector& acc) const {
Matrix M = zeros(3,4);
double sumMotors = (motors(0)) + (motors(1)) + (motors(2)) + (motors(3));
M(0,0) = acc(0)/sumMotors; M(0, 1) = 1.0/sumMotors;
M(1,2) = 1.0/sumMotors; M(1, 3) = acc(1)/sumMotors;
return M;
}
/** h(x)-z */
Vector evaluateError(const Pose3& pose, const LieVector& vel, const LieVector& c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
// error = R'*v - M*c, where
Rot3 wRb = pose.rotation();
Vector error;
if (H1 || H2 || H3) {
*H1 = zeros(3, 6);
*H2 = eye(3);
Matrix H1Rot;
error = wRb.unrotate(Point3(vel.vector()), H1Rot, H2).vector() - M_*c.vector();
(*H1).block(0,0,3,3) = H1Rot;
*H3 = -M_;
}
else {
error = wRb.unrotate(Point3(vel.vector())).vector() - M_*c.vector();
}
return error;
}
/** equals specialized to this factor */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL
&& Base::equals(*e, tol)
;
}
/** print contents */
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "DroneDynamicsVelXYFactor, motors = " << motors_.transpose() << " acc: " << acc_.transpose() << std::endl;
Base::print("", keyFormatter);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(motors_);
ar & BOOST_SERIALIZATION_NVP(acc_);
}
}; // DroneDynamicsVelXYFactor
} // namespace gtsam

View File

@ -1,78 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testCageFactor.cpp
* @brief Unit tests CageFactor class
* @author Krunal Chande
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/CageFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// Create a noise model
static SharedNoiseModel model(noiseModel::Unit::Create(6));
LieVector factorError(const Pose3& pose, const CageFactor& factor) {
return factor.evaluateError(pose);
}
/* ************************************************************************* */
TEST(CageFactor, Inside) {
Key poseKey(1);
Pose3 pose(Rot3::ypr(0,0,0),Point3(0,0,0));
double cageBoundary = 10; // in m
CageFactor factor(poseKey, pose, cageBoundary, model);
// Set the linearization point
Pose3 poseLin;
Matrix H;
Vector actualError(factor.evaluateError(poseLin, H));
Vector expectedError = zero(1);
CHECK(assert_equal(expectedError, actualError, 1e-9));
// use numerical derivatives to calculate the jacobians
Matrix HExpected;
HExpected = numericalDerivative11<Pose3>(boost::bind(&factorError, _1, factor), pose);
CHECK(assert_equal(HExpected, H, 1e-9));
}
/* ************************************************************************* */
TEST(CageFactor, Outside) {
Key poseKey(1);
Point3 translation = Point3(15,0,0);
Pose3 pose(Rot3::ypr(0,0,0),translation);
double cageBoundary = 10; // in m
CageFactor factor(poseKey, pose, cageBoundary, model);
// Set the linearization point
Pose3 poseLin;
Matrix H;
Vector actualError(factor.evaluateError(pose, H));
Vector expectedError(Vector(1)<<5);
CHECK(assert_equal(expectedError, actualError, 1e-9));
// use numerical derivatives to calculate the jacobians
Matrix HExpected;
HExpected = numericalDerivative11<Pose3>(boost::bind(&factorError, _1, factor), pose);
CHECK(assert_equal(HExpected, H, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -1,82 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testDistanceFactor.cpp
* @brief Unit tests for DistanceFactor Class
* @author Duy-Nguyen Ta
* @date Oct 2014
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/slam/DistanceFactor.h>
using namespace std;
using namespace gtsam;
typedef DistanceFactor<Point2> DistanceFactor2D;
typedef DistanceFactor<Point3> DistanceFactor3D;
SharedDiagonal noise = noiseModel::Unit::Create(1);
Point3 P(0., 1., 2.5), Q(10., -81., 7.);
Point2 p(1., 2.5), q(-81., 7.);
/* ************************************************************************* */
TEST(DistanceFactor, Point3) {
DistanceFactor3D distanceFactor(0, 1, P.distance(Q), noise);
Matrix H1, H2;
Vector error = distanceFactor.evaluateError(P, Q, H1, H2);
Vector expectedError = zero(1);
EXPECT(assert_equal(expectedError, error, 1e-10));
boost::function<Vector(const Point3&, const Point3&)> testEvaluateError(
boost::bind(&DistanceFactor3D::evaluateError, distanceFactor, _1, _2,
boost::none, boost::none));
Matrix numericalH1 = numericalDerivative21(testEvaluateError, P, Q, 1e-5);
Matrix numericalH2 = numericalDerivative22(testEvaluateError, P, Q, 1e-5);
EXPECT(assert_equal(numericalH1, H1, 1e-8));
EXPECT(assert_equal(numericalH2, H2, 1e-8));
}
/* ************************************************************************* */
TEST(DistanceFactor, Point2) {
DistanceFactor2D distanceFactor(0, 1, p.distance(q), noise);
Matrix H1, H2;
Vector error = distanceFactor.evaluateError(p, q, H1, H2);
Vector expectedError = zero(1);
EXPECT(assert_equal(expectedError, error, 1e-10));
boost::function<Vector(const Point2&, const Point2&)> testEvaluateError(
boost::bind(&DistanceFactor2D::evaluateError, distanceFactor, _1, _2,
boost::none, boost::none));
Matrix numericalH1 = numericalDerivative21(testEvaluateError, p, q, 1e-5);
Matrix numericalH2 = numericalDerivative22(testEvaluateError, p, q, 1e-5);
EXPECT(assert_equal(numericalH1, H1, 1e-8));
EXPECT(assert_equal(numericalH2, H2, 1e-8));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -1,102 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testRangeFactor.cpp
* @brief Unit tests for DroneDynamicsFactor Class
* @author Duy-Nguyen Ta
* @date Oct 2014
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/DroneDynamicsFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3));
/* ************************************************************************* */
LieVector factorError(const Pose3& pose, const LieVector& vel, const DroneDynamicsFactor& factor) {
return factor.evaluateError(pose, vel);
}
/* ************************************************************************* */
TEST( DroneDynamicsFactor, Error) {
// Create a factor
Key poseKey(1);
Key velKey(2);
LieVector measurement((Vector(3)<<10.0, 1.5, 0.0));
DroneDynamicsFactor factor(poseKey, velKey, measurement, model);
// Set the linearization point
Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3());
LieVector vel((Vector(3) <<
-2.913425624770731,
-2.200086236883632,
-9.429823523226959));
// Use the factor to calculate the error
Matrix H1, H2;
Vector actualError(factor.evaluateError(pose, vel, H1, H2));
Vector expectedError = zero(3);
// Verify we get the expected error
CHECK(assert_equal(expectedError, actualError, 1e-9));
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError, _1, vel, factor), pose);
H2Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, _1, factor), vel);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1, 1e-9));
CHECK(assert_equal(H2Expected, H2, 1e-9));
}
/* *************************************************************************
TEST( DroneDynamicsFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* *************************************************************************
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

View File

@ -1,108 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 testRangeFactor.cpp
* @brief Unit tests for DroneDynamicsVelXYFactor Class
* @author Duy-Nguyen Ta
* @date Oct 2014
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/DroneDynamicsVelXYFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
using namespace std;
using namespace gtsam;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(3));
/* ************************************************************************* */
LieVector factorError(const Pose3& pose, const LieVector& vel, const LieVector& coeffs, const DroneDynamicsVelXYFactor& factor) {
return factor.evaluateError(pose, vel, coeffs);
}
/* ************************************************************************* */
TEST( DroneDynamicsVelXYFactor, Error) {
// Create a factor
Key poseKey(1);
Key velKey(2);
Key coeffsKey(3);
Vector motors = (Vector(4) << 179, 180, 167, 168)/256.0;
Vector3 acc = (Vector(3) << 2., 1., 3.);
DroneDynamicsVelXYFactor factor(poseKey, velKey, coeffsKey, motors, acc, model);
// Set the linearization point
Pose3 pose(Rot3::ypr(1.0, 2.0, 0.57), Point3());
LieVector vel((Vector(3) <<
-2.913425624770731,
-2.200086236883632,
-9.429823523226959));
LieVector coeffs((Vector(4) << -9.3, 2.7, -6.5, 1.2));
// Use the factor to calculate the error
Matrix H1, H2, H3;
Vector actualError(factor.evaluateError(pose, vel, coeffs, H1, H2, H3));
Vector expectedError = zero(3);
// Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9));
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError, _1, vel, coeffs, factor), pose);
H2Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, _1, coeffs, factor), vel);
H3Expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&factorError, pose, vel, _1, factor), coeffs);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1, 1e-9));
CHECK(assert_equal(H2Expected, H2, 1e-9));
CHECK(assert_equal(H3Expected, H3, 1e-9));
}
/* *************************************************************************
TEST( DroneDynamicsVelXYFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Pose2 pose(1.0, 2.0, 0.57);
Point2 point(-4.0, 11.0);
// Use the factor to calculate the Jacobians
Matrix H1Actual, H2Actual;
factor.evaluateError(pose, point, H1Actual, H2Actual);
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* *************************************************************************
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */