g2o::Plane3D Class Reference

#include <plane3d.h>

Public Member Functions
	Plane3D ()
	Plane3D (const Eigen::Vector4d &v)
Eigen::Vector4d	toVector () const
const Eigen::Vector4d &	coeffs () const
void	fromVector (const Eigen::Vector4d &coeffs_)
double	distance () const
Eigen::Vector3d	normal () const
void	oplus (const Eigen::Vector3d &v)
Eigen::Vector3d	ominus (const Plane3D &plane)

Static Public Member Functions
static double	azimuth (const Eigen::Vector3d &v)
static double	elevation (const Eigen::Vector3d &v)
static Eigen::Matrix3d	rotation (const Eigen::Vector3d &v)
static void	normalize (Eigen::Vector4d &coeffs)

Public Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Vector4d	_coeffs

Friends
Plane3D	operator* (const Eigen::Isometry3d &t, const Plane3D &plane)

Detailed Description

Definition at line 37 of file plane3d.h.

Constructor & Destructor Documentation

g2o::Plane3D::Plane3D ( )

inline

Definition at line 42 of file plane3d.h.

               {
        Eigen::Vector4d v;
        v << 1., 0., 0., -1.;
        fromVector(v);
      }

g2o::Plane3D::Plane3D ( const Eigen::Vector4d &  v )

inline

Definition at line 48 of file plane3d.h.

                                     {
        fromVector(v);
      }

Member Function Documentation

static double g2o::Plane3D::azimuth ( const Eigen::Vector3d &  v )

inlinestatic

Definition at line 64 of file plane3d.h.

Referenced by main(), g2o::VertexPlane::write(), and g2o::EdgeSE3PlaneSensorCalib::write().

                                                    {
        return atan2(v(1),v(0));
      }

const Eigen::Vector4d& g2o::Plane3D::coeffs ( ) const

inline

Definition at line 57 of file plane3d.h.

{return _coeffs;}

double g2o::Plane3D::distance ( ) const

inline

Definition at line 72 of file plane3d.h.

Referenced by ominus(), g2o::VertexPlane::write(), and g2o::EdgeSE3PlaneSensorCalib::write().

                            {
      return -_coeffs(3);
    }

static double g2o::Plane3D::elevation ( const Eigen::Vector3d &  v )

inlinestatic

Definition at line 68 of file plane3d.h.

Referenced by main(), g2o::VertexPlane::write(), and g2o::EdgeSE3PlaneSensorCalib::write().

                                                       {
        return atan2(v(2), v.head<2>().norm());
      }

void g2o::Plane3D::fromVector ( const Eigen::Vector4d &  coeffs_ )

inline

Definition at line 59 of file plane3d.h.

References g2o::internal::normalize().

Referenced by main().

                                                           {
        _coeffs=coeffs_;
        normalize(_coeffs);
      }

Eigen::Vector3d g2o::Plane3D::normal ( ) const

inline

Definition at line 76 of file plane3d.h.

Referenced by main(), ominus(), g2o::VertexPlane::write(), and g2o::EdgeSE3PlaneSensorCalib::write().

                                 {
      return _coeffs.head<3>();
    }

static void g2o::Plane3D::normalize ( Eigen::Vector4d &  coeffs )

inlinestatic

Definition at line 112 of file plane3d.h.

                                                        {
      double n=coeffs.head<3>().norm();
      coeffs = coeffs * (1./n);
    }

Eigen::Vector3d g2o::Plane3D::ominus ( const Plane3D &  plane )

inline

Definition at line 102 of file plane3d.h.

References distance(), and normal().

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

                                                     {
      //construct the rotation that would bring the plane normal in (1 0 0)
      Eigen::Matrix3d R=rotation(normal()).transpose();
      Eigen::Vector3d n=R*plane.normal();
      double d=distance()-plane.distance();
      return Eigen::Vector3d(azimuth(n), elevation(n), d);
    }

void g2o::Plane3D::oplus ( const Eigen::Vector3d &  v )

inline

Definition at line 87 of file plane3d.h.

References g2o::internal::normalize().

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

                                             {
      //construct a normal from azimuth and evelation;
      double _azimuth=v[0];
      double _elevation=v[1];
      double s=sin(_elevation), c=cos(_elevation);
      Eigen::Vector3d n (c*cos(_azimuth), c*sin(_azimuth), s) ;
      
      // rotate the normal
      Eigen::Matrix3d R=rotation(normal());
      double d=distance()+v[2];
      _coeffs.head<3>() = R*n;
      _coeffs(3) = -d;
      normalize(_coeffs);
    }

static Eigen::Matrix3d g2o::Plane3D::rotation ( const Eigen::Vector3d &  v )

inlinestatic

Definition at line 81 of file plane3d.h.

                                                           {
      double _azimuth = azimuth(v);
      double _elevation = elevation(v); 
      return (Eigen::AngleAxisd(_azimuth,  Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(- _elevation, Eigen::Vector3d::UnitY())).toRotationMatrix();
    }

Eigen::Vector4d g2o::Plane3D::toVector ( ) const

inline

Definition at line 53 of file plane3d.h.

Referenced by g2o::EdgePlane::computeError(), operator<<(), g2o::EdgePlane::setMeasurementFromState(), g2o::VertexPlane::write(), and g2o::EdgeSE3PlaneSensorCalib::write().

                                            {
        return _coeffs;
      }

Friends And Related Function Documentation

Plane3D operator* ( const Eigen::Isometry3d &  t, const Plane3D &  plane  )

friend

Definition at line 120 of file plane3d.h.

                                                                          {
    Eigen::Vector4d v=plane._coeffs;
    Eigen::Vector4d v2;
    Eigen::Matrix3d R=t.rotation();
    v2.head<3>() = R*v.head<3>();
    v2(3)=v(3) - t.translation().dot(v2.head<3>());
    return Plane3D(v2);
  };

Member Data Documentation

Eigen::Vector4d g2o::Plane3D::_coeffs

Definition at line 117 of file plane3d.h.

Referenced by g2o::operator*().

g2o::Plane3D::EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Definition at line 39 of file plane3d.h.

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/types/slam3d_addons/plane3d.h