line3 compiled tested

doc/math.lyx

@@ -1,7 +1,9 @@
-#LyX 2.1 created this file. For more info see http://www.lyx.org/
+#LyX 2.3 created this file. For more info see http://www.lyx.org/
-\lyxformat 474
+\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_roman "times" "default"
-\font_sans default
+\font_sans "default" "default"
-\font_typewriter default
+\font_typewriter "default" "default"
-\font_math auto
+\font_math "auto" "auto"
 \font_default_family rmdefault
 \use_non_tex_fonts false
 \font_sc false
 \font_osf false
-\font_sf_scale 100
+\font_sf_scale 100 100
-\font_tt_scale 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
 
@@ -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.
+where 
- The first argument is incremented from the left, but the result is incremented
+\begin_inset Formula $R_{1}$
- on the right:
+\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\\
+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).
-I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\
+\]
-\Omega' & = & R'^{T}\Skew{S\omega}R'\\
+
-\Omega' & = & \Skew{R'^{T}S\omega}\\
+\end_inset
-\omega' & = & R'^{T}S\omega
+
-\end{eqnarray*}
+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
 

gtsam/geometry/Line3.cpp

@@ -0,0 +1,136 @@
+#include <gtsam/geometry/Line3.h>
+
+namespace gtsam {
+
+
+Vector6 Line3::points()
+{
+    Vector3 mid;
+    mid << a_, b_, 0;
+    Vector3 mid_rot = R_*mid;
+    Vector3 start = mid_rot + R_.r3();
+    Vector3 end = mid_rot - R_.r3();
+    Vector6 start_end;
+    start_end << start(0), start(1), start(2),
+                    end(0), end(1), end(2);
+    return start_end;
+}
+
+
+Line3 Line3::retract(const Vector4 &v, OptionalJacobian<4,4> H) const {
+    Vector3 w;
+    w << v[0], v[1], 0;
+    Rot3 eps;
+    if(H)
+    {
+        OptionalJacobian<3,3> Dw;
+        eps = Rot3::Expmap(w, Dw);
+        H->block<2,2>(0,0) = Dw->block<2,2>(0,0);
+        (*H)(2,2) = 1;
+        (*H)(3,3) = 1;
+    }
+    else
+    {
+        eps = Rot3::Expmap(w);
+    }
+    Rot3 Rt = R_*eps;
+    return Line3(Rt, a_+v[2], b_+v[3]);
+}
+
+
+Vector4 Line3::localCoordinates(const Line3 &q, OptionalJacobian<4,4> H) const {
+    Vector3 local_rot;
+    Vector4 local;
+    Vector2 ab_q(q.V());
+    if(H)
+    {
+        OptionalJacobian<3,3> Dw;
+        local_rot = Rot3::Logmap(R_.inverse()*q.R(), Dw);
+        H->block<2,2>(0, 0) = Dw->block<2,2>(0,0);
+        (*H)(2,2) = 1;
+        (*H)(3,3) = 1;
+    }
+    else
+    {
+        local_rot = Rot3::Logmap(R_.inverse()*q.R());
+    }
+    local << local_rot[0], local_rot[1], ab_q[0] - a_, ab_q[1] - 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;
+}
+
+
+Point3 Line3::project(OptionalJacobian<3, 4> Dline) const
+{
+    Vector3 V_0;
+    V_0 << -b_, a_, 0;
+
+    Point3 l = R_*V_0;
+    if(Dline)
+    {
+        Dline->setZero();
+        Dline->col(0) = a_*R_.r3();
+        Dline->col(1) = b_*R_.r3();
+        Dline->col(2) = R_.r2();
+        Dline->col(3) = -R_.r1();
+    }
+    return l;
+}
+
+
+Point3 projectLine(const Line3 &L,
+        OptionalJacobian<3, 4> Dline)
+{
+    return L.project(Dline);
+}
+
+
+Line3 transformTo(const Pose3 &p, const Line3 &l,
+                    OptionalJacobian<4, 6> Dpose, OptionalJacobian<4, 4> Dline)
+{
+    Rot3 w_R_l = l.R();
+    Rot3 w_R_c = p.rotation();
+    Rot3 c_R_w = w_R_c.inverse();
+    Rot3 c_R_l = c_R_w*w_R_l;
+
+    Vector2 ab_w = l.V();
+    Vector3 t = (w_R_l.transpose()*p.translation());
+    Vector2 ab_c(ab_w[0] - t[0], ab_w[1] - t[1]);
+
+    if(Dpose)
+    {
+        // translation due to translation
+        Matrix3 c_R_l_mat = c_R_l.matrix();
+        Matrix3 l_R_c = c_R_l_mat.transpose();
+        Dpose->block<1,3>(2, 3) = -l_R_c.row(0);
+        Dpose->block<1,3>(3, 3) = -l_R_c.row(1);
+
+        Dpose->block<1,3>(0, 0) = -l_R_c.row(0);
+        Dpose->block<1,3>(1, 0) = -l_R_c.row(1);
+    }
+    if(Dline)
+    {
+        Dline->col(0) << 1.0, 0.0, 0.0, -t[2];
+        Dline->col(1) << 0.0, 1.0, t[2], 0.0;
+        Dline->col(2) << 0.0, 0.0, 1.0, 0.0;
+        Dline->col(3) << 0.0, 0.0, 0.0, 1.0;
+    }
+    return Line3(c_R_l, ab_c[0], ab_c[1]);
+}
+
+}

gtsam/geometry/Line3.h

@@ -0,0 +1,178 @@
+/* ----------------------------------------------------------------------------
+ * 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 <gtsam/base/concepts.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/SO3.h>
+#include <gtsam/nonlinear/expressions.h>
+#include <gtsam/slam/expressions.h>
+
+#include <boost/random.hpp>
+#include <cstdlib>
+#include <ctime>
+
+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 x-y world plane
+
+public:
+    enum {dimension = 4};
+
+    /** Default constructor is the Z axis **/
+    Line3() :
+            a_(0), b_(0)
+    {}
+
+    /** Constructor:
+     * Parallel to z axis, intersecting x-y plane at (a,b) **/
+    Line3(const double a, const double b):
+            a_(a), b_(b)
+    {}
+
+    /** Constructor for general line from (R, a, b) **/
+    Line3(const Rot3 &R, const double a, const double b) :
+            R_(R), a_(a), b_(b)
+    {}
+
+    /** Sample two points lying on the line
+     * @return Vector6 where first 3 elements are one point, and
+     * next 3 elements are another point **/
+    Vector6 points();
+
+    /**
+     * The retract method maps from the tangent space back to the manifold.
+     * 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 H: Jacobian of retraction with respect to the increment
+     * @return: resulting line after adding the increment and mapping to the manifold
+     */
+    Line3 retract(const Vector4 &v, OptionalJacobian<4,4> H = boost::none) const;
+
+    /**
+     * The localCoordinates method is the inverse of retract and finds the difference
+     * between two lines in the tangent space.
+     * @param q Line3 on manifold
+     * @param H OptionalJacobian of localCoordinates with respect to line
+     * @return Vector4 difference in the tangent space
+     */
+    Vector4 localCoordinates(const Line3 &q, OptionalJacobian<4,4> H = boost::none) const;
+
+    /**
+     * Rotation of line accessor
+     * @return Rot3
+     */
+    Rot3 R() const
+    {
+        return R_;
+    }
+
+    /**
+     * Accessor for a, b
+     * @return Vector2(a, b)
+     */
+    Vector2 V() const
+    {
+        return Vector2(a_, b_);
+    }
+
+    /**
+     * 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 an image plane
+     * @param Dline: OptionalJacobian of projected line with respect to this line
+     * @return Point3 - projected line in image plane
+     */
+    Point3 project(OptionalJacobian<3, 4> Dline = boost::none) const;
+};
+
+template <>
+struct traits<Line3> : public internal::Manifold<Line3> {};
+
+template <>
+struct traits<const Line3> : public internal::Manifold<Line3> {};
+
+// For expression factor graphs
+typedef Expression<Line3> Line3_;
+
+/**
+ * Function projects input line to image plane
+ * @param L input line
+ * @param Dline OptionalJacbian of projected line with respect to input line
+ * @return Point3 - projected line
+ */
+Point3 projectLine(const Line3 &L,
+        OptionalJacobian<3, 4> Dline = boost::none);
+
+/**
+ * Transform a line from world to camera frame
+ * @param p - Pose3 of camera in world frame
+ * @param l - 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 &p, const Line3 &l,
+                    OptionalJacobian<4, 6> Dpose = boost::none,
+                    OptionalJacobian<4, 4> Dline = boost::none);
+
+/**
+ * Expression method for projection
+ * @param line Line3_ expression
+ * @return Point3_ projected line
+ */
+inline Point3_ projectLineExpression(const Line3_& line)
+{
+    Point3 (*f)(const Line3&, OptionalJacobian<3, 4>) = &projectLine;
+    return Point3_(f, line);
+}
+
+/**
+ * Expression method for transforming line
+ * @param p - Pose3_ expression
+ * @param line - Line3 expression
+ * @return transformed Line3_ expression
+ */
+inline Line3_ transformToExpression(const Pose3_& p, const Line3_& line)
+{
+    Line3 (*f)(const Pose3&, const Line3&,
+                OptionalJacobian<4,6>, OptionalJacobian<4,4>) = &transformTo;
+    return Line3_(f, p, line);
+}
+
+}

gtsam/geometry/tests/testLine3.cpp

@@ -0,0 +1,148 @@
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Line3.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/ExpressionFactor.h>
+#include <gtsam/nonlinear/expressionTesting.h>
+
+
+using namespace gtsam;
+
+GTSAM_CONCEPT_TESTABLE_INST(Line3)
+GTSAM_CONCEPT_MANIFOLD_INST(Line3)
+
+static const Line3 l(1, 1);
+
+// Testing equals function of Line3
+TEST(Line3, equals)
+{
+    Line3 l_same = l;
+    EXPECT(l.equals(l_same));
+    Line3 l2(1, 2);
+    EXPECT(!l.equals(l2));
+}
+
+
+// testing localCoordinates along 4 dimensions
+TEST(Line3, localCoordinates)
+{
+    // l1 and l differ only in a_
+    Line3 l1(2, 1);
+    Vector4 v1(0, 0, -1, 0);
+    CHECK(assert_equal(l1.localCoordinates(l), v1));
+
+    // l2 and l differ only in b_
+    Line3 l2(1, 2);
+    Vector4 v2(0, 0, 0, -1);
+    CHECK(assert_equal(l2.localCoordinates(l), v2));
+
+    // l3 and l differ in R_x
+    Rot3 r3;
+    r3 = r3.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;
+    r4 = r4.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));
+}
+
+// testing retract along 4 dimensions
+TEST(Line3, retract)
+{
+    // l1 and l differ only in a_
+    Vector4 v1(0, 0, 0, 1);
+    Line3 l1(1,2);
+    EXPECT(l1.equals(l.retract(v1)));
+
+    // l2 and l differ only in b_
+    Vector4 v2(0, 0, 1, 0);
+    Line3 l2(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;
+    r4 = r4.Expmap(Vector3(0, M_PI/4, 0));
+    Line3 l4(r4, 1, 1);
+    EXPECT(l4.equals(l.retract(v4)));
+}
+
+// testing manifold property - Retract(p, Local(p,q)) == q
+TEST(Line3, retractOfLocalCoordinates)
+{
+    Rot3 r2;
+    r2.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 I;
+    Rot3 r = I.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(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<Line3> f(model, transformTo(p, l_w), transformToExpression(p_, l_));
+    EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+}
+
+
+// projection in camera frame test
+TEST(Line3, projection)
+{
+    Rot3 r;
+    r = r.Expmap(Vector3(0, 0, 0));
+    Line3 l_w(r, 1, 1);
+
+    Point3 expected(-1, 1, 0);
+    EXPECT(expected.equals(projectLine(l_w)));
+
+    Values val;
+    val.insert(1, l_w);
+    Line3_ l_w_(1);
+
+    SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.1);
+    ExpressionFactor<Point3> f(model, expected, projectLineExpression(l_w_));
+    EXPECT_CORRECT_FACTOR_JACOBIANS(f, val, 1e-5, 1e-7);
+}
+
+
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}