15#include <gtsam/geometry/OrientedPlane3.h>
22typedef Expression<Point2> Point2_;
23typedef Expression<Rot2> Rot2_;
24typedef Expression<Pose2> Pose2_;
26inline Point2_
transformTo(
const Pose2_& x,
const Point2_& p) {
30inline Double_ range(
const Point2_& p,
const Point2_& q) {
31 return Double_(Range<Point2, Point2>(), p, q);
36typedef Expression<Point3> Point3_;
37typedef Expression<Unit3> Unit3_;
38typedef Expression<Rot3> Rot3_;
39typedef Expression<Pose3> Pose3_;
40typedef Expression<Line3> Line3_;
41typedef Expression<OrientedPlane3> OrientedPlane3_;
43inline Point3_
transformTo(
const Pose3_& x,
const Point3_& p) {
47inline Point3_ transformFrom(
const Pose3_& x,
const Point3_& p) {
51inline Line3_
transformTo(
const Pose3_ &wTc,
const Line3_ &wL) {
52 Line3 (*f)(
const Pose3 &,
const Line3 &,
53 OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &
transformTo;
54 return Line3_(f, wTc, wL);
57inline Pose3_ transformPoseTo(
const Pose3_& p,
const Pose3_& q) {
61inline Point3_
normalize(
const Point3_& a){
62 Point3 (*f)(
const Point3 &, OptionalJacobian<3, 3>) = &normalize;
66inline Point3_
cross(
const Point3_& a,
const Point3_& b) {
68 OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &
cross;
69 return Point3_(f, a, b);
72inline Double_
dot(
const Point3_& a,
const Point3_& b) {
74 OptionalJacobian<1, 3>, OptionalJacobian<1, 3>) = &
dot;
75 return Double_(f, a, b);
80inline Rot3 rotation(
const Pose3& pose, OptionalJacobian<3, 6> H) {
81 return pose.rotation(H);
84inline Point3 translation(
const Pose3& pose, OptionalJacobian<3, 6> H) {
85 return pose.translation(H);
89inline Rot3_ rotation(
const Pose3_& pose) {
90 return Rot3_(internal::rotation, pose);
93inline Point3_ translation(
const Pose3_& pose) {
94 return Point3_(internal::translation, pose);
97inline Point3_
rotate(
const Rot3_& x,
const Point3_& p) {
101inline Point3_ point3(
const Unit3_& v) {
105inline Unit3_
rotate(
const Rot3_& x,
const Unit3_& p) {
109inline Point3_ unrotate(
const Rot3_& x,
const Point3_& p) {
113inline Unit3_ unrotate(
const Rot3_& x,
const Unit3_& p) {
117inline Double_ distance(
const OrientedPlane3_& p) {
121inline Unit3_ normal(
const OrientedPlane3_& p) {
127typedef Expression<Cal3_S2> Cal3_S2_;
128typedef Expression<Cal3Bundler> Cal3Bundler_;
136inline Point2_
project(
const Unit3_& p_cam) {
138 return Point2_(f, p_cam);
143template <
class CAMERA,
class POINT>
144Point2 project4(
const CAMERA& camera,
const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam,
145 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
146 return camera.project2(p, Dcam, Dpoint);
150template <
class CAMERA,
class POINT>
151Point2_ project2(
const Expression<CAMERA>& camera_,
const Expression<POINT>& p_) {
152 return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
157template <
class CALIBRATION,
class POINT>
158inline Point2 project6(
const Pose3& x,
const Point3& p,
const Cal3_S2& K,
159 OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
160 OptionalJacobian<2, 5> Dcal) {
161 return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
165template <
class CALIBRATION,
class POINT>
166inline Point2_ project3(
const Pose3_& x,
const Expression<POINT>& p,
167 const Expression<CALIBRATION>& K) {
168 return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
171template <
class CALIBRATION>
172Point2_ uncalibrate(
const Expression<CALIBRATION>& K,
const Point2_& xy_hat) {
173 return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
176template <
class CALIBRATION>
177inline Pose3_ getPose(
const Expression<PinholeCamera<CALIBRATION> > & cam) {
P rotate(const T &r, const P &pt)
rotation functions
Definition: lieProxies.h:47
Base class for all pinhole cameras.
4 dimensional manifold of 3D lines
The most common 5DOF 3D->2D calibration.
Calibration used by Bundler.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition: expressions.h:192
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
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
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
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
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition: Point3.cpp:52
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:131
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
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
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition: OrientedPlane3.h:134
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition: OrientedPlane3.h:128
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition: PinholeCamera.h:169
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
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
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
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
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
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
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
Expression class that supports automatic differentiation.
Definition: Expression.h:48
Common expressions, both linear and non-linear.