Loading...
Searching...
No Matches
expressions.h
Go to the documentation of this file.
#pragma once
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/Pose2.h>
#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 {
// 2D Geometry
typedef Expression<Point2> Point2_;
typedef Expression<Rot2> Rot2_;
typedef Expression<Pose2> Pose2_;
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_;
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);
}
inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transformFrom, p);
}
inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
Line3 (*f)(const Pose3 &, const Line3 &,
OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo;
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);
}
inline Point3_ cross(const Point3_& a, const Point3_& b) {
Point3 (*f)(const Point3 &, const Point3 &,
OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;
return Point3_(f, a, b);
}
inline Double_ dot(const Point3_& a, const Point3_& b) {
double (*f)(const Point3 &, const Point3 &,
OptionalJacobian<1, 3>, OptionalJacobian<1, 3>) = &dot;
return Double_(f, a, b);
}
namespace internal {
// define getter that returns value rather than reference
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);
}
inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
return Point3_(x, &Rot3::unrotate, p);
}
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_;
typedef Expression<Cal3Bundler> Cal3Bundler_;
inline Point2_ project(const Point3_& p_cam) {
Point2 (*f)(const Point3&, OptionalJacobian<2, 3>) = &PinholeBase::Project;
return Point2_(f, p_cam);
}
inline Point2_ project(const Unit3_& p_cam) {
Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
return Point2_(f, p_cam);
}
namespace internal {
// Helper template for project2 expression below
template <class CAMERA, class POINT>
Point2 project4(const CAMERA& camera, const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
return camera.project2(p, Dcam, Dpoint);
}
}
template <class CAMERA, class POINT>
Point2_ project2(const Expression<CAMERA>& camera_, const Expression<POINT>& p_) {
return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
}
namespace internal {
// Helper template for project3 expression below
template <class CALIBRATION, class POINT>
inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
OptionalJacobian<2, 5> Dcal) {
return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
}
}
template <class CALIBRATION, class POINT>
inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
const Expression<CALIBRATION>& K) {
return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
}
template <class CALIBRATION>
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);
}
// TODO(dellaert): Should work but fails because of a type deduction conflict.
// template <typename T>
// gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
// const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
// return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
// x1, &T::logmap, x2);
// }
template <typename T>
gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
return Expression<typename gtsam::traits<T>::TangentVector>(
gtsam::traits<T>::Logmap, between(x1, x2));
}
} // \namespace gtsam
Pose2.h
2D Pose
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
Line3.h
4 dimensional manifold of 3D lines
PinholeCamera.h
Base class for all pinhole cameras.
Cal3Bundler.h
Calibration used by Bundler.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::logmap
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition expressions.h:192
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
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::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::Range
Definition BearingRange.h:40
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost::none)
Project from 3D point in camera coordinates into image Does not throw a CheiralityException,...
Definition CalibratedCamera.cpp:88
gtsam::Line3
A 3D line (R,a,b) : (Rot3,Scalar,Scalar).
Definition Line3.h:44
gtsam::OrientedPlane3::distance
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition OrientedPlane3.h:134
gtsam::OrientedPlane3::normal
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition OrientedPlane3.h:128
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholeCamera::getPose
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition PinholeCamera.h:169
gtsam::Pose2::transformTo
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition Pose2.cpp:207
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition Pose3.cpp:371
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
gtsam::Pose3::transformPoseTo
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself=boost::none, OptionalJacobian< 6, 6 > HwTb=boost::none) const
Assuming self == wTa, takes a pose wTb in world coordinates and transforms it to local coordinates aT...
Definition Pose3.cpp:338
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition Rot3.cpp:136
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition Unit3.cpp:144
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
expressions.h
Common expressions, both linear and non-linear.