Moved project specific factors into a different project.
krunalchande
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>
|
#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}>
|
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::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
|
||||||
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
|
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>
|
#include <gtsam/slam/BearingFactor.h>
|
||||||
template<POSE, POINT, ROTATION>
|
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