12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

moved RangeFactor to SAM

release/4.3a0
Frank Dellaert 2015-07-12 12:06:55 -07:00
parent fdcb4e8234
commit 3bad6fea67
14 changed files with 165 additions and 194 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/RangeISAMExample_plaza2.cpp
View File

@ -39,7 +39,7 @@
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own

2
gtsam.h
View File

@ -2262,7 +2262,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NoiseModelFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);

145
gtsam/sam/RangeFactor.h Normal file
View File

@ -0,0 +1,145 @@
/* ----------------------------------------------------------------------------
* 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 RangeFactor.h
* @brief Serializable factor induced by a range measurement between two points
*and/or poses
* @date July 2015
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/geometry/concepts.h>
#include <boost/lexical_cast.hpp>
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SAM
*/
template <class A1, class A2 = A1>
struct RangeFactor : public SerializableExpressionFactor2<double, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef SerializableExpressionFactor2<double, A1, A2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
public:
/// default constructor
RangeFactor() {}
RangeFactor(Key key1, Key key2, double measured,
const SharedNoiseModel& model)
: Base(key1, key2, model, measured) {
this->initialize(expression(key1, key2));
}
/// @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)));
}
// Return measurement expression
virtual Expression<double> expression(Key key1, Key key2) const {
Expression<A1> t1_(key1);
Expression<A2> t2_(key2);
return Expression<double>(t1_, &A1::range, t2_);
}
/// print
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "RangeFactor" << std::endl;
Base::print(s, kf);
}
}; // \ RangeFactor
/// traits
template <class A1, class A2>
struct traits<RangeFactor<A1, A2> > : public Testable<RangeFactor<A1, A2> > {};
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SAM
*/
template <class A1, class A2 = A1>
class RangeFactorWithTransform
: public SerializableExpressionFactor2<double, A1, A2> {
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef SerializableExpressionFactor2<double, A1, A2> Base;
A1 body_T_sensor_; ///< The pose of the sensor in the body frame
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(A1, A2)
public:
//// Default constructor
RangeFactorWithTransform() {}
RangeFactorWithTransform(Key key1, Key key2, double measured,
const SharedNoiseModel& model,
const A1& body_T_sensor)
: Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) {
this->initialize(expression(key1, key2));
}
virtual ~RangeFactorWithTransform() {}
/// @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)));
}
// Return measurement expression
virtual Expression<double> expression(Key key1, Key key2) const {
Expression<A1> body_T_sensor__(body_T_sensor_);
Expression<A1> nav_T_body_(key1);
Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
body_T_sensor__);
Expression<A2> t2_(key2);
return Expression<double>(nav_T_sensor_, &A1::range, t2_);
}
/** print contents */
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "RangeFactorWithTransform" << std::endl;
this->body_T_sensor_.print(" sensor pose in body frame: ");
Base::print(s, 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(
"Base", boost::serialization::base_object<Base>(*this));
ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
}
}; // \ RangeFactorWithTransform
/// traits
template <class A1, class A2>
struct traits<RangeFactorWithTransform<A1, A2> >
: public Testable<RangeFactorWithTransform<A1, A2> > {};
} // \ namespace gtsam

5
gtsam/slam/tests/testRangeFactor.cpp → gtsam/sam/tests/testRangeFactor.cpp
View File

@ -16,14 +16,15 @@
* @date Oct 2012
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;

187
gtsam/slam/RangeFactor.h
View File

@ -9,189 +9,14 @@
* -------------------------------------------------------------------------- */
/**
* @file RangeFactor.H
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/concepts.h>
#include <boost/lexical_cast.hpp>
#ifdef _MSC_VER
#pragma message("RangeFactor is now an ExpressionFactor in SAM directory")
#else
#warning "RangeFactor is now an ExpressionFactor in SAM directory"
#endif
namespace gtsam {
/**
* Binary factor for a range measurement
* @addtogroup SLAM
*/
template<class T1, class T2 = T1>
class RangeFactor: public NoiseModelFactor2<T1, T2> {
private:
#include <gtsam/sam/RangeFactor.h>
double measured_; /** measurement */
typedef RangeFactor<T1, T2> This;
typedef NoiseModelFactor2<T1, T2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2)
public:
RangeFactor() {
} /* Default constructor */
RangeFactor(Key key1, Key key2, double measured,
const SharedNoiseModel& model) :
Base(model, key1, key2), measured_(measured) {
}
virtual ~RangeFactor() {
}
/// @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 T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double hx;
hx = t1.range(t2, H1, H2);
return (Vector(1) << hx - measured_).finished();
}
/** 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 << "RangeFactor, range = " << 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_);
}
}; // \ RangeFactor
/// traits
template<class T1, class T2>
struct traits<RangeFactor<T1,T2> > : public Testable<RangeFactor<T1,T2> > {};
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SLAM
*/
template<class POSE, class T2 = POSE>
class RangeFactorWithTransform: public NoiseModelFactor2<POSE, T2> {
private:
double measured_; /** measurement */
POSE body_P_sensor_; ///< The pose of the sensor in the body frame
typedef RangeFactorWithTransform<POSE, T2> This;
typedef NoiseModelFactor2<POSE, T2> Base;
// Concept requirements for this factor
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2)
public:
RangeFactorWithTransform() {
} /* Default constructor */
RangeFactorWithTransform(Key key1, Key key2, double measured,
const SharedNoiseModel& model, const POSE& body_P_sensor) :
Base(model, key1, key2), measured_(measured), body_P_sensor_(
body_P_sensor) {
}
virtual ~RangeFactorWithTransform() {
}
/// @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 POSE& t1, const T2& t2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
double hx;
if (H1) {
Matrix H0;
hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
*H1 = *H1 * H0;
} else {
hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
}
return (Vector(1) << hx - measured_).finished();
}
/** 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
&& body_P_sensor_.equals(e->body_P_sensor_);
}
/** print contents */
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
this->body_P_sensor_.print(" sensor pose in body frame: ");
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_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \ RangeFactorWithTransform
/// traits
template<class T1, class T2>
struct traits<RangeFactorWithTransform<T1, T2> > : public Testable<RangeFactorWithTransform<T1, T2> > {};
} // \ namespace gtsam

2
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Rot2.h>

2
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>

2
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -9,7 +9,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam_unstable/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

2
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -39,7 +39,7 @@
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam_unstable/slam/SmartRangeFactor.h>
// Standard headers, added last, so we know headers above work on their own

2
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -39,7 +39,7 @@
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>

2
gtsam_unstable/gtsam_unstable.h
View File

@ -367,7 +367,7 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NonlinearFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);

2
gtsam_unstable/slam/serialization.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>

2
tests/testGaussianFactorGraphB.cpp
View File

@ -532,7 +532,7 @@ TEST(GaussianFactorGraph, hasConstraints)
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
/* ************************************************************************* */
TEST( GaussianFactorGraph, conditional_sigma_failure) {

2
tests/testSerializationSLAM.cpp
View File

@ -29,7 +29,7 @@
//#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>