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

Replaced ChartValue with GenericValue. Now at the point where we are bumping up against missing Jacobians in gtsam types. It feels like progress.

release/4.3a0
Paul Furgale 2014-12-14 12:13:59 +01:00
parent 6b04fee048
commit 7f7c181945
10 changed files with 350 additions and 276 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

438
gtsam/base/ChartValue.h
View File

@ -1,222 +1,222 @@
/* ---------------------------------------------------------------------------- ///* ----------------------------------------------------------------------------
//
* GTSAM Copyright 2010, Georgia Tech Research Corporation, // * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 // * Atlanta, Georgia 30332-0415
* All Rights Reserved // * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) // * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
* See LICENSE for the license information // * See LICENSE for the license information
//
* -------------------------------------------------------------------------- */ // * -------------------------------------------------------------------------- */
//
/* ///*
* @file ChartValue.h // * @file ChartValue.h
* @brief // * @brief
* @date October, 2014 // * @date October, 2014
* @author Michael Bosse, Abel Gawel, Renaud Dube // * @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DerivedValue.h by Duy Nguyen Ta // * based on DerivedValue.h by Duy Nguyen Ta
*/ // */
//
#pragma once #pragma once
//
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
#include <gtsam/base/Manifold.h> //#include <gtsam/base/Manifold.h>
#include <boost/make_shared.hpp> //#include <boost/make_shared.hpp>
//
////////////////// ////////////////////
// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR //// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
#include <boost/pool/singleton_pool.hpp> //#include <boost/pool/singleton_pool.hpp>
//
#ifdef min //#ifdef min
#undef min //#undef min
#endif //#endif
//
#ifdef max //#ifdef max
#undef max //#undef max
#endif //#endif
//
#ifdef ERROR //#ifdef ERROR
#undef ERROR //#undef ERROR
#endif //#endif
////////////////// ////////////////////
//
namespace gtsam { //namespace gtsam {
//
/** ///**
* ChartValue is derived from GenericValue<T> and Chart so that // * ChartValue is derived from GenericValue<T> and Chart so that
* Chart can be zero sized (as in DefaultChart<T>) // * Chart can be zero sized (as in DefaultChart<T>)
* if the Chart is a member variable then it won't ever be zero sized. // * if the Chart is a member variable then it won't ever be zero sized.
*/ // */
template<class T, class Chart_ = DefaultChart<T> > //template<class T, class Chart_ = DefaultChart<T> >
class ChartValue: public GenericValue<T>, public Chart_ { //class GenericValue: public GenericValue<T>, public Chart_ {
//
BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>)); // BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
//
public: //public:
//
typedef T type; // typedef T type;
typedef Chart_ Chart; // typedef Chart_ Chart;
//
public: //public:
//
/// Default Constructor. TODO might not make sense for some types // /// Default Constructor. TODO might not make sense for some types
ChartValue() : // GenericValue() :
GenericValue<T>(T()) { // GenericValue<T>(T()) {
} // }
//
/// Construct froma value // /// Construct froma value
ChartValue(const T& value) : // GenericValue(const T& value) :
GenericValue<T>(value) { // GenericValue<T>(value) {
} // }
//
/// Construct from a value and initialize the chart // /// Construct from a value and initialize the chart
template<typename C> // template<typename C>
ChartValue(const T& value, C chart_initializer) : // GenericValue(const T& value, C chart_initializer) :
GenericValue<T>(value), Chart(chart_initializer) { // GenericValue<T>(value), Chart(chart_initializer) {
} // }
//
/// Destructor // /// Destructor
virtual ~ChartValue() { // virtual ~ChartValue() {
} // }
//
/** // /**
* Create a duplicate object returned as a pointer to the generic Value interface. // * Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. // * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
* The result must be deleted with Value::deallocate_, not with the 'delete' operator. // * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
*/ // */
virtual Value* clone_() const { // virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc(); // void *place = boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in // ChartValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
return ptr; // return ptr;
} // }
//
/** // /**
* Destroy and deallocate this object, only if it was originally allocated using clone_(). // * Destroy and deallocate this object, only if it was originally allocated using clone_().
*/ // */
virtual void deallocate_() const { // virtual void deallocate_() const {
this->~ChartValue(); // Virtual destructor cleans up the derived object // this->~ChartValue(); // Virtual destructor cleans up the derived object
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool // boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
} // }
//
/** // /**
* Clone this value (normal clone on the heap, delete with 'delete' operator) // * Clone this value (normal clone on the heap, delete with 'delete' operator)
*/ // */
virtual boost::shared_ptr<Value> clone() const { // virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<ChartValue>(*this); // return boost::make_shared<ChartValue>(*this);
} // }
//
/// Chart Value interface version of retract // /// Chart Value interface version of retract
virtual Value* retract_(const Vector& delta) const { // virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class using the retract trait function // // Call retract on the derived class using the retract trait function
const T retractResult = Chart::retract(GenericValue<T>::value(), delta); // const T retractResult = Chart::retract(GenericValue<T>::value(), delta);
//
// Create a Value pointer copy of the result // // Create a Value pointer copy of the result
void* resultAsValuePlace = // void* resultAsValuePlace =
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc(); // boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult, // Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult,
static_cast<const Chart&>(*this)); // static_cast<const Chart&>(*this));
//
// Return the pointer to the Value base class // // Return the pointer to the Value base class
return resultAsValue; // return resultAsValue;
} // }
//
/// Generic Value interface version of localCoordinates // /// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const { // virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a templated generic class pointer // // Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 = // const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2); // static_cast<const GenericValue<T>&>(value2);
//
// Return the result of calling localCoordinates trait on the derived class // // Return the result of calling localCoordinates trait on the derived class
return Chart::local(GenericValue<T>::value(), genericValue2.value()); // return Chart::local(GenericValue<T>::value(), genericValue2.value());
} // }
//
/// Non-virtual version of retract // /// Non-virtual version of retract
ChartValue retract(const Vector& delta) const { // GenericValue retract(const Vector& delta) const {
return ChartValue(Chart::retract(GenericValue<T>::value(), delta), // return GenericValue(Chart::retract(GenericValue<T>::value(), delta),
static_cast<const Chart&>(*this)); // static_cast<const Chart&>(*this));
} // }
//
/// Non-virtual version of localCoordinates // /// Non-virtual version of localCoordinates
Vector localCoordinates(const ChartValue& value2) const { // Vector localCoordinates(const GenericValue& value2) const {
return localCoordinates_(value2); // return localCoordinates_(value2);
} // }
//
/// Return run-time dimensionality // /// Return run-time dimensionality
virtual size_t dim() const { // virtual size_t dim() const {
// need functional form here since the dimension may be dynamic // // need functional form here since the dimension may be dynamic
return Chart::getDimension(GenericValue<T>::value()); // return Chart::getDimension(GenericValue<T>::value());
} // }
//
/// Assignment operator // /// Assignment operator
virtual Value& operator=(const Value& rhs) { // virtual Value& operator=(const Value& rhs) {
// Cast the base class Value pointer to a derived class pointer // // Cast the base class Value pointer to a derived class pointer
const ChartValue& derivedRhs = static_cast<const ChartValue&>(rhs); // const GenericValue& derivedRhs = static_cast<const ChartValue&>(rhs);
//
// Do the assignment and return the result // // Do the assignment and return the result
*this = ChartValue(derivedRhs); // calls copy constructor // *this = GenericValue(derivedRhs); // calls copy constructor
return *this; // return *this;
} // }
//
protected: //protected:
//
// implicit assignment operator for (const ChartValue& rhs) works fine here // // implicit assignment operator for (const ChartValue& rhs) works fine here
/// Assignment operator, protected because only the Value or DERIVED // /// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used. // /// assignment operators should be used.
// DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) { // // DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// // Nothing to do, do not call base class assignment operator // // // Nothing to do, do not call base class assignment operator
// return *this; // // return *this;
// } // // }
//
private: //private:
//
/// Fake Tag struct for singleton pool allocator. In fact, it is never used! // /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag { // struct PoolTag {
}; // };
//
private: //private:
//
/** Serialization function */ // /** Serialization function */
friend class boost::serialization::access; // friend class boost::serialization::access;
template<class ARCHIVE> // template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { // void serialize(ARCHIVE & ar, const unsigned int version) {
// ar & boost::serialization::make_nvp("value",); // // ar & boost::serialization::make_nvp("value",);
// todo: implement a serialization for charts // // todo: implement a serialization for charts
ar // ar
& boost::serialization::make_nvp("GenericValue", // & boost::serialization::make_nvp("GenericValue",
boost::serialization::base_object<GenericValue<T> >(*this)); // boost::serialization::base_object<GenericValue<T> >(*this));
} // }
//
}; //};
//
// Define //// Define
namespace traits { //namespace traits {
//
/// The dimension of a ChartValue is the dimension of the chart ///// The dimension of a GenericValue is the dimension of the chart
template<typename T, typename Chart> //template<typename T, typename Chart>
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> { //struct dimension<GenericValue<T, Chart> > : public dimension<Chart> {
// TODO Frank thinks dimension is a property of type, chart should conform // // TODO Frank thinks dimension is a property of type, chart should conform
}; //};
//
} // \ traits //} // \ traits
//
/// Get the chart from a Value ///// Get the chart from a Value
template<typename Chart> //template<typename Chart>
const Chart& Value::getChart() const { //const Chart& Value::getChart() const {
return dynamic_cast<const Chart&>(*this); // return dynamic_cast<const Chart&>(*this);
} //}
//
/// Convenience function that can be used to make an expression to convert a value to a chart ///// Convenience function that can be used to make an expression to convert a value to a chart
template<typename T> //template<typename T>
ChartValue<T> convertToChartValue(const T& value, //GenericValue<T> convertToChartValue(const T& value,
boost::optional< // boost::optional<
Eigen::Matrix<double, traits_x<T>::dimension, // Eigen::Matrix<double, traits_x<T>::dimension,
traits_x<T>::dimension>&> H = boost::none) { // traits_x<T>::dimension>&> H = boost::none) {
if (H) { // if (H) {
*H = Eigen::Matrix<double, traits_x<T>::dimension, // *H = Eigen::Matrix<double, traits_x<T>::dimension,
traits_x<T>::dimension>::Identity(); // traits_x<T>::dimension>::Identity();
} // }
return ChartValue<T>(value); // return GenericValue<T>(value);
} //}
//
} /* namespace gtsam */ //} /* namespace gtsam */

109
gtsam/base/GenericValue.h
View File

@ -23,6 +23,7 @@
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
#include <cmath> #include <cmath>
#include <iostream> #include <iostream>
#include <boost/make_shared.hpp>
namespace gtsam { namespace gtsam {
@ -144,18 +145,108 @@ public:
traits::print<T>()(value_, str); traits::print<T>()(value_, str);
} }
// Serialization below: /**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
* The result must be deleted with Value::deallocate_, not with the 'delete' operator.
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
return ptr;
}
friend class boost::serialization::access; /**
template<class ARCHIVE> * Destroy and deallocate this object, only if it was originally allocated using clone_().
void serialize(ARCHIVE & ar, const unsigned int version) { */
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); virtual void deallocate_() const {
ar & BOOST_SERIALIZATION_NVP(value_); this->~GenericValue(); // Virtual destructor cleans up the derived object
} boost::singleton_pool<PoolTag, sizeof(GenericValue)>::free((void*) this); // Release memory from pool
}
protected: /**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<GenericValue>(*this);
}
// Assignment operator for this class not needed since GenericValue<T> is an abstract class /// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class using the retract trait function
const T retractResult = traits_x<T>::Retract(GenericValue<T>::value(), delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace =
boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult);
// Return the pointer to the Value base class
return resultAsValue;
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
// Return the result of calling localCoordinates trait on the derived class
return traits_x<T>::Local(GenericValue<T>::value(), genericValue2.value());
}
/// Non-virtual version of retract
GenericValue retract(const Vector& delta) const {
return GenericValue(traits_x<T>::Retract(GenericValue<T>::value(), delta));
}
/// Non-virtual version of localCoordinates
Vector localCoordinates(const GenericValue& value2) const {
return localCoordinates_(value2);
}
/// Return run-time dimensionality
virtual size_t dim() const {
// need functional form here since the dimension may be dynamic
return traits_x<T>::GetDimension(GenericValue<T>::value());
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
// Cast the base class Value pointer to a derived class pointer
const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
// Do the assignment and return the result
*this = GenericValue(derivedRhs); // calls copy constructor
return *this;
}
protected:
// implicit assignment operator for (const GenericValue& rhs) works fine here
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
// DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// // Nothing to do, do not call base class assignment operator
// return *this;
// }
private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag {
};
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("GenericValue",
boost::serialization::base_object<GenericValue<T> >(*this));
ar & boost::serialization::make_nvp("value", value_);
}
}; };

2
gtsam/base/Manifold.h
View File

@ -357,6 +357,8 @@
// //
//} // \ namespace gtsam //} // \ namespace gtsam
// //
// TODO(ASL) Remove these and fix the compiler errors.
///** ///**
// * Macros for using the ManifoldConcept // * Macros for using the ManifoldConcept
// * - An instantiation for use inside unit tests // * - An instantiation for use inside unit tests

10
gtsam/base/concepts.h
View File

@ -63,10 +63,10 @@ struct Manifold {
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
// For Testable // For Testable
void Print(const ManifoldType& m) { static void Print(const ManifoldType& m, const std::string& str = "") {
m.print(); m.print(str);
} }
bool Equals(const ManifoldType& m1, static bool Equals(const ManifoldType& m1,
const ManifoldType& m2, const ManifoldType& m2,
double tol = 1e-8) { double tol = 1e-8) {
return m1.equals(m2, tol); return m1.equals(m2, tol);
@ -113,10 +113,10 @@ struct LieGroup {
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
// For Testable // For Testable
void Print(const ManifoldType& m) { static void Print(const ManifoldType& m, const std::string& str = "") {
m.print(); m.print();
} }
bool Equals(const ManifoldType& m1, static bool Equals(const ManifoldType& m1,
const ManifoldType& m2, const ManifoldType& m2,
double tol = 1e-8) { double tol = 1e-8) {
return m1.equals(m2, tol); return m1.equals(m2, tol);

5
gtsam/geometry/PinholeCamera.h
View File

@ -486,7 +486,8 @@ private:
}; };
template<>
struct traits_x<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {}; template<typename Calibration>
struct traits_x< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam } // \ gtsam

2
gtsam/geometry/Pose3.h
View File

@ -53,7 +53,7 @@ private:
public: public:
enum { dimension = 3 }; enum { dimension = 6 };
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{

28
gtsam/nonlinear/Values-inl.h
View File

@ -292,37 +292,13 @@ namespace gtsam {
// insert a plain value using the default chart // insert a plain value using the default chart
template<typename ValueType> template<typename ValueType>
void Values::insert(Key j, const ValueType& val) { void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val))); insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
} }
// insert with custom chart type
template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
}
// overloaded insert with chart initializer
template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val, Chart chart) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
// update with default chart // update with default chart
template <typename ValueType> template <typename ValueType>
void Values::update(Key j, const ValueType& val) { void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val))); update(j, static_cast<const Value&>(GenericValue<ValueType >(val)));
}
// update with custom chart
template <typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
}
// update with chart initializer, /todo: perhaps there is a way to init chart from old value...
template<typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val, Chart chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
} }
} }

2
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -76,7 +76,7 @@ TEST (Serialization, TemplatedValues) {
std::cout << __LINE__ << std::endl; std::cout << __LINE__ << std::endl;
EXPECT(equalsObj(pt3)); EXPECT(equalsObj(pt3));
std::cout << __LINE__ << std::endl; std::cout << __LINE__ << std::endl;
ChartValue<Point3> chv1(pt3); GenericValue<Point3> chv1(pt3);
std::cout << __LINE__ << std::endl; std::cout << __LINE__ << std::endl;
EXPECT(equalsObj(chv1)); EXPECT(equalsObj(chv1));
std::cout << __LINE__ << std::endl; std::cout << __LINE__ << std::endl;

20
gtsam/slam/BetweenFactor.h
View File

@ -43,8 +43,9 @@ namespace gtsam {
VALUE measured_; /** The measurement */ VALUE measured_; /** The measurement */
/** concept check by type */ /** concept check by type */
GTSAM_CONCEPT_LIE_TYPE(T) // TODO(ASL) Reenable
GTSAM_CONCEPT_TESTABLE_TYPE(T) //GTSAM_CONCEPT_LIE_TYPE(T)
//GTSAM_CONCEPT_TESTABLE_TYPE(T)
public: public:
@ -74,14 +75,14 @@ namespace gtsam {
std::cout << s << "BetweenFactor(" std::cout << s << "BetweenFactor("
<< keyFormatter(this->key1()) << "," << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n"; << keyFormatter(this->key2()) << ")\n";
traits::print<T>()(measured_, " measured: "); traits_x<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected); const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol); return e != NULL && Base::equals(*e, tol) && traits_x<T>::Equals(this->measured_, e->measured_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
@ -90,10 +91,13 @@ namespace gtsam {
Vector evaluateError(const T& p1, const T& p2, Vector evaluateError(const T& p1, const T& p2,
boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
T hx = p1.between(p2, H1, H2); // h(x) T hx = traits_x<T>::Between(p1, p2, H1, H2); // h(x)
DefaultChart<T> chart;
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return chart.local(measured_, hx); OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
Vector rval = traits_x<T>::Local(measured_, hx, boost::none, Hlocal);
(*H1) = ((*Hlocal) * (*H1)).eval();
(*H2) = ((*Hlocal) * (*H2)).eval();
return rval;
} }
/** return the measured */ /** return the measured */
@ -131,7 +135,7 @@ namespace gtsam {
/** Syntactic sugar for constrained version */ /** Syntactic sugar for constrained version */
BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
BetweenFactor<VALUE>(key1, key2, measured, BetweenFactor<VALUE>(key1, key2, measured,
noiseModel::Constrained::All(DefaultChart<VALUE>::getDimension(measured), fabs(mu))) noiseModel::Constrained::All(traits_x<VALUE>::GetDimension(measured), fabs(mu)))
{} {}
private: private:

10
gtsam/slam/PriorFactor.h
View File

@ -67,24 +67,24 @@ namespace gtsam {
/** print */ /** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits::print<T>()(prior_, " prior mean: "); traits_x<T>::Print(prior_, " prior mean: ");
this->noiseModel_->print(" noise model: "); this->noiseModel_->print(" noise model: ");
} }
/** equals */ /** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected); const This* e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol); return e != NULL && Base::equals(*e, tol) && traits_x<T>::Equals(prior_, e->prior_, tol);
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const { Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
DefaultChart<T> chart; if (H) (*H) = eye(traits_x<T>::GetDimension(p));
if (H) (*H) = eye(chart.getDimension(p));
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
return chart.local(prior_,p); // TODO(ASL) Add Jacobians.
return traits_x<T>::Local(prior_,p);
} }
const VALUE & prior() const { return prior_; } const VALUE & prior() const { return prior_; }