iSAM
Point3dh.h
Go to the documentation of this file.
00001 
00028 #pragma once
00029 
00030 #include <Eigen/Dense>
00031 #include <Eigen/Geometry>
00032 
00033 #include "Point3d.h"
00034 
00035 namespace isam {
00036 
00037 class Point3dh {
00038   friend std::ostream& operator<<(std::ostream& out, const Point3dh& p) {
00039     p.write(out);
00040     return out;
00041   }
00042 
00043   double _x;
00044   double _y;
00045   double _z;
00046   double _w;
00047 
00048 public:
00049 
00050   static const int dim = 3;
00051   static const int size = 4;
00052   static const char* name() {
00053     return "Point3dh";
00054   }
00055 
00056   Point3dh() : _x(0.), _y(0.), _z(0.), _w(1.) {} // note: 0,0,0,0 is not allowed!
00057   Point3dh(double x, double y, double z, double w) : _x(x), _y(y), _z(z), _w(w) {}
00058   Point3dh(const Eigen::Vector4d& vec) : _x(vec(0)), _y(vec(1)), _z(vec(2)), _w(vec(3)) {}
00059   Point3dh(const Point3d& p) : _x(p.x()), _y(p.y()), _z(p.z()), _w(1.) {}
00060 
00061   double x() const {return _x;}
00062   double y() const {return _y;}
00063   double z() const {return _z;}
00064   double w() const {return _w;}
00065 
00066   void x(double x) {_x = x;}
00067   void y(double y) {_y = y;}
00068   void z(double z) {_z = z;}
00069   void w(double w) {_w = w;}
00070 
00071   static Eigen::Vector4d delta3_to_homogeneous(const Eigen::Vector3d& delta) {
00072     double theta = delta.norm();
00073     double S;
00074     if (theta < 0.0001) { // sqrt4(machine precession)
00075       S = 0.5 + theta*theta/48.;
00076     } else {
00077       S = sin(0.5*theta)/theta;
00078     }
00079     double C = cos(0.5*theta);
00080     return Eigen::Vector4d(S*delta(0), S*delta(1), S*delta(2), C);
00081   }
00082 
00083   Point3dh exmap(const Eigen::Vector3d& delta) const {
00084 #if 0
00085     // solution with Householder matrix following HZ second edition
00086 
00087     //    std::cout << norm() << std::endl;
00088     assert(norm() == 1);
00089     // make sure not above PI, otherwise needs to be corrected
00090     assert(delta.norm() < 3.14);
00091 
00092     Eigen::Vector4d delta4 = delta3_to_homogeneous(delta);
00093     // multiply with Householder matrix, without explicitly calculating the matrix
00094     Eigen::Vector4d e(0,0,0,1);
00095     Eigen::Vector4d v = vector() + (x()<0.?-1:1) * norm() * e;
00096 
00097     // checking if sign is correct to yield (0,0,0,1), otherwise fix
00098     Eigen::Vector4d test = vector() - 2*v*(v.transpose()*vector())/(v.transpose()*v);
00099     if (test(3)<0) {
00100       v = v - 2 * (x()<0.?-1:1) * norm() * e;
00101     }
00102 
00103     Eigen::Vector4d res = delta4 - 2*v*(v.transpose()*delta4)/(v.transpose()*v);
00104     //    std::cout << vector() << " " << res << std::endl;
00105     return Point3dh(res);
00106 #else
00107 #if 1
00108     // solution by mapping to unit Quaternion and back (both are unit spheres!)
00109     Eigen::Quaterniond delta_quat = Rot3d::delta3_to_quat(delta);
00110     Eigen::Quaterniond quat(_w, _x, _y, _z);
00111     //    quat.normalize(); // just to be safe...
00112     Eigen::Quaterniond res = quat * delta_quat;
00113     return Point3dh(res.x(), res.y(), res.z(), res.w());
00114 #else
00115 #if 1
00116     // 3DOF exmap - not recommended
00117     Point3dh res = *this;
00118     res.normalize();
00119     Eigen::Vector4d::Index pos;
00120     res.vector().cwiseAbs().maxCoeff(&pos);
00121     //    std::cout << *this << " -- " << pos << " -- " << res.vector() << std::endl;
00122     // only update 3 out of 4 entries
00123     int idx = 0;
00124     if (pos!=0) {res._x += delta(idx); idx++;}
00125     if (pos!=1) {res._y += delta(idx); idx++;}
00126     if (pos!=2) {res._z += delta(idx); idx++;}
00127     if (pos!=3) {res._w += delta(idx);}
00128     res.normalize();
00129     return res;
00130 #else
00131     // 4DOF exmap - not feasible without constrained optimization
00132     Point3dh res = *this;
00133     res._x += delta(0);
00134     res._y += delta(1);
00135     res._z += delta(2);
00136     res._w += delta(3);
00137     return res;
00138 #endif
00139 #endif
00140 #endif
00141   }
00142 
00143   Eigen::Vector4d vector() const {
00144     Eigen::VectorXd tmp(4);
00145     tmp << _x, _y, _z, _w;
00146     return tmp;
00147   }
00148 
00149   Eigen::VectorXb is_angle() const {
00150     Eigen::VectorXb isang (4);
00151     isang << false, false, false, false;
00152     return isang;
00153   }
00154 
00155   void set(double x, double y, double z, double w) {
00156     _x = x;
00157     _y = y;
00158     _z = z;
00159     _w = w;
00160   }
00161 
00162   void set(const Eigen::Vector4d& v) {
00163     _x = v(0);
00164     _y = v(1);
00165     _z = v(2);
00166     _w = v(3);
00167   }
00168 
00169   Point3d to_point3d() const {
00170     double w_inv = 1. / _w;
00171     return Point3d(_x*w_inv, _y*w_inv, _z*w_inv);
00172   }
00173 
00174   double norm() const {
00175     return sqrt(_x*_x + _y*_y + _z*_z + _w*_w);
00176   }
00177 
00178   Point3dh normalize() {
00179     double a = 1. / norm();
00180     _x *= a;
00181     _y *= a;
00182     _z *= a;
00183     _w *= a;
00184     return *this;
00185   }
00186 
00187   void write(std::ostream &out) const {
00188     out << "(" << _x << ", " << _y << ", " << _z << ", " << _w << ")";
00189   }
00190 };
00191 
00192 }
 All Classes Files Functions Variables