diff --git a/doc/math.lyx b/doc/math.lyx index b579d3ea4..2533822a7 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -1,7 +1,9 @@ -#LyX 2.1 created this file. For more info see http://www.lyx.org/ -\lyxformat 474 +#LyX 2.3 created this file. For more info see http://www.lyx.org/ +\lyxformat 544 \begin_document \begin_header +\save_transient_properties true +\origin unavailable \textclass article \use_default_options false \begin_modules @@ -14,16 +16,18 @@ theorems-ams-bytype \language_package default \inputencoding auto \fontencoding global -\font_roman times -\font_sans default -\font_typewriter default -\font_math auto +\font_roman "times" "default" +\font_sans "default" "default" +\font_typewriter "default" "default" +\font_math "auto" "auto" \font_default_family rmdefault \use_non_tex_fonts false \font_sc false \font_osf false -\font_sf_scale 100 -\font_tt_scale 100 +\font_sf_scale 100 100 +\font_tt_scale 100 100 +\use_microtype false +\use_dash_ligatures true \graphics default \default_output_format default \output_sync 0 @@ -53,6 +57,7 @@ theorems-ams-bytype \suppress_date false \justification true \use_refstyle 0 +\use_minted 0 \index Index \shortcut idx \color #008000 @@ -65,7 +70,10 @@ theorems-ams-bytype \tocdepth 3 \paragraph_separation indent \paragraph_indentation default -\quotes_language english +\is_math_indent 0 +\math_numbering_side default +\quotes_style english +\dynamic_quotes 0 \papercolumns 1 \papersides 1 \paperpagestyle default @@ -98,6 +106,11 @@ width "100col%" special "none" height "1in" height_special "totalheight" +thickness "0.4pt" +separation "3pt" +shadowsize "4pt" +framecolor "black" +backgroundcolor "none" status collapsed \begin_layout Plain Layout @@ -654,6 +667,7 @@ reference "eq:LocalBehavior" \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -934,6 +948,7 @@ See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -1025,6 +1040,7 @@ See \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -2209,6 +2225,7 @@ instantaneous velocity LatexCommand cite after "page 51 for rotations, page 419 for SE(3)" key "Murray94book" +literal "true" \end_inset @@ -2965,7 +2982,7 @@ B^{T} & I_{3}\end{array}\right] \begin_layout Subsection \begin_inset CommandInset label LatexCommand label -name "sub:Pushforward-of-Between" +name "subsec:Pushforward-of-Between" \end_inset @@ -3419,6 +3436,7 @@ A retraction \begin_inset CommandInset citation LatexCommand cite key "Absil07book" +literal "true" \end_inset @@ -3873,7 +3891,7 @@ BetweenFactor , derived in Section \begin_inset CommandInset ref LatexCommand ref -reference "sub:Pushforward-of-Between" +reference "subsec:Pushforward-of-Between" \end_inset @@ -4430,7 +4448,7 @@ In the case of \begin_inset Formula $\SOthree$ \end_inset - the vector space is + the vector space is \begin_inset Formula $\Rthree$ \end_inset @@ -4502,7 +4520,7 @@ reference "Th:InverseAction" \begin_layout Subsection \begin_inset CommandInset label LatexCommand label -name "sub:3DAngularVelocities" +name "subsec:3DAngularVelocities" \end_inset @@ -4695,6 +4713,7 @@ Absil LatexCommand cite after "page 58" key "Absil07book" +literal "true" \end_inset @@ -5395,6 +5414,7 @@ While not a Lie group, we can define an exponential map, which is given \begin_inset CommandInset citation LatexCommand cite key "Ma01ijcv" +literal "true" \end_inset @@ -5402,6 +5422,7 @@ key "Ma01ijcv" \begin_inset CommandInset href LatexCommand href name "http://stat.fsu.edu/~anuj/CVPR_Tutorial/Part2.pdf" +literal "false" \end_inset @@ -5605,6 +5626,7 @@ The exponential map uses \begin_inset CommandInset citation LatexCommand cite key "Absil07book" +literal "true" \end_inset @@ -6293,7 +6315,7 @@ d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right) \end_layout \begin_layout Section -Line3 (Ocaml) +Line3 \end_layout \begin_layout Standard @@ -6345,6 +6367,14 @@ R'=R(I+\Omega) \end_inset + +\end_layout + +\begin_layout Subsection +Projecting Line3 +\end_layout + +\begin_layout Standard Projecting a line to 2D can be done easily, as both \begin_inset Formula $v$ \end_inset @@ -6430,13 +6460,21 @@ or the \end_layout +\begin_layout Subsection +Action of +\begin_inset Formula $\SEthree$ +\end_inset + + on the line +\end_layout + \begin_layout Standard Transforming a 3D line \begin_inset Formula $(R,(a,b))$ \end_inset from a world coordinate frame to a camera frame -\begin_inset Formula $(R_{w}^{c},t^{w})$ +\begin_inset Formula $T_{c}^{w}=(R_{c}^{w},t^{w})$ \end_inset is done by @@ -6466,17 +6504,115 @@ b'=b-R_{2}^{T}t^{w} \end_inset -Again, we need to redo the derivatives, as R is incremented from the right. - The first argument is incremented from the left, but the result is incremented - on the right: +where +\begin_inset Formula $R_{1}$ +\end_inset + + and +\begin_inset Formula $R_{2}$ +\end_inset + + are the columns of +\begin_inset Formula $R$ +\end_inset + + , as before. + +\end_layout + +\begin_layout Standard +To find the derivatives, the transformation of a line +\begin_inset Formula $l^{w}=(R,a,b)$ +\end_inset + + from world coordinates to a camera coordinate frame +\begin_inset Formula $T_{c}^{w}$ +\end_inset + +, specified in world coordinates, can be written as a function +\begin_inset Formula $f:\SEthree\times L\rightarrow L$ +\end_inset + +, as given above, i.e., \begin_inset Formula -\begin{eqnarray*} -R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\ -I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\ -\Omega' & = & R'^{T}\Skew{S\omega}R'\\ -\Omega' & = & \Skew{R'^{T}S\omega}\\ -\omega' & = & R'^{T}S\omega -\end{eqnarray*} +\[ +f(T_{c}^{w},l^{w})=\left(\left(R_{c}^{w}\right)^{T}R,a-R_{1}^{T}t^{w},b-R_{2}^{T}t^{w}\right). +\] + +\end_inset + +Let us find the Jacobian +\begin_inset Formula $J_{1}$ +\end_inset + + of +\begin_inset Formula $f$ +\end_inset + + with respect to the first argument +\begin_inset Formula $T_{c}^{w}$ +\end_inset + +, which should obey +\begin_inset Formula +\begin{align*} +f(T_{c}^{w}e^{\xihat},l^{w}) & \approx f(T_{c}^{w},l^{w})+J_{1}\xi +\end{align*} + +\end_inset + +Note that +\begin_inset Formula +\[ +T_{c}^{w}e^{\xihat}\approx\left[\begin{array}{cc} +R_{c}^{w}\left(I_{3}+\Skew{\omega}\right) & t^{w}+R_{c}^{w}v\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +Let's write this out separately for each of +\begin_inset Formula $R,a,b$ +\end_inset + +: +\begin_inset Formula +\begin{align*} +\left(R_{c}^{w}\left(I_{3}+\Skew{\omega}\right)\right)^{T}R & \approx\left(R_{c}^{w}\right)^{T}R(I+\left[J_{R\omega}\omega\right]_{\times})\\ +a-R_{1}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx a-R_{1}^{T}t^{w}+J_{av}v\\ +b-R_{2}^{T}\left(t^{w}+R_{c}^{w}v\right) & \approx b-R_{2}^{T}t^{w}+J_{bv}v +\end{align*} + +\end_inset + +Simplifying, we get: +\begin_inset Formula +\begin{align*} +-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ +-R_{1}^{T}R_{c}^{w} & \approx J_{av}\\ +-R_{2}^{T}R_{c}^{w} & \approx J_{bv} +\end{align*} + +\end_inset + +which gives the expressions for +\begin_inset Formula $J_{av}$ +\end_inset + + and +\begin_inset Formula $J_{bv}$ +\end_inset + +. + The top line can be further simplified: +\begin_inset Formula +\begin{align*} +-\Skew{\omega}R' & \approx R'\left[J_{R\omega}\omega\right]_{\times}\\ +-R'^{T}\Skew{\omega}R' & \approx\left[J_{R\omega}\omega\right]_{\times}\\ +-\Skew{R'^{T}\omega} & \approx\left[J_{R\omega}\omega\right]_{\times}\\ +-R'^{T} & \approx J_{R\omega} +\end{align*} \end_inset @@ -6687,6 +6823,7 @@ Spivak \begin_inset CommandInset citation LatexCommand cite key "Spivak65book" +literal "true" \end_inset @@ -6795,6 +6932,7 @@ The following is adapted from Appendix A in \begin_inset CommandInset citation LatexCommand cite key "Murray94book" +literal "true" \end_inset @@ -6924,6 +7062,7 @@ might be LatexCommand cite after "page 45" key "Hall00book" +literal "true" \end_inset diff --git a/doc/math.pdf b/doc/math.pdf index ab8f1f69a..8dc7270f1 100644 Binary files a/doc/math.pdf and b/doc/math.pdf differ diff --git a/gtsam/geometry/Line3.cpp b/gtsam/geometry/Line3.cpp new file mode 100644 index 000000000..e3b4841e0 --- /dev/null +++ b/gtsam/geometry/Line3.cpp @@ -0,0 +1,120 @@ +#include + +namespace gtsam { + +Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4, 4> Dp, OptionalJacobian<4, 4> Dv) const { + Vector3 w; + w << v[0], v[1], 0; + Rot3 incR; + + if (Dp) { + Dp->setIdentity(); + incR = Rot3::Expmap(w); + Dp->block<2, 2>(0, 0) = ((incR.matrix()).transpose()).block<2, 2>(0, 0); + } + if (Dv) { + Matrix3 Dw; + incR = Rot3::Expmap(w, Dw); + Dv->setIdentity(); + Dv->block<2, 2>(0, 0) = Dw.block<2, 2>(0, 0); + } else { + incR = Rot3::Expmap(w); + } + Rot3 Rt = R_ * incR; + return Line3(Rt, a_ + v[2], b_ + v[3]); +} + +Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4, 4> Dp, + OptionalJacobian<4, 4> Dq) const { + Vector3 omega; + Matrix3 D_log; + omega = Rot3::Logmap(R_.inverse() * q.R_, D_log); + if (Dp) { + Matrix3 D_log_wp = -((q.R_).matrix()).transpose() * R_.matrix(); + Matrix3 Dwp = D_log * D_log_wp; + Dp->setIdentity(); + Dp->block<2, 2>(0, 0) = Dwp.block<2, 2>(0, 0); + (*Dp)(2, 2) = -1; + (*Dp)(3, 3) = -1; + } + if (Dq) { + Dq->setIdentity(); + Dq->block<2, 2>(0, 0) = D_log.block<2, 2>(0, 0); + } + Vector4 local; + local << omega[0], omega[1], q.a_ - a_, q.b_ - b_; + return local; +} + +void Line3::print(const std::string &s) const { + std::cout << s << std::endl; + R_.print("R:\n"); + std::cout << "a: " << a_ << ", b: " << b_ << std::endl; +} + +bool Line3::equals(const Line3 &l2, double tol) const { + Vector4 diff = localCoordinates(l2); + return fabs(diff[0]) < tol && fabs(diff[1]) < tol + && fabs(diff[2]) < tol && fabs(diff[3]) < tol; +} + +Unit3 Line3::project(OptionalJacobian<2, 4> Dline) const { + Vector3 V_0; + V_0 << -b_, a_, 0.0; + + Unit3 l; + if (Dline) { + // Jacobian of the normalized Unit3 projected line with respect to + // un-normalized Vector3 projected line in homogeneous coordinates. + Matrix23 D_unit_line; + l = Unit3::FromPoint3(Point3(R_ * V_0), D_unit_line); + // Jacobian of the un-normalized Vector3 line with respect to + // input 3D line + Matrix34 D_vec_line = Matrix34::Zero(); + D_vec_line.col(0) = a_ * R_.r3(); + D_vec_line.col(1) = b_ * R_.r3(); + D_vec_line.col(2) = R_.r2(); + D_vec_line.col(3) = -R_.r1(); + // Jacobian of output wrt input is the product of the two. + *Dline = D_unit_line * D_vec_line; + } else { + l = Unit3::FromPoint3(Point3(R_ * V_0)); + } + return l; +} + +Point3 Line3::point(double distance) const { + // defining "center" of the line to be the point where it + // intersects rotated XY axis + Point3 center(a_, b_, 0); + Point3 rotated_center = R_ * center; + return rotated_center + distance * R_.r3(); +} + +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline) { + Rot3 wRc = wTc.rotation(); + Rot3 cRw = wRc.inverse(); + Rot3 cRl = cRw * wL.R_; + + Vector2 w_ab; + Vector3 t = ((wL.R_).transpose() * wTc.translation()); + Vector2 c_ab(wL.a_ - t[0], wL.b_ - t[1]); + + if (Dpose) { + Matrix3 lRc = (cRl.matrix()).transpose(); + Dpose->setZero(); + // rotation + Dpose->block<2, 3>(0, 0) = -lRc.block<2, 3>(0, 0); + // translation + Dpose->block<2, 3>(2, 3) = -lRc.block<2, 3>(0, 0); + } + if (Dline) { + Dline->setIdentity(); + (*Dline)(0, 3) = -t[2]; + (*Dline)(1, 2) = t[2]; + } + return Line3(cRl, c_ab[0], c_ab[1]); +} + +} \ No newline at end of file diff --git a/gtsam/geometry/Line3.h b/gtsam/geometry/Line3.h new file mode 100644 index 000000000..1c7ed2f4c --- /dev/null +++ b/gtsam/geometry/Line3.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file Line3.h + * @brief 4 dimensional manifold of 3D lines + * @author Akshay Krishnan + * @author Frank Dellaert + */ +// \callgraph + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * A 3D line (R,a,b) : (Rot3,Scalar,Scalar) + * @addtogroup geometry + * \nosubgrouping + */ +class Line3 { + private: + Rot3 R_; // Rotation of line about x and y in world frame + double a_, b_; // Intersection of line with the world x-y plane rotated by R_ + // Also the closest point on line to origin + public: + enum { dimension = 4 }; + + /** Default constructor is the Z axis **/ + Line3() : + a_(0), b_(0) {} + + /** Constructor for general line from (R, a, b) **/ + Line3(const Rot3 &R, const double a, const double b) : + R_(R), a_(a), b_(b) {} + + /** + * The retract method maps from the tangent space back to the manifold. + * The method q = p + v, where p is this line, v is an increment along + * the tangent space and q is the resulting line. + * The tangent space for the rotation of a line is only two dimensional - + * rotation about x and y + * @param v: increment in tangent space + * @param Dp: increment of retraction with respect to this line + * @param Dv: Jacobian of retraction with respect to the increment + * @return q: resulting line after adding the increment and mapping to the manifold + */ + Line3 retract(const Vector4 &v, + OptionalJacobian<4, 4> Dp = boost::none, + OptionalJacobian<4, 4> Dv = boost::none) const; + + /** + * The localCoordinates method is the inverse of retract and finds the difference + * between two lines in the tangent space. It computes v = q - p where q is an input + * line, p is this line and v is their difference in the tangent space. + * @param q: Line3 on manifold + * @param Dp: OptionalJacobian of localCoordinates with respect to this line + * @param Dq: OptionalJacobian of localCoordinates with respect to this line + * @return v: difference in the tangent space + */ + Vector4 localCoordinates(const Line3 &q, + OptionalJacobian<4, 4> Dp = boost::none, + OptionalJacobian<4, 4> Dq = boost::none) const; + + /** + * Print R, a, b + * @param s: optional starting string + */ + void print(const std::string &s = "") const; + + /** + * Check if two lines are equal + * @param l2 - line to be compared + * @param tol : optional tolerance + * @return boolean - true if lines are equal + */ + bool equals(const Line3 &l2, double tol = 10e-9) const; + + /** + * Projecting a line to the image plane. Assumes this line is in camera frame. + * @param Dline: OptionalJacobian of projected line with respect to this line + * @return Unit3 - projected line in image plane, in homogenous coordinates. + * We use Unit3 since it is a manifold with the right dimension. + */ + Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const; + + /** + * Returns point on the line that is at a certain distance starting from the + * point where the rotated XY axis intersects the line. + * @param distance (can be positive or negative) - positive is the positive + * direction of the rotated z axis that forms the line. The default value of zero + * returns the point where the rotated XY axis intersects the line. + * @return Point on the line + */ + Point3 point(double distance = 0) const; + + /** + * Transform a line from world to camera frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame + * @param Dpose - OptionalJacobian of transformed line with respect to p + * @param Dline - OptionalJacobian of transformed line with respect to l + * @return Transformed line in camera frame + */ + friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose, + OptionalJacobian<4, 4> Dline); +}; + +/** + * Transform a line from world to camera frame + * @param wTc - Pose3 of camera in world frame + * @param wL - Line3 in world frame + * @param Dpose - OptionalJacobian of transformed line with respect to p + * @param Dline - OptionalJacobian of transformed line with respect to l + * @return Transformed line in camera frame + */ +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, + OptionalJacobian<4, 6> Dpose = boost::none, + OptionalJacobian<4, 4> Dline = boost::none); + +template<> +struct traits : public internal::Manifold {}; + +template<> +struct traits : public internal::Manifold {}; +} \ No newline at end of file diff --git a/gtsam/geometry/tests/testLine3.cpp b/gtsam/geometry/tests/testLine3.cpp new file mode 100644 index 000000000..9812c0c2d --- /dev/null +++ b/gtsam/geometry/tests/testLine3.cpp @@ -0,0 +1,155 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Line3) +GTSAM_CONCEPT_MANIFOLD_INST(Line3) + +static const Line3 l(Rot3(), 1, 1); + +// Testing equals function of Line3 +TEST(Line3, equals) { + Line3 l_same = l; + EXPECT(l.equals(l_same)); + Line3 l2(Rot3(), 1, 2); + EXPECT(!l.equals(l2)); +} + +// testing localCoordinates along 4 dimensions +TEST(Line3, localCoordinates) { + // l1 and l differ only in a_ + Line3 l1(Rot3(), 2, 1); + Vector4 v1(0, 0, -1, 0); + CHECK(assert_equal(l1.localCoordinates(l), v1)); + + // l2 and l differ only in b_ + Line3 l2(Rot3(), 1, 2); + Vector4 v2(0, 0, 0, -1); + CHECK(assert_equal(l2.localCoordinates(l), v2)); + + // l3 and l differ in R_x + Rot3 r3 = Rot3::Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + Vector4 v3(-M_PI / 4, 0, 0, 0); + CHECK(assert_equal(l3.localCoordinates(l), v3)); + + // l4 and l differ in R_y + Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + Vector4 v4(0, -M_PI / 4, 0, 0); + CHECK(assert_equal(l4.localCoordinates(l), v4)); + + // Jacobians + Line3 l5(Rot3::Expmap(Vector3(M_PI / 3, -M_PI / 4, 0)), -0.8, 2); + Values val; + val.insert(1, l); + val.insert(2, l5); + Line3_ l_(1); + Line3_ l5_(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + Vector4_ local_(l5_, &Line3::localCoordinates, l_); + ExpressionFactor f(model, l5.localCoordinates(l), local_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// testing retract along 4 dimensions +TEST(Line3, retract) { + // l1 and l differ only in a_ + Vector4 v1(0, 0, 0, 1); + Line3 l1(Rot3(), 1, 2); + EXPECT(l1.equals(l.retract(v1))); + + // l2 and l differ only in b_ + Vector4 v2(0, 0, 1, 0); + Line3 l2(Rot3(), 2, 1); + EXPECT(l2.equals(l.retract(v2))); + + // l3 and l differ in R_x + Vector4 v3(M_PI / 4, 0, 0, 0); + Rot3 r3; + r3 = r3.Expmap(Vector3(M_PI / 4, 0, 0)); + Line3 l3(r3, 1, 1); + EXPECT(l3.equals(l.retract(v3))); + + // l4 and l differ in R_y + Vector4 v4(0, M_PI / 4, 0, 0); + Rot3 r4 = Rot3::Expmap(Vector3(0, M_PI / 4, 0)); + Line3 l4(r4, 1, 1); + EXPECT(l4.equals(l.retract(v4))); + + // Jacobians + Vector4 v5(M_PI / 3, -M_PI / 4, -0.4, 1.2); // arbitrary vector + Values val; + val.insert(1, l); + val.insert(2, v5); + Line3_ l_(1); + Vector4_ v5_(2); + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + Line3_ retract_(l_, &Line3::retract, v5_); + ExpressionFactor f(model, l.retract(v5), retract_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// testing manifold property - Retract(p, Local(p,q)) == q +TEST(Line3, retractOfLocalCoordinates) { + Rot3 r2 = Rot3::Expmap(Vector3(M_PI / 4, M_PI / 3, 0)); + Line3 l2(r2, 5, 9); + EXPECT(assert_equal(l.retract(l.localCoordinates(l2)), l2)); +} + +// testing manifold property - Local(p, Retract(p, v)) == v +TEST(Line3, localCoordinatesOfRetract) { + Vector4 r2(2.3, 0.987, -3, 4); + EXPECT(assert_equal(l.localCoordinates(l.retract(r2)), r2)); +} + +// transform from world to camera test +TEST(Line3, transformToExpressionJacobians) { + Rot3 r = Rot3::Expmap(Vector3(0, M_PI / 3, 0)); + Vector3 t(0, 0, 0); + Pose3 p(r, t); + + Line3 l_c(r.inverse(), 1, 1); + Line3 l_w(Rot3(), 1, 1); + EXPECT(l_c.equals(transformTo(p, l_w))); + + Line3_ l_(1); + Pose3_ p_(2); + Values val; + val.insert(1, l_w); + val.insert(2, p); + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(4, 0.1); + ExpressionFactor f(model, transformTo(p, l_w), transformTo(p_, l_)); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +// projection in camera frame test +TEST(Line3, projection) { + Rot3 r = Rot3::Expmap(Vector3(0, 0, 0)); + Line3 wL(r, 1, 1); + + Unit3 expected = Unit3::FromPoint3(Point3(-1, 1, 0)); + EXPECT(expected.equals(wL.project())); + + Values val; + val.insert(1, wL); + Line3_ wL_(1); + + SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.1); + Unit3_ projected_(wL_, &Line3::project); + ExpressionFactor f(model, expected, projected_); + EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7); +} + +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 4294d17d1..d60923d8e 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -11,6 +11,7 @@ #include #include #include +#include #include namespace gtsam { @@ -31,6 +32,7 @@ typedef Expression Point3_; typedef Expression Unit3_; typedef Expression Rot3_; typedef Expression Pose3_; +typedef Expression Line3_; inline Point3_ transformTo(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transformTo, p); @@ -40,6 +42,12 @@ 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); +} + namespace internal { // define getter that returns value rather than reference inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {