moved BearingFactor to sam
Frank Dellaert
parent
76d478c0e0
commit
98ba94c748
2
gtsam.h
2
gtsam.h
|
|
@ -2282,7 +2282,7 @@ typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
|
|||
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
|
||||
|
||||
|
||||
#include <gtsam/slam/BearingFactor.h>
|
||||
#include <gtsam/sam/BearingFactor.h>
|
||||
template<POSE, POINT, ROTATION>
|
||||
virtual class BearingFactor : gtsam::NoiseModelFactor {
|
||||
BearingFactor(size_t key1, size_t key2, const ROTATION& measured, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,102 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 BearingFactor.H
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor for a bearing measurement
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
|
||||
class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
|
||||
private:
|
||||
|
||||
typedef POSE Pose;
|
||||
typedef ROTATION Rot;
|
||||
typedef POINT Point;
|
||||
|
||||
typedef BearingFactor<POSE, POINT> This;
|
||||
typedef NoiseModelFactor2<POSE, POINT> Base;
|
||||
|
||||
Rot measured_; /** measurement */
|
||||
|
||||
/** concept check by type */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
|
||||
GTSAM_CONCEPT_POSE_TYPE(Pose)
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor for serialization/testing only */
|
||||
BearingFactor() {}
|
||||
|
||||
/** primary constructor */
|
||||
BearingFactor(Key poseKey, Key pointKey, const Rot& measured,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, poseKey, pointKey), measured_(measured) {
|
||||
}
|
||||
|
||||
virtual ~BearingFactor() {}
|
||||
|
||||
/// @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 -> between(z,h(x)) for Rot2 manifold */
|
||||
Vector evaluateError(const Pose& pose, const Point& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Rot hx = pose.bearing(point, H1, H2);
|
||||
return Rot::Logmap(measured_.between(hx));
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
const Rot& measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** equals */
|
||||
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) && this->measured_.equals(e->measured_, tol);
|
||||
}
|
||||
|
||||
/** print contents */
|
||||
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "BearingFactor, bearing = ";
|
||||
measured_.print();
|
||||
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_);
|
||||
}
|
||||
|
||||
}; // BearingFactor
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -16,87 +16,11 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#ifdef _MSC_VER
|
||||
#pragma message("BearingFactor is now an ExpressionFactor in sam")
|
||||
#else
|
||||
#warning "BearingFactor is now an ExpressionFactor in sam"
|
||||
#endif
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor for a bearing measurement
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
|
||||
class BearingFactor: public NoiseModelFactor2<POSE, POINT> {
|
||||
private:
|
||||
|
||||
typedef POSE Pose;
|
||||
typedef ROTATION Rot;
|
||||
typedef POINT Point;
|
||||
|
||||
typedef BearingFactor<POSE, POINT> This;
|
||||
typedef NoiseModelFactor2<POSE, POINT> Base;
|
||||
|
||||
Rot measured_; /** measurement */
|
||||
|
||||
/** concept check by type */
|
||||
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
|
||||
GTSAM_CONCEPT_POSE_TYPE(Pose)
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor for serialization/testing only */
|
||||
BearingFactor() {}
|
||||
|
||||
/** primary constructor */
|
||||
BearingFactor(Key poseKey, Key pointKey, const Rot& measured,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, poseKey, pointKey), measured_(measured) {
|
||||
}
|
||||
|
||||
virtual ~BearingFactor() {}
|
||||
|
||||
/// @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 -> between(z,h(x)) for Rot2 manifold */
|
||||
Vector evaluateError(const Pose& pose, const Point& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Rot hx = pose.bearing(point, H1, H2);
|
||||
return Rot::Logmap(measured_.between(hx));
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
const Rot& measured() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** equals */
|
||||
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) && this->measured_.equals(e->measured_, tol);
|
||||
}
|
||||
|
||||
/** print contents */
|
||||
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
std::cout << s << "BearingFactor, bearing = ";
|
||||
measured_.print();
|
||||
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_);
|
||||
}
|
||||
|
||||
}; // BearingFactor
|
||||
|
||||
} // namespace gtsam
|
||||
#include <gtsam/sam/BearingFactor.h>
|
||||
|
|
|
|||
Loading…
Reference in New Issue