line3 compiled tested
akrishnan86
parent
cce936f03a
commit
4356c1953d
189
doc/math.lyx
189
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
|
||||
|
||||
|
|
|
|||
BIN
doc/math.pdf
BIN
doc/math.pdf
Binary file not shown.
|
|
@ -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]);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue