Moved project specific factors into a different project.
parent
d49396c1d2
commit
708d114b3c
23
gtsam.h
23
gtsam.h
|
@ -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>
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
@ -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 */
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
||||
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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); }
|
||||
/* ************************************************************************* */
|
||||
|
Loading…
Reference in New Issue