g2o::Line3D Class Reference

#include <line3d.h>

Inheritance diagram for g2o::Line3D:

Collaboration diagram for g2o::Line3D:

Public Member Functions
G2O_TYPES_SLAM3D_ADDONS_API	Line3D ()
G2O_TYPES_SLAM3D_ADDONS_API	Line3D (const Vector6 &v)
G2O_TYPES_SLAM3D_ADDONS_API Vector6	toCartesian () const
G2O_TYPES_SLAM3D_ADDONS_API Vector3	w () const
G2O_TYPES_SLAM3D_ADDONS_API Vector3	d () const
G2O_TYPES_SLAM3D_ADDONS_API void	setW (const Vector3 &w_)
G2O_TYPES_SLAM3D_ADDONS_API void	setD (const Vector3 &d_)
G2O_TYPES_SLAM3D_ADDONS_API void	normalize ()
G2O_TYPES_SLAM3D_ADDONS_API Line3D	normalized () const
G2O_TYPES_SLAM3D_ADDONS_API void	oplus (const Vector4 &v)
G2O_TYPES_SLAM3D_ADDONS_API Vector4	ominus (const Line3D &line)

Static Public Member Functions
static G2O_TYPES_SLAM3D_ADDONS_API Line3D	fromCartesian (const Vector6 &cart)
static G2O_TYPES_SLAM3D_ADDONS_API Line3D	fromOrthonormal (const OrthonormalLine3D &ortho)
static G2O_TYPES_SLAM3D_ADDONS_API OrthonormalLine3D	toOrthonormal (const Line3D &line)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Friends
G2O_TYPES_SLAM3D_ADDONS_API friend Line3D	operator* (const Isometry3 &t, const Line3D &line)

Detailed Description

Definition at line 53 of file line3d.h.

Constructor & Destructor Documentation

Line3D() [1/2]

G2O_TYPES_SLAM3D_ADDONS_API g2o::Line3D::Line3D ( )

inline

Definition at line 60 of file line3d.h.

                                       {
    *this << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0;
  }

Referenced by normalized().

Line3D() [2/2]

G2O_TYPES_SLAM3D_ADDONS_API g2o::Line3D::Line3D ( const Vector6 &  v )

inline

Definition at line 64 of file line3d.h.

{ (Vector6&)* this = v; }

Member Function Documentation

d()

G2O_TYPES_SLAM3D_ADDONS_API Vector3 g2o::Line3D::d ( ) const

inline

Definition at line 70 of file line3d.h.

{ return tail<3>(); }

Referenced by fromOrthonormal(), main(), normalize(), normalized(), printPlueckerLine(), toCartesian(), and toOrthonormal().

fromCartesian()

static G2O_TYPES_SLAM3D_ADDONS_API Line3D g2o::Line3D::fromCartesian ( const Vector6 &  cart )

inlinestatic

Definition at line 80 of file line3d.h.

                           {
    Line3D l;
    Vector3 _p = cart.head<3>();
    Vector3 _d = cart.tail<3>() * 1.0 / cart.tail<3>().norm();
    _p -= _d * (_d.dot(_p));
    l.setW(_p.cross(_p + _d));
    l.setD(_d);
    return l;
  }

References setD(), and setW().

Referenced by main().

fromOrthonormal()

static G2O_TYPES_SLAM3D_ADDONS_API Line3D g2o::Line3D::fromOrthonormal ( const OrthonormalLine3D &  ortho )

inlinestatic

Definition at line 91 of file line3d.h.

                                      {
    Vector3 w;
    w.x() = ortho.U(0, 0) * ortho.W(0, 0);
    w.y() = ortho.U(1, 0) * ortho.W(0, 0);
    w.z() = ortho.U(2, 0) * ortho.W(0, 0);
 
    Vector3 d;
    d.x() = ortho.U(0, 1) * ortho.W(1, 0);
    d.y() = ortho.U(1, 1) * ortho.W(1, 0);
    d.z() = ortho.U(2, 1) * ortho.W(1, 0);
 
    Line3D l;
    l.setW(w);
    l.setD(d);
    l.normalize();
 
    return l;
  }

References d(), normalize(), setD(), setW(), g2o::OrthonormalLine3D::U, g2o::OrthonormalLine3D::W, and w().

Referenced by oplus().

normalize()

G2O_TYPES_SLAM3D_ADDONS_API void g2o::Line3D::normalize ( )

inline

Definition at line 133 of file line3d.h.

                                                      {
    double n = 1.0 / d().norm();
    (*this) *= n;
  }

References d().

Referenced by fromOrthonormal(), main(), and oplus().

normalized()

G2O_TYPES_SLAM3D_ADDONS_API Line3D g2o::Line3D::normalized ( ) const

inline

Definition at line 138 of file line3d.h.

                                                               {
    return Line3D((Vector6)(*this) * (1.0 / d().norm()));
  }

References d(), and Line3D().

ominus()

G2O_TYPES_SLAM3D_ADDONS_API Vector4 g2o::Line3D::ominus ( const Line3D &  line )

inline

Definition at line 158 of file line3d.h.

                                                                        {
    OrthonormalLine3D ortho_estimate = toOrthonormal(*this);
    OrthonormalLine3D ortho_line = toOrthonormal(line);
 
    Matrix2 W_delta = ortho_estimate.W.transpose() * ortho_line.W;
    Matrix3 U_delta = ortho_estimate.U.transpose() * ortho_line.U;
 
    Vector4 delta;
    Quaternion q(U_delta);
    q.normalize();
    delta[0] = q.x();
    delta[1] = q.y();
    delta[2] = q.z();
    delta[3] = std::atan2(W_delta(1, 0), W_delta(0, 0));
 
    return delta;
  }

References toOrthonormal(), g2o::OrthonormalLine3D::U, and g2o::OrthonormalLine3D::W.

Referenced by g2o::EdgeSE3Line3D::computeError(), and main().

oplus()

G2O_TYPES_SLAM3D_ADDONS_API void g2o::Line3D::oplus ( const Vector4 &  v )

inline

Definition at line 142 of file line3d.h.

                                                                  {
    OrthonormalLine3D ortho_estimate = toOrthonormal(*this);
    OrthonormalLine3D ortho_update;
    ortho_update.W << std::cos(v[3]), -std::sin(v[3]), std::sin(v[3]),
        std::cos(v[3]);
    Quaternion quat(std::sqrt(1 - v.head<3>().squaredNorm()), v[0], v[1], v[2]);
    quat.normalize();
    ortho_update.U = quat.toRotationMatrix();
 
    ortho_estimate.U = ortho_estimate.U * ortho_update.U;
    ortho_estimate.W = ortho_estimate.W * ortho_update.W;
 
    *this = fromOrthonormal(ortho_estimate);
    this->normalize();
  }

References fromOrthonormal(), normalize(), toOrthonormal(), g2o::OrthonormalLine3D::U, and g2o::OrthonormalLine3D::W.

Referenced by main(), and LineSensor::sense().

setD()

G2O_TYPES_SLAM3D_ADDONS_API void g2o::Line3D::setD ( const Vector3 &  d_ )

inline

Definition at line 76 of file line3d.h.

                                                                  {
    tail<3>() = d_;
  }

Referenced by fromCartesian(), and fromOrthonormal().

setW()

G2O_TYPES_SLAM3D_ADDONS_API void g2o::Line3D::setW ( const Vector3 &  w_ )

inline

Definition at line 72 of file line3d.h.

                                                                  {
    head<3>() = w_;
  }

Referenced by fromCartesian(), and fromOrthonormal().

toCartesian()

Vector6 g2o::Line3D::toCartesian

(

)

const

Definition at line 41 of file line3d.cpp.

                                  {
  Vector6 cartesian;
  cartesian.tail<3>() = d() / d().norm();
  Matrix3 W = -_skew(d());
  double damping = cst(1e-9);
  Matrix3 A = W.transpose() * W + (Matrix3::Identity() * damping);
  cartesian.head<3>() = A.ldlt().solve(W.transpose() * w());
  return cartesian;
}

References g2o::_skew(), g2o::cst(), d(), and w().

toOrthonormal()

static G2O_TYPES_SLAM3D_ADDONS_API OrthonormalLine3D g2o::Line3D::toOrthonormal ( const Line3D &  line )

inlinestatic

Definition at line 111 of file line3d.h.

                          {
    OrthonormalLine3D ortho;
 
    Vector2 mags;
    mags << line.d().norm(), line.w().norm();
 
    double wn = 1.0 / mags.norm();
    ortho.W << mags.y() * wn, -mags.x() * wn, mags.x() * wn, mags.y() * wn;
 
    double mn = 1.0 / mags.y();
    double dn = 1.0 / mags.x();
    Vector3 mdcross;
    mdcross = line.w().cross(line.d());
    double mdcrossn = 1.0 / mdcross.norm();
    ortho.U << line.w().x() * mn, line.d().x() * dn, mdcross.x() * mdcrossn,
        line.w().y() * mn, line.d().y() * dn, mdcross.y() * mdcrossn,
        line.w().z() * mn, line.d().z() * dn, mdcross.z() * mdcrossn;
 
    return ortho;
  }

References d(), g2o::OrthonormalLine3D::U, g2o::OrthonormalLine3D::W, and w().

Referenced by ominus(), and oplus().

w()

G2O_TYPES_SLAM3D_ADDONS_API Vector3 g2o::Line3D::w ( ) const

inline

Definition at line 68 of file line3d.h.

{ return head<3>(); }

Referenced by fromOrthonormal(), main(), printPlueckerLine(), toCartesian(), and toOrthonormal().

Friends And Related Symbol Documentation

operator*

G2O_TYPES_SLAM3D_ADDONS_API friend Line3D operator* ( const Isometry3 &  t, const Line3D &  line  )

friend

Definition at line 51 of file line3d.cpp.

                                                         {
  Matrix6 A = Matrix6::Zero();
  A.block<3, 3>(0, 0) = t.linear();
  A.block<3, 3>(0, 3) = _skew(t.translation()) * t.linear();
  A.block<3, 3>(3, 3) = t.linear();
  Vector6 v = (Vector6)line;
  return Line3D(A * v);
}

Member Data Documentation

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

g2o::Line3D::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 55 of file line3d.h.

---

The documentation for this class was generated from the following files:

• /build/reproducible-path/g2o-0~20230806/g2o/types/slam3d_addons/line3d.h
• /build/reproducible-path/g2o-0~20230806/g2o/types/slam3d_addons/line3d.cpp