109 lines
2.7 KiB
C++
109 lines
2.7 KiB
C++
/**
|
|
* @file BearingS2.h
|
|
*
|
|
* @brief Manifold measurement between two points on a unit sphere
|
|
*
|
|
* @date Jan 26, 2012
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam_unstable/dllexport.h>
|
|
#include <gtsam/geometry/Rot2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
|
|
namespace gtsam {
|
|
|
|
class GTSAM_UNSTABLE_EXPORT BearingS2 {
|
|
protected:
|
|
Rot2 azimuth_, elevation_;
|
|
|
|
public:
|
|
static const size_t dimension = 2;
|
|
|
|
/// @name Constructors
|
|
/// @{
|
|
|
|
/** Default constructor - straight ahead */
|
|
BearingS2() {}
|
|
|
|
/** Build from components */
|
|
BearingS2(double azimuth, double elevation)
|
|
: azimuth_(Rot2::fromAngle(azimuth)), elevation_(Rot2::fromAngle(elevation)) {}
|
|
|
|
BearingS2(const Rot2& azimuth, const Rot2& elevation)
|
|
: azimuth_(azimuth), elevation_(elevation) {}
|
|
|
|
// access
|
|
const Rot2& azimuth() const { return azimuth_; }
|
|
const Rot2& elevation() const { return elevation_; }
|
|
|
|
/// @}
|
|
/// @name Measurements
|
|
/// @{
|
|
|
|
/**
|
|
* Observation function for downwards-facing camera
|
|
*/
|
|
// FIXME: will not work for TARGET = Point3
|
|
template<class POSE, class TARGET>
|
|
static BearingS2 fromDownwardsObservation(const POSE& A, const TARGET& B) {
|
|
return fromDownwardsObservation(A.pose(), B.translation());
|
|
}
|
|
|
|
static BearingS2 fromDownwardsObservation(const Pose3& A, const Point3& B);
|
|
|
|
/** Observation function with standard, forwards-facing camera */
|
|
static BearingS2 fromForwardObservation(const Pose3& A, const Point3& B);
|
|
|
|
/// @}
|
|
/// @name Testable
|
|
/// @{
|
|
|
|
/** print with optional string */
|
|
void print(const std::string& s = "") const;
|
|
|
|
/** assert equality up to a tolerance */
|
|
bool equals(const BearingS2& x, double tol = 1e-9) const;
|
|
|
|
/// @}
|
|
/// @name Manifold
|
|
/// @{
|
|
|
|
/// Dimensionality of tangent space = 2 DOF - used to autodetect sizes
|
|
inline static size_t Dim() { return dimension; }
|
|
|
|
/// Dimensionality of tangent space = 2 DOF
|
|
inline size_t dim() const { return dimension; }
|
|
|
|
/// Retraction from R^2 to BearingS2 manifold neighborhood around current pose
|
|
/// Tangent space parameterization is [azimuth elevation]
|
|
BearingS2 retract(const Vector& v) const;
|
|
|
|
/// Local coordinates of BearingS2 manifold neighborhood around current pose
|
|
Vector localCoordinates(const BearingS2& p2) const;
|
|
|
|
private:
|
|
|
|
/// @}
|
|
/// @name Advanced Interface
|
|
/// @{
|
|
|
|
#if GTSAM_ENABLE_BOOST_SERIALIZATION //
|
|
// Serialization function
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int /*version*/) {
|
|
ar & BOOST_SERIALIZATION_NVP(azimuth_);
|
|
ar & BOOST_SERIALIZATION_NVP(elevation_);
|
|
}
|
|
#endif
|
|
|
|
};
|
|
|
|
/// traits
|
|
template<> struct traits<BearingS2> : public Testable<BearingS2> {};
|
|
|
|
} // \namespace gtsam
|