Moved RelativeElevationFactor from MastSLAM
Alex Cunningham
parent
4a57ef65d7
commit
4fb9f3b39e
|
|
@ -4,7 +4,7 @@ install(FILES ${slam_headers} DESTINATION include/gtsam_unstable/slam)
|
|||
|
||||
# Components to link tests in this subfolder against
|
||||
set(slam_local_libs
|
||||
#slam_unstable # No sources currently, so no convenience lib
|
||||
slam_unstable
|
||||
slam
|
||||
nonlinear
|
||||
linear
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/**
|
||||
* @file RelativeElevationFactor.cpp
|
||||
*
|
||||
* @date Aug 17, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
RelativeElevationFactor::RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
|
||||
const SharedNoiseModel& model)
|
||||
: Base(model, poseKey, pointKey), measured_(measured)
|
||||
{
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
double hx = pose.z() - point.z();
|
||||
if (H1) {
|
||||
*H1 = zeros(1, 6);
|
||||
// Use bottom row of rotation matrix for derivative of translation
|
||||
(*H1)(0, 3) = pose.rotation().r1().z();
|
||||
(*H1)(0, 4) = pose.rotation().r2().z();
|
||||
(*H1)(0, 5) = pose.rotation().r3().z();
|
||||
}
|
||||
|
||||
if (H2) {
|
||||
*H2 = zeros(1, 3);
|
||||
(*H2)(0, 2) = -1.0;
|
||||
}
|
||||
return Vector_(1, hx - measured_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
|
||||
const This *e = dynamic_cast<const This*> (&expected);
|
||||
return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void RelativeElevationFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||
std::cout << s << "RelativeElevationFactor, relative elevation = " << measured_ << std::endl;
|
||||
Base::print("", keyFormatter);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
/**
|
||||
* @file RelativeElevationFactor.h
|
||||
*
|
||||
* @brief Factor representing a known relative altitude in global frame
|
||||
*
|
||||
* @date Aug 17, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor for a relative elevation. Note that this
|
||||
* factor takes into account only elevation, and corrects for orientation.
|
||||
* Unlike a range factor, the relative elevation is signed, and only affects
|
||||
* the Z coordinate. Measurement function h(pose, pt) = h.z() - pt.z()
|
||||
*
|
||||
* Dimension: 1
|
||||
*
|
||||
* TODO: enable use of a Pose3 for the target as well
|
||||
*/
|
||||
class RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> {
|
||||
private:
|
||||
|
||||
double measured_; /** measurement */
|
||||
|
||||
typedef RelativeElevationFactor This;
|
||||
typedef NoiseModelFactor2<Pose3, Point3> Base;
|
||||
|
||||
public:
|
||||
|
||||
RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
|
||||
|
||||
RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
|
||||
const SharedNoiseModel& model);
|
||||
|
||||
virtual ~RelativeElevationFactor() {}
|
||||
|
||||
/// @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 Point3& point,
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/** return the measured */
|
||||
inline double measured() const { return measured_; }
|
||||
|
||||
/** equals specialized to this factor */
|
||||
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
|
||||
|
||||
/** print contents */
|
||||
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
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_);
|
||||
}
|
||||
}; // RelativeElevationFactor
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,138 @@
|
|||
/**
|
||||
* @file testRelativeElevationFactor.cpp
|
||||
*
|
||||
* @date Aug 17, 2012
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam_unstable/slam/RelativeElevationFactor.h>
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
SharedNoiseModel model1 = noiseModel::Unit::Create(1);
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
||||
const Pose3 pose1(Rot3(), Point3(2.0, 3.0, 4.0));
|
||||
const Pose3 pose2(Rot3::pitch(-M_PI_2), Point3(2.0, 3.0, 4.0));
|
||||
const Pose3 pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), Point3(2.0, 3.0, 4.0));
|
||||
const Point3 point1(3.0, 4.0, 2.0);
|
||||
const gtsam::Key poseKey = 1, pointKey = 2;
|
||||
|
||||
/* ************************************************************************* */
|
||||
LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
|
||||
return LieVector(factor.evaluateError(x, p));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, level_zero_error ) {
|
||||
// Zero error
|
||||
double measured = 2.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, level_positive ) {
|
||||
// Positive meas
|
||||
double measured = 0.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, level_negative ) {
|
||||
// Negative meas
|
||||
double measured = -1.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, rotated_zero_error ) {
|
||||
// Zero error
|
||||
double measured = 2.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, rotated_positive ) {
|
||||
// Positive meas
|
||||
double measured = 0.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(Vector_(1, 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, rotated_negative1 ) {
|
||||
// Negative meas
|
||||
double measured = -1.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testRelativeElevationFactor, rotated_negative2 ) {
|
||||
// Negative meas
|
||||
double measured = -1.0;
|
||||
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
|
||||
Matrix actH1, actH2;
|
||||
EXPECT(assert_equal(Vector_(1, 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
|
||||
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
|
||||
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>(
|
||||
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
EXPECT(assert_equal(expH2, actH2, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue