g2o::Line3D Class Reference

#include <line3d.h>

Inheritance diagram for g2o::Line3D:

Collaboration diagram for g2o::Line3D:

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

Static Public Member Functions
static G2O_TYPES_SLAM3D_ADDONS_API Line3D	fromCartesian (const Vector6d &cart)
static G2O_TYPES_SLAM3D_ADDONS_API void	jacobian (Matrix7x6d &Jp, Matrix7x6d &Jl, const Isometry3D &x, const Line3D &l)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

Detailed Description

Definition at line 44 of file line3d.h.

Constructor & Destructor Documentation

g2o::Line3D::Line3D ( )

inline

Definition at line 48 of file line3d.h.

Referenced by normalized().

              {
        *this << 0., 0., 0., 1., 0., 0.;
      }

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

inline

Definition at line 52 of file line3d.h.

References G2O_TYPES_SLAM3D_ADDONS_API, and toCartesian().

                                                           {
        (Vector6d&)*this = v;
      }

Member Function Documentation

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

inline

Definition at line 62 of file line3d.h.

Referenced by jacobian(), normalize(), and normalized().

                                                            {
        return tail<3>();
      }

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

inlinestatic

Definition at line 74 of file line3d.h.

References setD(), and setW().

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

void g2o::Line3D::jacobian ( Matrix7x6d &  Jp, Matrix7x6d &  Jl, const Isometry3D &  x, const Line3D &  l  )

static

Definition at line 61 of file line3d.cpp.

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

Referenced by ominus().

                                                                                           {
    Jp.setZero();
    Jl.setZero();
    Matrix6d A=Matrix6d::Zero();
    A.block<3,3>(0,0)=x.linear();
    A.block<3,3>(0,3)= _skew(x.translation())*x.linear();
    A.block<3,3>(3,3)=x.linear();
    Vector6d v=(Vector6d)l;
    Line3D lRemapped(v);

    Matrix6d D = Matrix6d::Zero();
    D.block<3,3>(0,0) = -_skew(l.d());
    D.block<3,3>(0,3) = -2*_skew(l.w());
    D.block<3,3>(3,3) = -2*_skew(l.d());
    Jp.block<6,6>(0,0) = A*D;

    Vector3D d = l.d();
    Vector3D w = l.w();
    double ln = d.norm();
    double iln = 1./ln;
    double iln3 = std::pow(iln,3);
    Matrix6d Jll;
    Jll <<
      iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3,
      0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3,
      0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3,
      0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3,
      0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3,
      0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3;
    Jl.block<6,6>(0,0) = A*Jll;
    Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z();
  }

G2O_TYPES_SLAM3D_ADDONS_API void g2o::Line3D::normalize ( )

inline

Definition at line 84 of file line3d.h.

References d().

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

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

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

inline

Definition at line 89 of file line3d.h.

References d(), and Line3D().

                                                                   {
        return  Line3D((Vector6d)(*this)*(1./d().norm()));
      }

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

inline

Definition at line 98 of file line3d.h.

References G2O_TYPES_SLAM3D_ADDONS_API, jacobian(), g2o::internal::normalizeCartesianLine(), operator*, and g2o::internal::transformCartesianLine().

                                                                            {
        return (Vector6d)(*this)-line;
      }

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

inline

Definition at line 93 of file line3d.h.

References normalize().

                                                                      {
        *this+=v;
        normalize();
      }

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

inline

Definition at line 70 of file line3d.h.

Referenced by fromCartesian().

                                                                        {
        tail<3>()=d_;
      }

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

inline

Definition at line 66 of file line3d.h.

Referenced by fromCartesian().

                                                                        {
        head<3>()=w_;
      }

Vector6d g2o::Line3D::toCartesian

(

)

const

Definition at line 51 of file line3d.cpp.

References g2o::_skew().

Referenced by Line3D(), and main().

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

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

inline

Definition at line 58 of file line3d.h.

Referenced by jacobian().

                                                            {
        return head<3>();
      }

Friends And Related Function Documentation

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

friend

Definition at line 94 of file line3d.cpp.

Referenced by ominus().

                                                           {
    Matrix6d A=Matrix6d::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();
    Vector6d v = (Vector6d)line;
    // cout << "v" << endl << v << endl;
    // cout << "A" << endl << A << endl;
    // cout << "Av" << endl << A*v << endl;
    return Line3D(A*v);
  }

Member Data Documentation

g2o::Line3D::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 46 of file line3d.h.

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d_addons/line3d.h
• /home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d_addons/line3d.cpp