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

Support bearing factors between Pose3 values 

I am modelling a system in which there are two bodies, and one can
observe the other but cannot estimate the other's pose. This is
perfectly modeled by a BearingRangeFactor, but without this patch, you
cannot make a BearingRangeFactor between two Pose3 values. This adds
support for that by extending the Pose3 class to support calling
bearing() on another Pose3.
release/4.3a0
Haldean Brown 2017-03-09 16:57:29 -08:00
parent fbb9d3bdda
commit 68d26ff279
2 changed files with 21 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Pose3.cpp
View File

@ -378,6 +378,15 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> H1,
}
}
/* ************************************************************************* */
Unit3 Pose3::bearing(const Pose3& point, OptionalJacobian<2, 6> H1,
OptionalJacobian<2, 6> H2) const {
if (H2) {
return bearing(point.translation(), H1, H2.cols<3>(3));
}
return bearing(point.translation(), H1, boost::none);
}
/* ************************************************************************* */
boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs) {
const size_t n = abPointPairs.size();

12
gtsam/geometry/Pose3.h
View File

@ -282,6 +282,15 @@ public:
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 3> H2 = boost::none) const;
/**
* Calculate bearing to another pose
* @param other 3D location and orientation of other body. The orientation
* information is ignored.
* @return bearing (Unit3)
*/
Unit3 bearing(const Pose3& point, OptionalJacobian<2, 6> H1 = boost::none,
OptionalJacobian<2, 6> H2 = boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
@ -353,6 +362,9 @@ struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
template<>
struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
template <typename T>
struct Range<Pose3, T> : HasRange<Pose3, T, double> {};