iSAM
Point3d.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <Eigen/Dense>
00031 
00032 #include "Point2d.h"
00033 
00034 namespace isam {
00035 
00036 class Point3d {
00037   friend std::ostream& operator<<(std::ostream& out, const Point3d& p) {
00038     p.write(out);
00039     return out;
00040   }
00041 
00042   double _x;
00043   double _y;
00044   double _z;
00045 public:
00046   static const int dim = 3;
00047   static const int size = 3;
00048   static const char* name() {
00049     return "Point3d";
00050   }
00051   Point3d() : _x(0.), _y(0.), _z(0.) {}
00052   Point3d(double x, double y, double z) : _x(x), _y(y), _z(z) {}
00053   Point3d(const Eigen::Vector3d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)) {}
00054 
00055   double x() const {return _x;}
00056   double y() const {return _y;}
00057   double z() const {return _z;}
00058 
00059   void set_x(double x) {_x = x;}
00060   void set_y(double y) {_y = y;}
00061   void set_z(double z) {_z = z;}
00062 
00063   Point3d exmap(const Eigen::Vector3d& delta) const {
00064     Point3d res = *this;
00065     res._x += delta(0);
00066     res._y += delta(1);
00067     res._z += delta(2);
00068     return res;
00069   }
00070 
00071   Eigen::Vector3d vector() const {
00072     Eigen::Vector3d tmp(_x, _y, _z);
00073     return tmp;
00074   }
00075   void set(double x, double y, double z) {
00076     _x = x;
00077     _y = y;
00078     _z = z;
00079   }
00080   void set(const Eigen::Vector3d& v) {
00081     _x = v(0);
00082     _y = v(1);
00083     _z = v(2);
00084   }
00085   
00086   Eigen::VectorXb is_angle() const {
00087     return Eigen::VectorXb::Zero(size);
00088   }
00089 
00090   Point3d to_point3d() {
00091     return *this;
00092   }
00093 
00094   void of_point2d(const Point2d& p) {
00095     _x = p.x();
00096     _y = p.y();
00097     _z = 0.;
00098   }
00099   void write(std::ostream &out) const {
00100     out << "(" << _x << ", " << _y << ", " << _z << ")";
00101   }
00102 };
00103 
00104 }
 All Classes Files Functions Variables