Loading...
Searching...
No Matches
BearingRange.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept/assert.hpp>
#include <boost/serialization/nvp.hpp>
#include <iostream>
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;
template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
struct BearingRange {
private:
B bearing_;
R range_;
public:
enum { dimB = traits<B>::dimension };
enum { dimR = traits<R>::dimension };
enum { dimension = dimB + dimR };
BearingRange() {}
BearingRange(const B& b, const R& r) : bearing_(b), range_(r) {}
const B& bearing() const { return bearing_; }
const R& range() const { return range_; }
static BearingRange Measure(
const A1& a1, const A2& a2,
OptionalJacobian<dimension, traits<A1>::dimension> H1 = boost::none,
OptionalJacobian<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);
}
static B MeasureBearing(const A1& a1, const A2& a2) {
return Bearing<A1, A2>()(a1, a2);
}
static R MeasureRange(const A1& a1, const A2& a2) {
return Range<A1, A2>()(a1, a2);
}
void print(const std::string& str = "") const {
std::cout << str;
traits<B>::Print(bearing_, "bearing ");
traits<R>::Print(range_, "range ");
}
bool equals(const BearingRange<A1, A2>& m2, double tol = 1e-8) const {
return traits<B>::Equals(bearing_, m2.bearing_, tol) &&
traits<R>::Equals(range_, m2.range_, tol);
}
inline static size_t Dim() { return dimension; }
inline size_t dim() const { return dimension; }
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
BearingRange retract(const TangentVector& xi) const {
B m1 = traits<B>::Retract(bearing_, xi.template head<dimB>());
R m2 = traits<R>::Retract(range_, xi.template tail<dimR>());
return BearingRange(m1, m2);
}
TangentVector localCoordinates(const BearingRange& other) const {
typename traits<B>::TangentVector v1 = traits<B>::Local(bearing_, other.bearing_);
typename traits<R>::TangentVector v2 = traits<R>::Local(range_, other.range_);
TangentVector v;
v << v1, v2;
return v;
}
private:
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp("bearing", bearing_);
ar& boost::serialization::make_nvp("range", range_);
}
friend class boost::serialization::access;
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum {
NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
};
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
// 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=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
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 PinholeCamera, this can even be templated:
// template <typename T> struct Range<PinholeCamera, T> : HasRange<PinholeCamera, 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=boost::none,
OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
return a1.range(a2, H1, H2);
}
};
} // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
OptionalJacobian.h
Special class for optional Jacobian arguments.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
Manifold.h
Base class and basic functions for Manifold types.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Bearing
Definition BearingRange.h:34
gtsam::Range
Definition BearingRange.h:40
gtsam::BearingRange
Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
Definition BearingRange.h:51
gtsam::BearingRange::MeasureRange
static R MeasureRange(const A1 &a1, const A2 &a2)
Predict range.
Definition BearingRange.h:101
gtsam::BearingRange::Measure
static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< dimension, traits< A2 >::dimension > H2=boost::none)
Prediction function that stacks measurements.
Definition BearingRange.h:78
gtsam::BearingRange::retract
BearingRange retract(const TangentVector &xi) const
Retract delta to manifold.
Definition BearingRange.h:130
gtsam::BearingRange::bearing
const B & bearing() const
Return bearing measurement.
Definition BearingRange.h:72
gtsam::BearingRange::localCoordinates
TangentVector localCoordinates(const BearingRange &other) const
Compute the coordinates in the tangent space.
Definition BearingRange.h:137
gtsam::BearingRange::range
const R & range() const
Return range measurement.
Definition BearingRange.h:75
gtsam::BearingRange::MeasureBearing
static B MeasureBearing(const A1 &a1, const A2 &a2)
Predict bearing.
Definition BearingRange.h:96
gtsam::HasBearing
Definition BearingRange.h:180
gtsam::HasRange
Definition BearingRange.h:194