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

wraps more getters in Expressions

release/4.3a0
BrettRD 2023-01-09 13:40:41 +11:00
parent 00dc32df0d
commit a8288746b4
2 changed files with 40 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/OrientedPlane3.h
View File

@ -125,12 +125,14 @@ public:
}
/// Return the normal
inline Unit3 normal() const {
inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const {
if (H) *H << I_2x2, Z_2x1;
return n_;
}
/// Return the perpendicular distance to the origin
inline double distance() const {
inline double distance(OptionalJacobian<1, 3> H = boost::none) const {
if (H) *H << 0,0,1;
return d_;
}
};

36
gtsam/slam/expressions.h
View File

@ -12,6 +12,7 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Line3.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/PinholeCamera.h>
namespace gtsam {
@ -26,6 +27,11 @@ inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
return Point2_(x, &Pose2::transformTo, p);
}
inline Double_ range(const Point2_& p, const Point2_& q)
{
return Double_(Range<Point2, Point2>(), p, q);
}
// 3D Geometry
typedef Expression<Point3> Point3_;
@ -33,6 +39,7 @@ typedef Expression<Unit3> Unit3_;
typedef Expression<Rot3> Rot3_;
typedef Expression<Pose3> Pose3_;
typedef Expression<Line3> Line3_;
typedef Expression<OrientedPlane3> OrientedPlane3_;
inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transformTo, p);
@ -48,6 +55,10 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
return Line3_(f, wTc, wL);
}
inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) {
return Pose3_(p, &Pose3::transformPoseTo, q);
}
inline Point3_ normalize(const Point3_& a){
Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
return Point3_(f, a);
@ -70,16 +81,28 @@ namespace internal {
inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
return pose.rotation(H);
}
inline Point3 translation(const Pose3& pose, OptionalJacobian<3, 6> H) {
return pose.translation(H);
}
} // namespace internal
inline Rot3_ rotation(const Pose3_& pose) {
return Rot3_(internal::rotation, pose);
}
inline Point3_ translation(const Pose3_& pose) {
return Point3_(internal::translation, pose);
}
inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
return Point3_(x, &Rot3::rotate, p);
}
inline Point3_ point3(const Unit3_& v) {
return Point3_(&Unit3::point3, v);
}
inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
return Unit3_(x, &Rot3::rotate, p);
}
@ -92,6 +115,14 @@ inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
return Unit3_(x, &Rot3::unrotate, p);
}
inline Double_ distance(const OrientedPlane3_& p) {
return Double_(&OrientedPlane3::distance, p);
}
inline Unit3_ normal(const OrientedPlane3_& p) {
return Unit3_(&OrientedPlane3::normal, p);
}
// Projection
typedef Expression<Cal3_S2> Cal3_S2_;
@ -143,6 +174,11 @@ Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
}
template <class CALIBRATION>
inline Pose3_ getPose(const Expression<PinholeCamera<CALIBRATION> > & cam) {
return Pose3_(&PinholeCamera<CALIBRATION>::getPose, cam);
}
/// logmap
// TODO(dellaert): Should work but fails because of a type deduction conflict.