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

Merged in feature/sam_sfm_directories (pull request #174) 

[CI Skip] First Push to SAM
release/4.3a0
Frank Dellaert 2015-07-15 23:03:30 -07:00
parent adc653c281 10a653ad7f
commit 07577189d9
74 changed files with 3108 additions and 2492 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3574
.cproject
View File

File diff suppressed because it is too large Load Diff

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/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
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>

40
gtsam.h
View File

@ -1,4 +1,5 @@
/**
* GTSAM Wrap Module Definition
*
* These are the current classes available through the matlab toolbox interface,
@ -156,7 +157,7 @@ virtual class Value {
size_t dim() const;
};
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
LieScalar();
@ -185,7 +186,7 @@ class LieScalar {
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>
class LieVector {
// Standard constructors
LieVector();
@ -217,7 +218,7 @@ class LieVector {
void serialize() const;
};
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieMatrix.h>
class LieMatrix {
// Standard constructors
LieMatrix();
@ -2262,7 +2263,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);
@ -2272,20 +2273,16 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
template<POSE, POINT, ROTATION>
#include <gtsam/sam/BearingFactor.h>
template<POSE, POINT, BEARING>
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 BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
@ -2293,19 +2290,18 @@ virtual class BearingFactor : gtsam::NoiseModelFactor {
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/slam/BearingRangeFactor.h>
template<POSE, POINT, ROTATION>
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
pair<ROTATION, double> measured() const;
BearingRangeFactor(size_t poseKey, size_t pointKey,
const BEARING& measuredBearing, const RANGE& measuredRange,
const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingRangeFactor2D;
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
#include <gtsam/slam/ProjectionFactor.h>

3
gtsam/CMakeLists.txt
View File

@ -9,7 +9,10 @@ set (gtsam_subdirs
discrete
linear
nonlinear
sam
sfm
slam
smart
navigation
)

3
gtsam/base/CMakeLists.txt
View File

@ -5,5 +5,8 @@ install(FILES ${base_headers} DESTINATION include/gtsam/base)
file(GLOB base_headers_tree "treeTraversal/*.h")
install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal)
file(GLOB deprecated_headers "deprecated/*.h")
install(FILES ${deprecated_headers} DESTINATION include/gtsam/base/deprecated)
# Build tests
add_subdirectory(tests)

5
gtsam/base/GenericValue.h
View File

@ -194,6 +194,11 @@ public:
};
// traits
template <typename ValueType>
struct traits<GenericValue<ValueType> >
: public Testable<GenericValue<ValueType> > {};
// define Value::cast here since now GenericValue has been declared
template<typename ValueType>
const ValueType& Value::cast() const {

27
gtsam/base/LieMatrix.cpp
View File

@ -1,27 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LieMatrix.cpp
* @brief A wrapper around Matrix providing Lie compatibility
* @author Richard Roberts and Alex Cunningham
*/
#include <gtsam/base/LieMatrix_Deprecated.h>
namespace gtsam {
/* ************************************************************************* */
void LieMatrix::print(const std::string& name) const {
gtsam::print(matrix(), name);
}
}

4
gtsam/base/LieMatrix.h
View File

@ -11,7 +11,7 @@
/**
* @file LieMatrix.h
* @brief External deprecation warning, see LieMatrix_Deprecated.h for details
* @brief External deprecation warning, see deprecated/LieMatrix.h for details
* @author Paul Drews
*/
@ -23,4 +23,4 @@
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include "gtsam/base/LieMatrix_Deprecated.h"
#include "gtsam/base/deprecated/LieMatrix.h"

17
gtsam/base/LieScalar.cpp
View File

@ -1,17 +0,0 @@
/*
* LieScalar.cpp
*
* Created on: Apr 12, 2013
* Author: thduynguyen
*/
#include <gtsam/base/LieScalar_Deprecated.h>
namespace gtsam {
void LieScalar::print(const std::string& name) const {
std::cout << name << ": " << d_ << std::endl;
}
}

4
gtsam/base/LieScalar.h
View File

@ -11,7 +11,7 @@
/**
* @file LieScalar.h
* @brief External deprecation warning, see LieScalar_Deprecated.h for details
* @brief External deprecation warning, see deprecated/LieScalar.h for details
* @author Kai Ni
*/
@ -23,4 +23,4 @@
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>

40
gtsam/base/LieVector.cpp
View File

@ -1,40 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 LieVector.cpp
* @brief Implementations for LieVector functions
* @author Alex Cunningham
*/
#include <gtsam/base/LieVector_Deprecated.h>
#include <cstdarg>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
LieVector::LieVector(size_t m, const double* const data)
: Vector(m)
{
for(size_t i = 0; i < m; i++)
(*this)(i) = data[i];
}
/* ************************************************************************* */
void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name);
}
// Does not compile because LieVector is not fixed size.
// GTSAM_CONCEPT_LIE_INST(LieVector)
} // \namespace gtsam

4
gtsam/base/LieVector.h
View File

@ -11,7 +11,7 @@
/**
* @file LieVector.h
* @brief Deprecation warning for LieVector_Deprecated.h, see LieVector_Deprecated.h for details.
* @brief Deprecation warning for LieVector, see deprecated/LieVector.h for details.
* @author Paul Drews
*/
@ -23,4 +23,4 @@
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>

4
gtsam/base/Manifold.h
View File

@ -187,8 +187,8 @@ public:
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Default constructor yields identity
ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
/// Default constructor needs default constructors to be defined
ProductManifold():std::pair<M1,M2>(M1(),M2()) {}
// Construct from two original manifold values
ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}

10
gtsam/base/OptionalJacobian.h
View File

@ -171,6 +171,16 @@ public:
// forward declare
template <typename T> struct traits;
/**
* @brief: meta-function to generate Jacobian
* @param T return type
* @param A argument type
*/
template <class T, class A>
struct MakeJacobian {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
/**
* @brief: meta-function to generate JacobianTA optional reference
* Used mainly by Expressions

5
gtsam/base/LieMatrix_Deprecated.h → gtsam/base/deprecated/LieMatrix.h
View File

@ -60,8 +60,9 @@ struct LieMatrix : public Matrix {
/// @{
/** print @param s optional string naming the object */
GTSAM_EXPORT void print(const std::string& name="") const;
GTSAM_EXPORT void print(const std::string& name = "") const {
gtsam::print(matrix(), name);
}
/** equality up to tolerance */
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);

6
gtsam/base/LieScalar_Deprecated.h → gtsam/base/deprecated/LieScalar.h
View File

@ -48,8 +48,10 @@ namespace gtsam {
/// @name Testable
/// @{
void print(const std::string& name="") const;
bool equals(const LieScalar& expected, double tol=1e-5) const {
void print(const std::string& name = "") const {
std::cout << name << ": " << d_ << std::endl;
}
bool equals(const LieScalar& expected, double tol = 1e-5) const {
return fabs(expected.d_ - d_) <= tol;
}
/// @}

9
gtsam/base/LieVector_Deprecated.h → gtsam/base/deprecated/LieVector.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <cstdarg>
namespace gtsam {
@ -50,11 +51,15 @@ struct LieVector : public Vector {
LieVector(double d) : Vector((Vector(1) << d).finished()) {}
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
GTSAM_EXPORT LieVector(size_t m, const double* const data) : Vector(m) {
for (size_t i = 0; i < m; i++) (*this)(i) = data[i];
}
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& name="") const;
GTSAM_EXPORT void print(const std::string& name="") const {
gtsam::print(vector(), name);
}
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}

18
gtsam/base/serializationTestHelpers.h
View File

@ -49,15 +49,15 @@ bool equality(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsObj(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// De-referenced version for pointers
// De-referenced version for pointers, requires equals method
template<class T>
bool equalsDereferenced(const T& input) {
T output;
@ -84,15 +84,15 @@ bool equalityXML(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// This version is for pointers
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
@ -119,15 +119,15 @@ bool equalityBinary(const T& input = T()) {
return input==output;
}
// This version requires equals
// This version requires Testable
template<class T>
bool equalsBinary(const T& input = T()) {
T output;
roundtripBinary<T>(input,output);
return input.equals(output);
return assert_equal(input, output);
}
// This version is for pointers
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedBinary(const T& input = T()) {
T output;

3
gtsam/base/tests/testLieMatrix.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieMatrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

3
gtsam/base/tests/testLieScalar.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

3
gtsam/base/tests/testLieVector.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Manifold.h>

3
gtsam/base/tests/testTestableAssertions.cpp
View File

@ -15,8 +15,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam/base/TestableAssertions.h>
using namespace gtsam;

133
gtsam/geometry/BearingRange.h Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* 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 BearingRange.h
* @date July, 2015
* @author Frank Dellaert
* @brief Bearing-Range product
*/
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
// Forward declaration of Bearing functor which should be of A1*A2 -> return_type
// For example Bearing<Pose3,Point3>(pose,point), defined in Pose3.h will return Unit3
// At time of writing only Pose2 and Pose3 specialize this functor.
template <typename A1, typename A2>
struct Bearing;
// Forward declaration of Range functor which should be of A1*A2 -> return_type
// For example Range<Pose2,Pose2>(T1,T2), defined in Pose2.h will return double
// At time of writing Pose2, Pose3, and several Camera variants specialize this for several types
template <typename A1, typename A2>
struct Range;
/**
* Bearing-Range product for a particular A1,A2 combination will use the functors above to create
* a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type>
* For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double>
* and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>
*/
template <typename A1, typename A2>
struct BearingRange
: public ProductManifold<typename Bearing<A1, A2>::result_type,
typename Range<A1, A2>::result_type> {
typedef typename Bearing<A1, A2>::result_type B;
typedef typename Range<A1, A2>::result_type R;
typedef ProductManifold<B, R> Base;
BearingRange() {}
BearingRange(const ProductManifold<B, R>& br) : Base(br) {}
BearingRange(const B& b, const R& r) : Base(b, r) {}
/// Prediction function that stacks measurements
static BearingRange Measure(
const A1& a1, const A2& a2,
OptionalJacobian<Base::dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<Base::dimension, traits<A2>::dimension> H2 =
boost::none) {
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>()(a1, a2, H1 ? &HB1 : 0, H2 ? &HB2 : 0);
R r = Range<A1, A2>()(a1, a2, H1 ? &HR1 : 0, H2 ? &HR2 : 0);
if (H1) *H1 << HB1, HR1;
if (H2) *H2 << HB2, HR2;
return BearingRange(b, r);
}
void print(const std::string& str = "") const {
std::cout << str;
traits<B>::Print(this->first, "bearing ");
traits<R>::Print(this->second, "range ");
}
bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
return traits<B>::Equals(this->first, m2.first, tol) &&
traits<R>::Equals(this->second, m2.second, tol);
}
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;
};
// Declare this to be both Testable and a Manifold
template <typename A1, typename A2>
struct traits<BearingRange<A1, A2> >
: Testable<BearingRange<A1, A2> >,
internal::ManifoldTraits<BearingRange<A1, A2> > {};
// Helper class for to implement Range traits for classes with a bearing method
// For example, to specialize Bearing to Pose3 and Point3, using Pose3::bearing, it suffices to say
// template <> struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
// where the third argument is used to indicate the return type
template <class A1, typename A2, class RT>
struct HasBearing {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
return a1.bearing(a2, H1, H2);
}
};
// Similar helper class for to implement Range traits for classes with a range method
// For classes with overloaded range methods, such as SimpleCamera, this can even be templated:
// template <typename T> struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
template <class A1, typename A2, class RT>
struct HasRange {
typedef RT result_type;
RT operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<RT>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2) {
return a1.range(a2, H1, H2);
}
};
} // namespace gtsam

21
gtsam/geometry/CalibratedCamera.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
@ -318,7 +319,7 @@ public:
}
/// @}
/// @name Transformations and mesaurement functions
/// @name Transformations and measurement functions
/// @{
/**
@ -384,14 +385,16 @@ private:
/// @}
};
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
};
// manifold traits
template <>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
template <>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
}
// range traits, used in RangeFactor
template <typename T>
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
} // namespace gtsam

29
gtsam/geometry/PinholeCamera.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/PinholePose.h>
#include <gtsam/geometry/BearingRange.h>
namespace gtsam {
@ -263,24 +264,22 @@ public:
template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
boost::optional<Matrix&> Dother = boost::none) const {
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6 + CalibrationB::dimension);
Dother->setZero();
Dother->block<1, 6>(0, 0) = Dother_;
Dother->template block<1, 6>(0, 0) = Dother_;
}
return result;
}
/**
* Calculate range to another camera
* Calculate range to a calibrated camera
* @param camera Other camera
* @return range (double)
*/
@ -304,14 +303,18 @@ private:
};
template<typename Calibration>
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
// manifold traits
template<typename Calibration>
struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
template <typename Calibration>
struct traits<PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
template <typename Calibration>
struct traits<const PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
// range traits, used in RangeFactor
template <typename Calibration, typename T>
struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
} // \ gtsam

18
gtsam/geometry/Point3.h
View File

@ -22,6 +22,7 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
@ -199,4 +200,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
}
template <typename A1, typename A2>
struct Range;
template <>
struct Range<Point3, Point3> {
typedef double result_type;
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
}
};
} // namespace gtsam

13
gtsam/geometry/Pose2.h
View File

@ -20,9 +20,11 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
namespace gtsam {
@ -289,11 +291,18 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<>
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template<>
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
// bearing and range traits, used in RangeFactor
template <typename T>
struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
template <typename T>
struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
} // namespace gtsam

50
gtsam/geometry/Pose3.cpp
View File

@ -315,34 +315,46 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return transform_to(point).norm();
return local.norm();
} else {
Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
}
}
/* ************************************************************************* */
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
OptionalJacobian<1,6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Matrix13::Zero(), H2_;
}
double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 6> H2) const {
Matrix13 D_local_point;
double r = range(pose.translation(), H1, H2 ? &D_local_point : 0);
if (H2) *H2 << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
return r;
}
/* ************************************************************************* */
Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 3> H2) const {
Matrix36 D_local_pose;
Matrix3 D_local_point;
Point3 local = transform_to(point, H1 ? &D_local_pose : 0, H2 ? &D_local_point : 0);
if (!H1 && !H2) {
return Unit3(local);
} else {
Matrix23 D_b_local;
Unit3 b = Unit3::FromPoint3(local, D_b_local);
if (H1) *H1 = D_b_local * D_local_pose;
if (H2) *H2 = D_b_local * D_local_point;
return b;
}
}
/* ************************************************************************* */
boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
const size_t n = pairs.size();

19
gtsam/geometry/Pose3.h
View File

@ -19,6 +19,7 @@
#include <gtsam/config.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
@ -262,6 +263,14 @@ public:
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const;
/**
* Calculate bearing to a landmark
* @param point 3D location of landmark
* @return bearing (Unit3)
*/
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
@ -323,11 +332,17 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;
template<>
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template<>
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
template <typename T>
struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
} // namespace gtsam

23
gtsam/geometry/SimpleCamera.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
@ -125,14 +126,18 @@ public:
};
template<>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
template<>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
// manifold traits
template <>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}
template <>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
// range traits, used in RangeFactor
template <typename T>
struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
} // namespace gtsam

22
gtsam/geometry/concepts.h
View File

@ -59,24 +59,6 @@ private:
}
};
/**
* Range measurement concept
* Given a pair of Lie variables, there must exist a function to calculate
* range with derivatives.
*/
template<class V1, class V2>
class RangeMeasurementConcept {
private:
static double checkRangeMeasurement(const V1& x, const V2& p) {
return x.range(p);
}
static double checkRangeMeasurementDerivatives(const V1& x, const V2& p) {
boost::optional<Matrix&> H1, H2;
return x.range(p, H1, H2);
}
};
} // \namespace gtsam
/**
@ -92,7 +74,3 @@ private:
#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T;
/** Range Measurement macros */
#define GTSAM_CONCEPT_RANGE_MEASUREMENT_INST(V1,V2) template class gtsam::RangeMeasurementConcept<V1,V2>;
#define GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(V1,V2) typedef gtsam::RangeMeasurementConcept<V1,V2> _gtsam_RangeMeasurementConcept##V1##V2;

79
gtsam/geometry/tests/testBearingRange.cpp Normal file
View File

@ -0,0 +1,79 @@
/* ----------------------------------------------------------------------------
* 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 testBearingRange.cpp
* @brief Unit tests for BearingRange Class
* @author Frank Dellaert
* @date July 2015
*/
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace serializationTestHelpers;
typedef BearingRange<Pose2, Point2> BearingRange2D;
BearingRange2D br2D(1, 2);
typedef BearingRange<Pose3, Point3> BearingRange3D;
BearingRange3D br3D(Pose3().bearing(Point3(1, 0, 0)), 1);
//******************************************************************************
TEST(BearingRange2D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange2D>));
}
/* ************************************************************************* */
TEST(BearingRange, 2D) {
BearingRange2D expected(0, 1);
BearingRange2D actual = BearingRange2D::Measure(Pose2(), Point2(1, 0));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(BearingRange, Serialization2D) {
EXPECT(equalsObj<BearingRange2D>(br2D));
EXPECT(equalsXML<BearingRange2D>(br2D));
EXPECT(equalsBinary<BearingRange2D>(br2D));
}
//******************************************************************************
TEST(BearingRange3D, Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<BearingRange3D>));
}
/* ************************************************************************* */
TEST(BearingRange, 3D) {
BearingRange3D expected(Unit3(), 1);
BearingRange3D actual = BearingRange3D::Measure(Pose3(), Point3(1, 0, 0));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(BearingRange, Serialization3D) {
EXPECT(equalsObj<BearingRange3D>(br3D));
EXPECT(equalsXML<BearingRange3D>(br3D));
EXPECT(equalsBinary<BearingRange3D>(br3D));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -633,6 +633,22 @@ TEST( Pose3, range_pose )
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
Unit3 bearing_proxy(const Pose3& pose, const Point3& point) {
return pose.bearing(point);
}
TEST( Pose3, bearing )
{
Matrix expectedH1, actualH1, expectedH2, actualH2;
EXPECT(assert_equal(Unit3(1,0,0),x1.bearing(l1, actualH1, actualH2),1e-9));
// Check numerical derivatives
expectedH1 = numericalDerivative21(bearing_proxy, x1, l1);
expectedH2 = numericalDerivative22(bearing_proxy, x1, l1);
EXPECT(assert_equal(expectedH1,actualH1));
EXPECT(assert_equal(expectedH2,actualH2));
}
/* ************************************************************************* */
TEST( Pose3, unicycle )
{

1
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -38,6 +38,7 @@ using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");

3
gtsam/nonlinear/Expression.h
View File

@ -172,6 +172,9 @@ public:
private:
/// Default constructor, for serialization
Expression() {}
/// Keys and dimensions in same order
typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
KeysAndDims keysAndDims() const;

157
gtsam/nonlinear/ExpressionFactor.h
View File

@ -23,43 +23,54 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <numeric>
namespace gtsam {
/**
* Factor that supports arbitrary expressions via AD
*/
template<class T>
template<typename T>
class ExpressionFactor: public NoiseModelFactor {
BOOST_CONCEPT_ASSERT((IsTestable<T>));
protected:
typedef ExpressionFactor<T> This;
static const int Dim = traits<T>::dimension;
T measurement_; ///< the measurement to be compared with the expression
T measured_; ///< the measurement to be compared with the expression
Expression<T> expression_; ///< the expression that is AD enabled
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
static const int Dim = traits<T>::dimension;
public:
typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
/// Constructor
ExpressionFactor(const SharedNoiseModel& noiseModel, //
const T& measurement, const Expression<T>& expression) :
measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
const T& measurement, const Expression<T>& expression)
: NoiseModelFactor(noiseModel), measured_(measurement) {
initialize(expression);
}
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
boost::tie(keys_, dims_) = expression_.keysAndDims();
/// Destructor
virtual ~ExpressionFactor() {}
/** return the measurement */
const T& measured() const { return measured_; }
/// print relies on Testable traits being defined for T
void print(const std::string& s, const KeyFormatter& keyFormatter) const {
NoiseModelFactor::print(s, keyFormatter);
traits<T>::Print(measured_, s + ".measured_");
}
/// equals relies on Testable traits being defined for T
bool equals(const NonlinearFactor& f, double tol) const {
const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
return p && NoiseModelFactor::equals(f, tol) &&
traits<T>::Equals(measured_, p->measured_, tol) &&
dims_ == p->dims_;
}
/**
@ -71,10 +82,10 @@ public:
boost::optional<std::vector<Matrix>&> H = boost::none) const {
if (H) {
const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
return traits<T>::Local(measurement_, value);
return traits<T>::Local(measured_, value);
} else {
const T value = expression_.value(x);
return traits<T>::Local(measurement_, value);
return traits<T>::Local(measured_, value);
}
}
@ -105,7 +116,7 @@ public:
T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
Ab(size()).col(0) = -traits<T>::Local(measurement_, value);
Ab(size()).col(0) = -traits<T>::Local(measured_, value);
// Whiten the corresponding system, Ab already contains RHS
Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
@ -119,8 +130,116 @@ public:
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
protected:
ExpressionFactor() {}
/// Default constructor, for serialization
/// Constructor for serializable derived classes
ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
: NoiseModelFactor(noiseModel), measured_(measurement) {
// Not properly initialized yet, need to call initialize
}
/// Initialize with constructor arguments
void initialize(const Expression<T>& expression) {
if (!noiseModel_)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel_->dim() != Dim)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
expression_ = expression;
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
boost::tie(keys_, dims_) = expression_.keysAndDims();
}
/// Recreate expression from keys_ and measured_, used in load below.
/// Needed to deserialize a derived factor
virtual Expression<T> expression() const {
throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
}
private:
/// Save to an archive: just saves the base class
template <class Archive>
void save(Archive& ar, const unsigned int /*version*/) const {
ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar << boost::serialization::make_nvp("measured_", this->measured_);
}
/// Load from an archive, creating a valid expression using the overloaded
/// [expression] method
template <class Archive>
void load(Archive& ar, const unsigned int /*version*/) {
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
ar >> boost::serialization::make_nvp("measured_", this->measured_);
this->initialize(expression());
}
// Indicate that we implement save/load separately, and be friendly to boost
BOOST_SERIALIZATION_SPLIT_MEMBER()
friend class boost::serialization::access;
};
// ExpressionFactor
/// traits
template <typename T>
struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
/**
* Binary specialization of ExpressionFactor meant as a base class for binary factors
* Enforces expression method with two keys, and provides evaluateError
* Derived needs to call initialize.
*/
template <typename T, typename A1, typename A2>
class ExpressionFactor2 : public ExpressionFactor<T> {
public:
/// Destructor
virtual ~ExpressionFactor2() {}
/// Backwards compatible evaluateError, to make existing tests compile
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Values values;
values.insert(this->keys_[0], a1);
values.insert(this->keys_[1], a2);
std::vector<Matrix> H(2);
Vector error = this->unwhitenedError(values, H);
if (H1) (*H1) = H[0];
if (H2) (*H2) = H[1];
return error;
}
/// Recreate expression from given keys_ and measured_, used in load
/// Needed to deserialize a derived factor
virtual Expression<T> expression(Key key1, Key key2) const {
throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
}
protected:
/// Default constructor, for serialization
ExpressionFactor2() {}
/// Constructor takes care of keys, but still need to call initialize
ExpressionFactor2(Key key1, Key key2,
const SharedNoiseModel& noiseModel,
const T& measurement)
: ExpressionFactor<T>(noiseModel, measurement) {
this->keys_.push_back(key1);
this->keys_.push_back(key2);
}
private:
/// Return an expression that predicts the measurement given Values
virtual Expression<T> expression() const {
return expression(this->keys_[0], this->keys_[1]);
}
};
// ExpressionFactor2
}// \ namespace gtsam

33
gtsam/nonlinear/expressionTesting.h
View File

@ -20,44 +20,11 @@
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam {
namespace internal {
// CPPUnitLite-style test for linearization of a factor
bool testFactorJacobians(TestResult& result_, const std::string& name_,
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
// Check cast result and then equality
return actual && assert_equal(expected, *actual, tolerance);
}
}
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); }
namespace internal {
// CPPUnitLite-style test for linearization of an ExpressionFactor
template<typename T>

34
gtsam/nonlinear/factorTesting.h
View File

@ -22,6 +22,10 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam {
/**
@ -29,6 +33,10 @@ namespace gtsam {
* The benefit of this method is that it does not need to know what types are
* involved to evaluate the factor. If all the machinery of gtsam is working
* correctly, we should get the correct numerical derivatives out the other side.
* NOTE(frank): factor that have non vector-space measurements use between or LocalCoordinates
* to evaluate the error, and their derivatives will only be correct for near-zero errors.
* This is fixable but expensive, and does not matter in practice as most factors will sit near
* zero errors anyway. However, it means that below will only be exact for the correct measurement.
*/
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) {
@ -65,4 +73,30 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
return JacobianFactor(jacobians, -e);
}
namespace internal {
// CPPUnitLite-style test for linearization of a factor
bool testFactorJacobians(TestResult& result_, const std::string& name_,
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
double tolerance) {
// Create expected value by numerical differentiation
JacobianFactor expected = linearizeNumerically(factor, values, delta);
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
// Check cast result and then equality
return actual && assert_equal(expected, *actual, tolerance);
}
}
/// \brief Check the Jacobians produced by a factor against finite differences.
/// \param factor The factor to test.
/// \param values Values filled in for testing the Jacobians.
/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians
/// \param tolerance The numerical tolerance to use when comparing Jacobians.
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
{ EXPECT(gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance)); }
} // namespace gtsam

70
gtsam/sam/BearingFactor.h Normal file
View File

@ -0,0 +1,70 @@
/* ----------------------------------------------------------------------------
* 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
* @brief Serializable factor induced by a bearing measurement
* @date July 2015
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
/**
* Binary factor for a bearing measurement
* Works for any two types A1,A2 for which the functor Bearing<A1,A2>() is
* defined
* @addtogroup SAM
*/
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
typedef ExpressionFactor2<T, A1, A2> Base;
/// default constructor
BearingFactor() {}
/// primary constructor
BearingFactor(Key key1, Key key2, const T& measured,
const SharedNoiseModel& model)
: Base(key1, key2, model, measured) {
this->initialize(expression(key1, key2));
}
// Return measurement expression
virtual Expression<T> expression(Key key1, Key key2) const {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
}
/// print
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "BearingFactor" << std::endl;
Base::print(s, kf);
}
}; // BearingFactor
/// traits
template <typename A1, typename A2, typename T>
struct traits<BearingFactor<A1, A2, T> >
: public Testable<BearingFactor<A1, A2, T> > {};
} // namespace gtsam

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

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* 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/ExpressionFactor.h>
#include <gtsam/geometry/BearingRange.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
/**
* 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 ExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
private:
typedef BearingRange<A1, A2> T;
typedef ExpressionFactor2<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This;
public:
typedef boost::shared_ptr<This> shared_ptr;
/// 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 {
return Expression<T>(T::Measure, Expression<A1>(key1),
Expression<A2>(key2));
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const {
std::cout << s << "BearingRangeFactor" << std::endl;
Base::print(s, kf);
}
}; // 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

6
gtsam/sam/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB sam_headers "*.h")
install(FILES ${sam_headers} DESTINATION include/gtsam/sam)
# Build tests
add_subdirectory(tests)

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

@ -0,0 +1,142 @@
/* ----------------------------------------------------------------------------
* 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
* @date July 2015
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/ExpressionFactor.h>
namespace gtsam {
// forward declaration of Range functor, assumed partially specified
template <typename A1, typename A2>
struct Range;
/**
* Binary factor for a range measurement
* Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
* @addtogroup SAM
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactor : public ExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactor<A1, A2> This;
typedef ExpressionFactor2<T, A1, A2> Base;
public:
/// default constructor
RangeFactor() {}
RangeFactor(Key key1, Key key2, T 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<T> expression(Key key1, Key key2) const {
Expression<A1> a1_(key1);
Expression<A2> a2_(key2);
return Expression<T>(Range<A1, A2>(), a1_, a2_);
}
/// 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 <typename A1, typename A2, typename T>
struct traits<RangeFactor<A1, A2, T> >
: public Testable<RangeFactor<A1, A2, T> > {};
/**
* Binary factor for a range measurement, with a transform applied
* @addtogroup SAM
*/
template <typename A1, typename A2 = A1,
typename T = typename Range<A1, A2>::result_type>
class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
private:
typedef RangeFactorWithTransform<A1, A2> This;
typedef ExpressionFactor2<T, A1, A2> Base;
A1 body_T_sensor_; ///< The pose of the sensor in the body frame
public:
//// Default constructor
RangeFactorWithTransform() {}
RangeFactorWithTransform(Key key1, Key key2, T 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<T> 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> a2_(key2);
return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
}
/** 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 <typename 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 <typename A1, typename A2, typename T>
struct traits<RangeFactorWithTransform<A1, A2, T> >
: public Testable<RangeFactorWithTransform<A1, A2, T> > {};
} // \ namespace gtsam

1
gtsam/sam/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
gtsamAddTestsGlob(sam "test*.cpp" "" "gtsam")

107
gtsam/sam/tests/testBearingFactor.cpp Normal file
View File

@ -0,0 +1,107 @@
/* ----------------------------------------------------------------------------
* 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 testBearingFactor.cpp
* @brief Unit tests for BearingFactor Class
* @author Frank Dellaert
* @date July 2015
*/
#include <gtsam/sam/BearingFactor.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;
Key poseKey(1);
Key pointKey(2);
typedef BearingFactor<Pose2, Point2> BearingFactor2D;
double measurement2D(10.0);
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
typedef BearingFactor<Pose3, Point3> BearingFactor3D;
Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, 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(BearingFactor, Serialization2D) {
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(BearingFactor, 2D) {
// Serialize the factor
std::string serialized = serializeXML(factor2D);
// And de-serialize it
BearingFactor2D 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(BearingFactor, Serialization3D) {
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
TEST(BearingFactor, 3D) {
// Serialize the factor
std::string serialized = serializeXML(factor3D);
// And de-serialize it
BearingFactor3D 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);
}
/* ************************************************************************* */

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

@ -0,0 +1,106 @@
/* ----------------------------------------------------------------------------
* 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;
Key poseKey(1);
Key pointKey(2);
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
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(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(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(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(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);
}
/* ************************************************************************* */

130
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/geometry/SimpleCamera.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
using namespace std;
@ -37,6 +38,10 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
/* ************************************************************************* */
Vector factorError2D(const Pose2& pose, const Point2& point,
const RangeFactor2D& factor) {
@ -63,19 +68,33 @@ Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
/* ************************************************************************* */
TEST( RangeFactor, Constructor) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
}
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
/* ************************************************************************* */
TEST(RangeFactor, Serialization2D) {
RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
EXPECT(serializationTestHelpers::equalsObj(factor2D));
EXPECT(serializationTestHelpers::equalsXML(factor2D));
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
}
/* ************************************************************************* */
TEST(RangeFactor, Serialization3D) {
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
EXPECT(serializationTestHelpers::equalsObj(factor3D));
EXPECT(serializationTestHelpers::equalsXML(factor3D));
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
}
/* ************************************************************************* */
TEST( RangeFactor, ConstructorWithTransform) {
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
@ -89,10 +108,6 @@ TEST( RangeFactor, ConstructorWithTransform) {
/* ************************************************************************* */
TEST( RangeFactor, Equals ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model);
RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model);
CHECK(assert_equal(factor2D_1, factor2D_2));
@ -105,9 +120,6 @@ TEST( RangeFactor, Equals ) {
/* ************************************************************************* */
TEST( RangeFactor, EqualsWithTransform ) {
// Create two identical factors and make sure they're equal
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
@ -128,9 +140,6 @@ TEST( RangeFactor, EqualsWithTransform ) {
/* ************************************************************************* */
TEST( RangeFactor, Error2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
@ -150,9 +159,6 @@ TEST( RangeFactor, Error2D ) {
/* ************************************************************************* */
TEST( RangeFactor, Error2DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
@ -176,9 +182,6 @@ TEST( RangeFactor, Error2DWithTransform ) {
/* ************************************************************************* */
TEST( RangeFactor, Error3D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor3D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
@ -198,9 +201,6 @@ TEST( RangeFactor, Error3D ) {
/* ************************************************************************* */
TEST( RangeFactor, Error3DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
@ -225,9 +225,6 @@ TEST( RangeFactor, Error3DWithTransform ) {
/* ************************************************************************* */
TEST( RangeFactor, Jacobian2D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor2D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
@ -253,9 +250,6 @@ TEST( RangeFactor, Jacobian2D ) {
/* ************************************************************************* */
TEST( RangeFactor, Jacobian2DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
body_P_sensor);
@ -285,9 +279,6 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
/* ************************************************************************* */
TEST( RangeFactor, Jacobian3D ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor3D factor(poseKey, pointKey, measurement, model);
// Set the linearization point
@ -313,9 +304,6 @@ TEST( RangeFactor, Jacobian3D ) {
/* ************************************************************************* */
TEST( RangeFactor, Jacobian3DWithTransform ) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
@ -343,6 +331,64 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
CHECK(assert_equal(H2Expected, H2Actual, 1e-9));
}
/* ************************************************************************* */
// Do a test with Point3
TEST(RangeFactor, Point3) {
// Create a factor
RangeFactor<Point3> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Point3 pose(1.0, 2.0, 00);
Point3 point(-4.0, 11.0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
}
/* ************************************************************************* */
// Do tests with SimpleCamera
TEST( RangeFactor, Camera) {
RangeFactor<SimpleCamera,Point3> factor1(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,Pose3> factor2(poseKey, pointKey, measurement, model);
RangeFactor<SimpleCamera,SimpleCamera> factor3(poseKey, pointKey, measurement, model);
}
/* ************************************************************************* */
// Do a test with non GTSAM types
namespace gtsam{
template <>
struct Range<Vector3, Vector3> {
typedef double result_type;
double operator()(const Vector3& v1, const Vector3& v2,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
return (v2 - v1).norm();
// derivatives not implemented
}
};
}
TEST(RangeFactor, NonGTSAM) {
// Create a factor
Key poseKey(1);
Key pointKey(2);
double measurement(10.0);
RangeFactor<Vector3> factor(poseKey, pointKey, measurement, model);
// Set the linearization point
Vector3 pose(1.0, 2.0, 00);
Vector3 point(-4.0, 11.0, 0);
// The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
Vector expectedError = (Vector(1) << 0.295630141).finished();
// Verify we get the expected error
CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/sfm/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB sfm_headers "*.h")
install(FILES ${sfm_headers} DESTINATION include/gtsam/sfm)
# Build tests
add_subdirectory(tests)

1
gtsam/sfm/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
gtsamAddTestsGlob(sfm "test*.cpp" "" "gtsam")

93
gtsam/slam/BearingFactor.h
View File

@ -9,94 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @file BearingFactor.H
* @author Frank Dellaert
**/
#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 directory")
#else
#warning "BearingFactor is now an ExpressionFactor in SAM directory"
#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>

117
gtsam/slam/BearingRangeFactor.h
View File

@ -9,116 +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/geometry/concepts.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/NonlinearFactor.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
namespace gtsam {
/**
* Binary factor for a bearing measurement
* @addtogroup SLAM
*/
template<class POSE, class POINT, class ROTATION = typename POSE::Rotation>
class BearingRangeFactor: public NoiseModelFactor2<POSE, POINT>
{
public:
typedef BearingRangeFactor<POSE, POINT> This;
typedef NoiseModelFactor2<POSE, POINT> Base;
typedef boost::shared_ptr<This> shared_ptr;
private:
typedef POSE Pose;
typedef ROTATION Rot;
typedef POINT Point;
// the measurement
Rot measuredBearing_;
double measuredRange_;
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(Rot)
GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point)
GTSAM_CONCEPT_POSE_TYPE(Pose)
public:
BearingRangeFactor() {} /* Default constructor */
BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double 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, double 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 Rot manifold */
Vector evaluateError(const Pose& pose, const Point& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Matrix H11, H21, H12, H22;
boost::optional<Matrix&> H11_ = H1 ? boost::optional<Matrix&>(H11) : boost::optional<Matrix&>();
boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
Rot y1 = pose.bearing(point, H11_, H12_);
Vector e1 = Rot::Logmap(measuredBearing_.between(y1));
double y2 = pose.range(point, H21_, H22_);
Vector e2 = (Vector(1) << y2 - measuredRange_).finished();
if (H1) *H1 = gtsam::stack(2, &H11, &H21);
if (H2) *H2 = gtsam::stack(2, &H12, &H22);
return concatVectors(2, &e1, &e2);
}
/** return the measured */
const std::pair<Rot, double> measured() const {
return std::make_pair(measuredBearing_, measuredRange_);
}
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(measuredBearing_);
ar & BOOST_SERIALIZATION_NVP(measuredRange_);
}
}; // BearingRangeFactor
} // namespace gtsam
#include <gtsam/sam/BearingRangeFactor.h>

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/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/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>

6
gtsam/smart/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# Install headers
file(GLOB smart_headers "*.h")
install(FILES ${smart_headers} DESTINATION include/gtsam/smart)
# Build tests
add_subdirectory(tests)

1
gtsam/smart/tests/CMakeLists.txt Normal file
View File

@ -0,0 +1 @@
gtsamAddTestsGlob(smart "test*.cpp" "" "gtsam")

13
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -170,4 +170,17 @@ private:
template<>
struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<PoseRTV, PoseRTV> {
typedef double result_type;
double operator()(const PoseRTV& pose1, const PoseRTV& pose2,
OptionalJacobian<1, 9> H1 = boost::none,
OptionalJacobian<1, 9> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
} // \namespace gtsam

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>

4
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);
@ -452,7 +452,7 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
};
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {

14
gtsam_unstable/slam/serialization.cpp
View File

@ -5,20 +5,19 @@
* @author Alex Cunningham
*/
#include <gtsam/base/LieMatrix_Deprecated.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieVector_Deprecated.h>
#include <gtsam/slam/serialization.h>
#include <gtsam/base/serialization.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingFactor.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>
#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>
@ -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");

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

@ -21,7 +21,7 @@
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/LieVector_Deprecated.h>
#include <gtsam/base/deprecated/LieVector.h>
using namespace std;
using namespace gtsam;

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/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) {

4
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>
@ -25,7 +25,7 @@
#include <gtsam/base/treeTraversal-inst.h>
#include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/base/LieScalar_Deprecated.h>
#include <gtsam/base/deprecated/LieScalar.h>
using namespace boost::assign;
#include <boost/range/adaptor/map.hpp>
namespace br { using namespace boost::adaptors; using namespace boost::range; }

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>

20
tests/testSerializationSLAM.cpp
View File

@ -22,15 +22,14 @@
#include <tests/smallExample.h>
//#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/BearingFactor.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>
//#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>
@ -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));

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>

7
timing/timeRot2.cpp
View File

@ -92,11 +92,8 @@ int main()
int n = 50000000;
cout << "NOTE: Times are reported for " << n << " calls" << endl;
// create a random direction:
double norm=sqrt(16.0+4.0);
double x=4.0/norm, y=2.0/norm;
Vector v = (Vector(2) << x, y).finished();
Rot2 R = Rot2(0.4), R2 = R.retract(v), R3(0.6);
Vector1 v; v << 0.1;
Rot2 R = Rot2(0.4), R2(0.5), R3(0.6);
TEST(Rot2_Expmap, Rot2::Expmap(v));
TEST(Rot2_Retract, R.retract(v));