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

cleaning up

release/4.3a0
Frank Dellaert 2015-07-11 18:29:39 -07:00
parent 12bbe2f911
commit 5052eb2c64
4 changed files with 16 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam.h
View File

@ -2283,9 +2283,9 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
#include <gtsam/sam/BearingFactor.h>
template<POSE, POINT, ROTATION>
template<POSE, POINT, MEASURED>
virtual class BearingFactor : gtsam::NoiseModelFactor {
BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
BearingFactor(size_t key1, size_t key2, const MEASURED& measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
@ -2295,11 +2295,11 @@ typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFa
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, ROTATION>
template<POSE, POINT, MEASURED>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
BearingRangeFactor(size_t poseKey, size_t pointKey, const MEASURED& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
pair<ROTATION, double> measured() const;
pair<MEASURED, double> measured() const;
// enabling serialization functionality
void serialize() const;

25
gtsam/sam/BearingFactor.h
View File

@ -11,7 +11,7 @@
/**
* @file BearingFactor.h
* @brief Serializable factor induced by a bearing measurement on a point from a pose
* @brief Serializable factor induced by a bearing measurement of a point from a given pose
* @date July 2015
* @author Frank Dellaert
**/
@ -20,7 +20,6 @@
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
@ -28,37 +27,35 @@ namespace gtsam {
* Binary factor for a bearing measurement
* @addtogroup SAM
*/
template <typename Pose, typename Point, typename Measured>
struct BearingFactor : public SerializableExpressionFactor<Measured> {
GTSAM_CONCEPT_POSE_TYPE(Pose)
template <typename POSE, typename POINT, typename T>
struct BearingFactor : public SerializableExpressionFactor<T> {
/// default constructor
BearingFactor() {}
/// primary constructor
BearingFactor(Key poseKey, Key pointKey, const Measured& measured, const SharedNoiseModel& model)
: SerializableExpressionFactor<Measured>(model, measured) {
BearingFactor(Key poseKey, Key pointKey, const T& measured, const SharedNoiseModel& model)
: SerializableExpressionFactor<T>(model, measured) {
this->keys_.push_back(poseKey);
this->keys_.push_back(pointKey);
this->initialize(expression());
}
// Return measurement expression
virtual Expression<Measured> expression() const {
return Expression<Measured>(Expression<Pose>(this->keys_[0]), &Pose::bearing,
Expression<Point>(this->keys_[1]));
virtual Expression<T> expression() const {
return Expression<T>(Expression<POSE>(this->keys_[0]), &POSE::bearing,
Expression<POINT>(this->keys_[1]));
}
/// print
void print(const std::string& s = "", const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "BearingFactor" << std::endl;
SerializableExpressionFactor<Measured>::print(s, kf);
SerializableExpressionFactor<T>::print(s, kf);
}
}; // BearingFactor
/// traits
template <class Pose, class Point, class Measured>
struct traits<BearingFactor<Pose, Point, Measured> >
: public Testable<BearingFactor<Pose, Point, Measured> > {};
template <class POSE, class POINT, class T>
struct traits<BearingFactor<POSE, POINT, T> > : public Testable<BearingFactor<POSE, POINT, T> > {};
} // namespace gtsam

6
gtsam_unstable/slam/serialization.cpp
View File

@ -11,7 +11,6 @@
#include <gtsam/base/serialization.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
@ -80,9 +79,6 @@ typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraP
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
typedef BearingFactor<Pose3, Point3, Rot3> BearingFactor3D;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
@ -185,8 +181,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D");
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");

16
tests/testSerializationSLAM.cpp
View File

@ -22,7 +22,6 @@
#include <tests/smallExample.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
@ -106,9 +105,6 @@ typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraP
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
typedef BearingFactor<Pose2, Point2, Rot2> BearingFactor2D;
typedef BearingFactor<Pose3, Point3, Rot3> BearingFactor3D;
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
@ -217,8 +213,6 @@ BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleC
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
BOOST_CLASS_EXPORT_GUID(BearingFactor2D, "gtsam::BearingFactor2D");
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2");
@ -393,8 +387,6 @@ TEST (testSerializationSLAM, factors) {
RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1);
BearingFactor2D bearingFactor2D(a08, a03, rot2, model1);
BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
GenericProjectionFactorCal3_S2 genericProjectionFactorCal3_S2(point2, model2, a09, a05, boost::make_shared<Cal3_S2>(cal3_s2));
@ -456,8 +448,6 @@ TEST (testSerializationSLAM, factors) {
graph += rangeFactorCalibratedCamera;
graph += rangeFactorSimpleCamera;
graph += bearingFactor2D;
graph += bearingRangeFactor2D;
graph += genericProjectionFactorCal3_S2;
@ -524,8 +514,6 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
EXPECT(equalsObj<BearingFactor2D>(bearingFactor2D));
EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
EXPECT(equalsObj<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
@ -592,8 +580,6 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
EXPECT(equalsXML<BearingFactor2D>(bearingFactor2D));
EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
EXPECT(equalsXML<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));
@ -660,8 +646,6 @@ TEST (testSerializationSLAM, factors) {
EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
EXPECT(equalsBinary<BearingFactor2D>(bearingFactor2D));
EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));
EXPECT(equalsBinary<GenericProjectionFactorCal3_S2>(genericProjectionFactorCal3_S2));