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

HasBearing and HasRange helper classes

release/4.3a0
Frank Dellaert 2015-07-14 00:32:24 -07:00
parent 5f6f9847d6
commit 64d315df19
7 changed files with 96 additions and 187 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/BearingRange.h
View File

@ -93,4 +93,28 @@ 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
template <class A1, typename A2, class T = double>
struct HasBearing {
typedef T result_type;
T operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<T>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<T>::dimension, traits<A2>::dimension> H2) {
return a1.bearing(a2, H1, H2);
}
};
// Helper class for to implement Range traits for classes with a range method
template <class A1, typename A2, class T = double>
struct HasRange {
typedef T result_type;
T operator()(
const A1& a1, const A2& a2,
OptionalJacobian<traits<T>::dimension, traits<A1>::dimension> H1,
OptionalJacobian<traits<T>::dimension, traits<A2>::dimension> H2) {
return a1.range(a2, H1, H2);
}
};
} // namespace gtsam

50
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>
@ -384,47 +385,22 @@ private:
/// @}
};
template<>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
};
template<>
struct traits<const CalibratedCamera> : public internal::Manifold<
CalibratedCamera> {
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
// manifold traits
template <>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template <>
struct Range<CalibratedCamera, Point3> {
typedef double result_type;
double operator()(const CalibratedCamera& camera, const Point3& point,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return camera.range(point, H1, H2);
}
};
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
// range traits, used in RangeFactor
template <>
struct Range<CalibratedCamera, Point3> : HasRange<CalibratedCamera, Point3> {};
template <>
struct Range<CalibratedCamera, Pose3> {
typedef double result_type;
double operator()(const CalibratedCamera& camera, const Pose3& pose,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(pose, H1, H2);
}
};
struct Range<CalibratedCamera, Pose3> : HasRange<CalibratedCamera, Pose3> {};
template <>
struct Range<CalibratedCamera, CalibratedCamera> {
typedef double result_type;
double operator()(const CalibratedCamera& camera1,
const CalibratedCamera& camera2,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera1.range(camera2, H1, H2);
}
};
}
struct Range<CalibratedCamera, CalibratedCamera> : HasRange<CalibratedCamera, CalibratedCamera> {};
} // namespace gtsam

68
gtsam/geometry/PinholeCamera.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/PinholePose.h>
#include <gtsam/geometry/BearingRange.h>
namespace gtsam {
@ -302,62 +303,31 @@ private:
};
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> > {
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
// manifold traits
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Point3> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const Point3& point,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return camera.range(point, H1, H2);
}
};
struct traits<PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Pose3> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const Pose3& pose,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(pose, H1, H2);
}
};
struct traits<const PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
// range traits, used in RangeFactor
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Point3>
: HasRange<PinholeCamera<Calibration>, Point3> {};
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, Pose3>
: HasRange<PinholeCamera<Calibration>, Pose3> {};
template <typename CalibrationA, typename CalibrationB>
struct Range<PinholeCamera<CalibrationA>, PinholeCamera<CalibrationB> > {
typedef double result_type;
typedef PinholeCamera<CalibrationA> CameraA;
typedef PinholeCamera<CalibrationB> CameraB;
double operator()(const CameraA& camera1, const CameraB& camera2,
OptionalJacobian<1, CameraA::dimension> H1 = boost::none,
OptionalJacobian<1, CameraB::dimension> H2 = boost::none) {
return camera1.range(camera2, H1, H2);
}
};
struct Range<PinholeCamera<CalibrationA>, PinholeCamera<CalibrationB> >
: HasRange<PinholeCamera<CalibrationA>, PinholeCamera<CalibrationB> > {};
template <typename Calibration>
struct Range<PinholeCamera<Calibration>, CalibratedCamera> {
typedef double result_type;
typedef PinholeCamera<Calibration> Camera;
double operator()(const Camera& camera, const CalibratedCamera& cc,
OptionalJacobian<1, Camera::dimension> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return camera.range(cc, H1, H2);
}
};
struct Range<PinholeCamera<Calibration>, CalibratedCamera>
: HasRange<PinholeCamera<Calibration>, CalibratedCamera> {};
} // \ gtsam

50
gtsam/geometry/Pose2.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
@ -290,57 +291,24 @@ 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> {};
// Define Bearing functor specializations that are used in BearingFactor
template <typename A1, typename A2> struct Bearing;
// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose2, Point2> : HasBearing<Pose2, Point2, Rot2> {};
template <>
struct Bearing<Pose2, Point2> {
typedef Rot2 result_type;
Rot2 operator()(const Pose2& pose, const Point2& point,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return pose.bearing(point, H1, H2);
}
};
struct Bearing<Pose2, Pose2> : HasBearing<Pose2, Pose2, Rot2> {};
template <>
struct Bearing<Pose2, Pose2> {
typedef Rot2 result_type;
Rot2 operator()(const Pose2& pose1, const Pose2& pose2,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose1.bearing(pose2, H1, H2);
}
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
struct Range<Pose2, Point2> : HasRange<Pose2, Point2> {};
template <>
struct Range<Pose2, Point2> {
typedef double result_type;
double operator()(const Pose2& pose, const Point2& point,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none) {
return pose.range(point, H1, H2);
}
};
template <>
struct Range<Pose2, Pose2> {
typedef double result_type;
double operator()(const Pose2& pose1, const Pose2& pose2,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
struct Range<Pose2, Pose2> : HasRange<Pose2, Pose2> {};
} // namespace gtsam

37
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>
@ -337,40 +338,14 @@ struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
// Define Bearing functor specializations that are used in BearingFactor
template <typename A1, typename A2> struct Bearing;
// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
template <>
struct Bearing<Pose3, Point3> {
typedef Unit3 result_type;
Unit3 operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) {
return pose.bearing(point, H1, H2);
}
};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
struct Range<Pose3, Point3> : HasRange<Pose3, Point3> {};
template <>
struct Range<Pose3, Point3> {
typedef double result_type;
double operator()(const Pose3& pose, const Point3& point,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return pose.range(point, H1, H2);
}
};
template <>
struct Range<Pose3, Pose3> {
typedef double result_type;
double operator()(const Pose3& pose1, const Pose3& pose2,
OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
struct Range<Pose3, Pose3> : HasRange<Pose3, Pose3> {};
} // namespace gtsam

53
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,34 +126,28 @@ public:
};
template<>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
template<>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {
};
template <>
struct Range<SimpleCamera, Point3> {
typedef double result_type;
double operator()(const SimpleCamera& camera, const Point3& point,
OptionalJacobian<1, 11> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return camera.range(point, H1, H2);
}
};
template <>
struct Range<SimpleCamera, SimpleCamera> {
typedef double result_type;
double operator()(const SimpleCamera& camera, const SimpleCamera& sc,
OptionalJacobian<1, 11> H1 = boost::none,
OptionalJacobian<1, 11> H2 = boost::none) {
return camera.range(sc, H1, H2);
}
};
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}
// manifold traits
template <>
struct traits<SimpleCamera> : public internal::Manifold<SimpleCamera> {};
template <>
struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
// range traits, used in RangeFactor
template <>
struct Range<SimpleCamera, Point3> : HasRange<SimpleCamera, Point3> {};
template <>
struct Range<SimpleCamera, Pose3> : HasRange<SimpleCamera, Pose3> {};
template <>
struct Range<SimpleCamera, SimpleCamera> : HasRange<SimpleCamera, SimpleCamera> {};
template <typename Calibration>
struct Range<SimpleCamera, PinholeCamera<Calibration> >
: HasRange<SimpleCamera, PinholeCamera<Calibration> > {};
} // namespace gtsam

1
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -357,6 +357,7 @@ struct Range<Vector3, Vector3> {
double operator()(const Vector3& v1, const Vector3& v2,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) {
return (v2 - v1).norm();
// derivatives not implemented
}
};
}