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

Moved BearingRangeFactor to SAM

release/4.3a0
Frank Dellaert 2015-07-12 18:57:26 -07:00
parent 9c525c2e7f
commit 30435da070
15 changed files with 276 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/PlanarSLAMExample.cpp
View File

@ -42,7 +42,7 @@
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.

2
examples/SolverComparer.cpp
View File

@ -32,7 +32,7 @@
*/
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>

2
gtsam.h
View File

@ -2290,7 +2290,7 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const BEARING& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);

148
gtsam/sam/BearingRangeFactor.h Normal file
View File

@ -0,0 +1,148 @@
/* ----------------------------------------------------------------------------
* 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 BearingRangeFactor.h
* @date Apr 1, 2010
* @author Kai Ni
* @brief a single factor contains both the bearing and the range to prevent
* handle to pair bearing and range factors
*/
#pragma once
#include <gtsam/nonlinear/SerializableExpressionFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
// forward declaration of Range functor, assumed partially specified
template <typename A1, typename A2>
struct Range;
template <typename B, typename R>
struct BearingRange : public ProductManifold<B, R> {
BearingRange() {}
BearingRange(const ProductManifold<B, R>& br) : ProductManifold<B, R>(br) {}
BearingRange(const B& b, const R& r) : ProductManifold<B, R>(b, r) {}
private:
/// Serialization function
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("bearing", this->first);
ar& boost::serialization::make_nvp("range", this->second);
}
friend class boost::serialization::access;
};
template <typename B, typename R>
struct traits<BearingRange<B, R> >
: internal::ManifoldTraits<BearingRange<B, R> > {
static void Print(const BearingRange<B, R>& m, const std::string& str = "") {
traits<B>::Print(m.first, str);
traits<R>::Print(m.second, str);
}
static bool Equals(const BearingRange<B, R>& m1, const BearingRange<B, R>& m2,
double tol = 1e-8) {
return traits<B>::Equals(m1.first, m2.first, tol) &&
traits<R>::Equals(m1.second, m2.second, tol);
}
};
/**
* Binary factor for a bearing/range measurement
* @addtogroup SLAM
*/
template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public SerializableExpressionFactor2<BearingRange<B, R>, A1, A2> {
public:
typedef BearingRange<B, R> T;
typedef SerializableExpressionFactor2<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This;
typedef boost::shared_ptr<This> shared_ptr;
private:
/** concept check by type */
BOOST_CONCEPT_ASSERT((IsTestable<B>));
BOOST_CONCEPT_ASSERT((IsTestable<R>));
public:
/// default constructor
BearingRangeFactor() {}
/// primary constructor
BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
const R measuredRange, const SharedNoiseModel& model)
: Base(key1, key2, model, T(measuredBearing, measuredRange)) {
this->initialize(expression(key1, key2));
}
virtual ~BearingRangeFactor() {}
/// @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<T> expression(Key key1, Key key2) const {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(This::Predict, a1_, a2_);
}
/** Print */
virtual void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor(" << keyFormatter(this->keys_[0])
<< "," << keyFormatter(this->keys_[1]) << ")\n";
traits<B>::Print(this->measured_.first, "measured bearing: ");
traits<R>::Print(this->measured_.second, "measured range: ");
this->noiseModel_->print("noise model:\n");
}
/// Prediction function that stacks measurements
static T Predict(const A1& pose, const A2& point,
typename MakeOptionalJacobian<T, A1>::type H1,
typename MakeOptionalJacobian<T, A2>::type H2) {
typename MakeJacobian<B, A1>::type HB1;
typename MakeJacobian<B, A2>::type HB2;
typename MakeJacobian<R, A1>::type HR1;
typename MakeJacobian<R, A2>::type HR2;
B b = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
R r = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
if (H1) *H1 << HB1, HR1;
if (H2) *H2 << HB2, HR2;
return T(b, r);
}
}; // BearingRangeFactor
/// traits
template <typename A1, typename A2, typename B, typename R>
struct traits<BearingRangeFactor<A1, A2, B, R> >
: public Testable<BearingRangeFactor<A1, A2, B, R> > {};
} // namespace gtsam

109
gtsam/sam/tests/testBearingRangeFactor.cpp Normal file
View File

@ -0,0 +1,109 @@
/* ----------------------------------------------------------------------------
* 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 testBearingRangeFactor.cpp
* @brief Unit tests for BearingRangeFactor Class
* @author Frank Dellaert
* @date July 2015
*/
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace serializationTestHelpers;
Key poseKey(1);
Key pointKey(2);
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
BearingRangeFactor2D::T measurement2D(1, 2);
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
BearingRangeFactor3D::T measurement3D(Pose3().bearing(Point3(1, 0, 0)), 0);
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
BearingRangeFactor3D factor3D(poseKey, pointKey,
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization2D) {
EXPECT(equalsObj<BearingRangeFactor2D>(factor2D));
EXPECT(equalsXML<BearingRangeFactor2D>(factor2D));
EXPECT(equalsBinary<BearingRangeFactor2D>(factor2D));
}
/* ************************************************************************* */
TEST(BearingRangeFactor, 2D) {
// Serialize the factor
std::string serialized = serializeXML(factor2D);
// And de-serialize it
BearingRangeFactor2D factor;
deserializeXML(serialized, factor);
// Set the linearization point
Values values;
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(BearingRangeFactor, Serialization3D) {
EXPECT(equalsObj<BearingRangeFactor3D>(factor3D));
EXPECT(equalsXML<BearingRangeFactor3D>(factor3D));
EXPECT(equalsBinary<BearingRangeFactor3D>(factor3D));
}
/* ************************************************************************* */
TEST(BearingRangeFactor, 3D) {
// Serialize the factor
std::string serialized = serializeXML(factor3D);
// And de-serialize it
BearingRangeFactor3D factor;
deserializeXML(serialized, factor);
// Set the linearization point
Values values;
values.insert(poseKey, Pose3());
values.insert(pointKey, Point3(1, 0, 0));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
values, 1e-7, 1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

131
gtsam/slam/BearingRangeFactor.h
View File

@ -9,130 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @file BearingRangeFactor.h
* @date Apr 1, 2010
* @author Kai Ni
* @brief a single factor contains both the bearing and the range to prevent
* handle to pair bearing and range factors
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Testable.h>
#ifdef _MSC_VER
#pragma message( \
"BearingRangeFactor is now an ExpressionFactor in SAM directory")
#else
#warning "BearingRangeFactor is now an ExpressionFactor in SAM directory"
#endif
#include <boost/concept/assert.hpp>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
// forward declaration of Range functor, assumed partially specified
template <typename A1, typename A2>
struct Range;
/**
* Binary factor for a bearing/range measurement
* @addtogroup SLAM
*/
template <typename A1, typename A2,
typename BEARING = typename Bearing<A1, A2>::result_type,
typename RANGE = typename Range<A1, A2>::result_type>
class BearingRangeFactor : public NoiseModelFactor2<A1, A2> {
public:
typedef BearingRangeFactor<A1, A2> This;
typedef NoiseModelFactor2<A1, A2> Base;
typedef boost::shared_ptr<This> shared_ptr;
private:
// the measurement
BEARING measuredBearing_;
RANGE measuredRange_;
/** concept check by type */
BOOST_CONCEPT_ASSERT((IsTestable<BEARING>));
BOOST_CONCEPT_ASSERT((IsTestable<RANGE>));
public:
BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(Key poseKey, Key pointKey, const BEARING& measuredBearing,
const RANGE measuredRange, const SharedNoiseModel& model)
: Base(model, poseKey, pointKey),
measuredBearing_(measuredBearing),
measuredRange_(measuredRange) {}
virtual ~BearingRangeFactor() {}
/// @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)));
}
/** Print */
virtual void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measuredBearing_.print("measured bearing: ");
std::cout << "measured range: " << measuredRange_ << std::endl;
this->noiseModel_->print("noise model:\n");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, RANGE tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) &&
fabs(this->measuredRange_ - e->measuredRange_) < tol &&
this->measuredBearing_.equals(e->measuredBearing_, tol);
}
/** h(x)-z -> between(z,h(x)) for BEARING manifold */
Vector evaluateError(const A1& pose, const A2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
typename MakeJacobian<BEARING, A1>::type HB1;
typename MakeJacobian<BEARING, A2>::type HB2;
typename MakeJacobian<RANGE, A1>::type HR1;
typename MakeJacobian<RANGE, A2>::type HR2;
BEARING y1 = Bearing<A1, A2>()(pose, point, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
Vector e1 = traits<BEARING>::Logmap(traits<BEARING>::Between(measuredBearing_,y1));
RANGE y2 = Range<A1, A2>()(pose, point, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
Vector e2 = traits<RANGE>::Logmap(traits<RANGE>::Between(measuredRange_, y2));
if (H1) {
H1->resize(HB1.RowsAtCompileTime + HR1.RowsAtCompileTime, HB1.ColsAtCompileTime);
*H1 << HB1, HR1;
}
if (H2) {
H2->resize(HB2.RowsAtCompileTime + HR2.RowsAtCompileTime, HB2.ColsAtCompileTime);
*H2 << HB2, HR2;
}
return concatVectors(2, &e1, &e2);
}
/** return the measured */
const std::pair<BEARING, RANGE> measured() const {
return std::make_pair(measuredBearing_, measuredRange_);
}
private:
/** Serialization function */
friend typename boost::serialization::access;
template <typename 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(measuredBearing_);
ar& BOOST_SERIALIZATION_NVP(measuredRange_);
}
}; // BearingRangeFactor
} // namespace gtsam
#include <gtsam/sam/BearingRangeFactor.h>

2
gtsam/slam/dataset.cpp
View File

@ -15,7 +15,7 @@
* @brief utility functions for loading datasets
*/
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Point3.h>

2
gtsam_unstable/slam/serialization.cpp
View File

@ -11,7 +11,7 @@
#include <gtsam/base/serialization.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/GeneralSFMFactor.h>

2
gtsam_unstable/slam/tests/testSerialization.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <stdlib.h>
#include <fstream>

2
tests/testGaussianISAM2.cpp
View File

@ -9,7 +9,7 @@
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h>

2
tests/testGaussianJunctionTreeB.cpp
View File

@ -16,7 +16,7 @@
*/
#include <tests/smallExample.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>

2
tests/testMarginals.cpp
View File

@ -31,7 +31,7 @@
// add in headers for specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/Marginals.h>

2
tests/testNonlinearISAM.cpp
View File

@ -6,7 +6,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearISAM.h>

2
tests/testSerializationSLAM.cpp
View File

@ -22,7 +22,7 @@
#include <tests/smallExample.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/GeneralSFMFactor.h>

2
timing/timeIncremental.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>