aheigen
ns http://eigen.tuxfamily.org/dox/TutorialGeometry.html
ns http://eigen.tuxfamily.org/dox/QuickRefPage.html
INCLUDES
TYPES
EQUALITY (compare all types)
VECTOR3D_USAGE
MATRIX_USAGE
QUATERNIOND_USAGE
AFFINE3D_USAGE
MAP_USAGE -- use external types as eigen types
EIGENSTL_USAGE
PLANE
LINE
TF_TO_EIGEN
EIGEN_TO_MSG
FIT_POINTS_TO_PARABOLA
FIT_POINTS_TO_PLANE
INCLUDES
--------
#include <Eigen/Core> // Matrix and Array
#include <Eigen/Geometry>
#include <Eigen/LU> // inverse, determinant, etc
#include <Eigen/Cholesky>
#include <Eigen/Householder>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>
#include <Eigen/Dense> // includes Core, Geometry, LU, Cholesky, SVD, QR, Eigenvalues
#include <Eigen/Eigen> // includes EVERYTHING (i.e. Dense and Sparse)
TYPES
-----
Eigen::Matrix3d m3
Eigen::Matrix4d m4
Eigen::Affine3d a
Eigen::Vector3d v
Eigen::Quaterniond q
Eigen::Translation3d trans
GENERAL: MatrixNT - square matrix with N=2..4 or X and T is type:
f = float
d = double
i = int
cf = std::complex<float>
cd = std::complex<double>
GENERAL: VectorNT, RowVectorNT
EQUALITY
--------
a.isApprox(b, prec); // true if a==b with precision prec. a,b must be same type
VECTOR3D_USAGE
--------------
Eigen::Vector3d v1, v2, v3;
Eigen::Vector2d v(x,y);
Eigen::Vector3d v(x,y,z);
Eigen::Vector4d v(x,y,z,w);
v1 = v2.cross(v3); // Vector3* only
double val = v2.dot(v3);
v1.normalize(); // in place
v1 = v2.normalized(); // v2 not modified
double len = v.norm();
double lensq = v.squaredNorm();
v(index) // r/w access to elements
v.x() v.y() v.z() v.w() // r/w access to elements
MATRIX_USAGE
------------
Eigen::Matrix3d m3;
Eigen::Matrix4d m4;
m3 << 1, 2, 3,
4, 5, 6,
7, 8, 9;
ma = ma * mb;
ma = mb * ma;
ma.noalias() = mb * mc; // noalias is optional, but is faster (only use if ma is not mb or mc)
m.transpose()
m.conjugate() // no-op for real matrix
m.adjoint() // same as transpose for real matrix
m.transposeInPlace()
m.sum(), m.prod(), m.mean(), m.trace() // sum, etc, all elements of the matrix
m.minCoeff(), m.maxCoeff()
std::ptrdiff)t row, col;
m.minCoeff(&row, &col); // return min and set row,col to its position
m.diagonal().sum() // same as m.trace()
ARRAY and COMPONENT WISE
m.array() // convert to Array (can assign to Matrix or Array)
Eigen::Array4d arr;
arr.matrix() // convert array to matrix
m = ma.array() + mb.array() // + - * /
QUATERNIOND_USAGE
-----------------
Eigen::Quaternionf (float version)
?? Eigen::Quaterniond q(Eigen::AngleAxis(angle,axis)); // angle in radian, axis is Vector3d
Eigen::Quaterniond::Identity();
TO QUATERNION
q = Eigen::AngleAxis(angle,axis)); // angle in radian, axis is Vector3d
q = Eigen::Vector4d(x,y,z,w);
q = m3; // m3 is Eigen::Matrix3d -- 3x3 matrix (MUST be PURE ROTATION -- no scale, shear)
q = a.linear() // a is Affine3d with rotation and translation (no shear, scale)
FROM QUATERNION
m3 = q.matrix(); or q.toRotationMatrix();
a2 = q * a1; or a1 * q
a = Eigen::Translation3d(x,y,z) * q;
OPERATIONS
q3 = q1.slerp(alpha, q2);
q1.angularDistance(q2); // radians between q1..q2
q.conjugate() // same as inverse if normalized
q.inverse() // EXPENSIVE -- use conjugate instead
q.normalize() // most functions assume q is normalized already
q1.dot(q2) // half the angle between the 2 quaternions
q.norm() // should be 1 if q is normalized
q.vec().normalized() // the axis of rotation
q3 = q1 * q2; // concatenate rotations
q3 *= q1;
ACCESS ELEMENTS
q.x(), q.y(), q.z(), q.w() // r/w access
q.coeffs() // r/w Vector4d(x,y,z,w) reference
q.setFromTwoVectors(v1,v2) // set q so it rotates v1 to v2 (v1 and v2 pass thru origin)
AFFINE3D_USAGE
--------------
THIS DOES NOT WORK: Eigen::Affine3d a = Translation3d(x,y,z); Instead use:
WORKS:
Eigen::Affine3d a; // uninitialized
Eigen::Affine3d a(Eigen::AngleAxis(angle,axis)); // angle in radian, axis is Vector3d
Eigen::Affine3d a(Eigen::Scaling(sx, sy, sz));
Eigen::Affine3d a(Eigen::Translation3d(x, y, z));
Eigen::Affine3d a(Eigen::Matrix3d m3);
Eigen::Affine3d a(Eigen::Matrix4d m4);
Eigen::Isometry3d a; // better if only rotation & translation
Eigen::Projective3d a; // if projective (i.e. last row is not 0 0 0 1)
Eigen::Affine3d a; // assumes last row is 0 0 0 1
Eigen::AffineCompact3d a; // like Affine3d but do not store the last row.
a = Eigen::Translation3d(v)
a = Eigen::Translation3d(v) * q;
a = Eigen::Translation3d(v) * q * Scaling(scale);
a = Eigen::Translation3d(v) * q * Scaling(sx,sy,sz);
a = Eigen::Affine3d::Identity();
a.fromPositionOrientationScale(pos, orient, scale); // pos/scale are vec3, orient is m3/q/aa
ainv = a.inverse(); // requires #include <Eigen/LU>
ainv = a.inverse(Eigen::Isometry); // if a only has rot + translate (much faster)
v = a.translation();
m3 = a.linear(); // if pure translation + rotation (no scale/shear)
q = a.linear(); // if pure translation + rotation (no scale/shear)
q = a.rotation(); // extract rotation (slower than linear())
a.computeRotationScaling(q, s); // fill in q and s with rot and scale. #include <Eigen/SVD>
v2 = a * v1;
a3 = a1 * a2;
a3 = a1 * m3;
a3 = a1 * Matrix4d;
a3 = a1 * diagonal
ACCESS ELEMENTS
a.data() // r/w double* ptr to column-major data
a.matrix() // r/w Matrix4d reference
a(row,col) // r/w access
a = Matrix3d // sets translation to 0
a = Matrix4d
MAP_USAGE
---------
double *data;
Eigen::Map<Matrix3d>(data) // treat data as a packed 3x3 matrix
Eigen::Map<Matrix4d>(data) // treat data as a packed 4x4 matrix
Eigen::Map<Vector3d>(data) // treat data as a packed vec3
Eigen::Map<Quaterniond>(data) // treat data as x,y,z,w quaternion
EIGENSTL_USAGE
--------------
EigenSTL from
#include <eigen_stl_containers/eigen_stl_vector_container.h>
typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > vector_Vector3d;
typedef std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > vector_Affine3d;
PLANE
-----
typedef Eigen::Hyperplane<double,3> HyperPlane3d;
Eigen::Vector3d pt, normal;
Eigen::Hyperplane<double,3> h(normal, pt) // norm must be unit vector
Eigen::Hyperplane<double,3> h(normal, double dist) // norm must be unit vector
h.normalize()
h.signedDistance(pt) // normal.dot(pt) + dist
h.absDistance(pt)
h.coeffs() // Vector4d(nx, ny, nz, dist) reference
h.normal() // Vector3d(nx, ny, nz) reference
h.offset() // returns dist
h.projection(v) // project v onto plane (returns Vector3d)
h.transform(mtx) // mtx is arbitrary Matrix3d or Affine3d
h.transform(mtx,Eigen::Isometry) // mtx is pure rotation Matrix3d or Affine3d (faster)
Eigen::Hyperplane<double,3>::Through(pt0, pt2, pt2) // plane through 3 points
LINE
----
typedef Eigen::ParametrizedLine<double,3> ParametrizedLine3d;
Eigen::ParametrizedLine<double,3> line; // ununitialized
Eigen::ParametrizedLine<double,3> line(origin, direction); // both Vector3d
Eigen::ParametrizedLine<double,3> line(origin, direction); // both Vector3d
Eigen::Vector3d pt;
line.distance(pt);
line.squaredDistance(pt);
line.intersection(plane) // returns line param for intersection point
line.projection(pt) // point projected onto line
Eigen::ParametrizedLine<double,3>::Through(pt0, pt1); // line thru 2 points
// NOT FOUND? LATER VERSION OF EIGEN?
line.intersectionParameter(plane) // returns line param for intersection point
line.intersectionPoint(plane) // returns intersection point Vector3d
line.pointAt(double param) // point at param (0...1)
TF_TO_EIGEN
-----------
#include <tf_conversions/tf_eigen.h>
vw /opt/ros/hydro/include/tf_conversions/tf_eigen.h
package: ros-hydro-tf-conversions tf_conversions
TF_TO_EIGEN
void tf::matrixTFToEigen( const tf::Matrix3x3 &t, Eigen::Matrix3d &e);
void tf::poseTFToEigen( const tf::Pose &t, Eigen::Affine3d &e);
void tf::quaternionTFToEigen(const tf::Quaternion &t, Eigen::Quaterniond &e);
void tf::transformTFToEigen( const tf::Transform &t, Eigen::Affine3d &e);
void tf::vectorTFToEigen( const tf::Vector3 &t, Eigen::Vector3d &e);
EIGEN_TO_TF
void tf::matrixEigenToTF( const Eigen::Matrix3d &e, tf::Matrix3x3 &t);
void tf::poseEigenToTF( const Eigen::Affine3d &e, tf::Pose &t);
void tf::quaternionEigenToTF(const Eigen::Quaterniond &e, tf::Quaternion &t);
void tf::transformEigenToTF( const Eigen::Affine3d &e, tf::Transform &t);
void tf::vectorEigenToTF( const Eigen::Vector3d &e, tf::Vector3 &t);
EIGEN_TO_MSG
------------
#include <eigen_conversions/eigen_msg.h>
vw /opt/ros/hydro/include/eigen_conversions/eigen_msg.h
package: ros-hydro-eigen-conversions eigen_conversions
EIGEN_TO_MSG
void tf::pointEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Point &m);
void tf::poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m);
void tf::quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m);
void tf::transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m);
void tf::twistEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Twist &m);
void tf::vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m);
void tf::wrenchEigenToMsg(const Eigen::Matrix<double,6,1> &e, geometry_msgs::Wrench &m);
void tf::matrixEigenToMsg(const Eigen::MatrixBase<Derived> &e, std_msgs::Float64MultiArray &m)
MSG_TO_EIGEN
void tf::pointMsgToEigen(const geometry_msgs::Point &m, Eigen::Vector3d &e);
void tf::poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
void tf::quaternionMsgToEigen(const geometry_msgs::Quaternion &m, Eigen::Quaterniond &e);
void tf::transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e);
void tf::twistMsgToEigen(const geometry_msgs::Twist &m, Eigen::Matrix<double,6,1> &e);
void tf::vectorMsgToEigen(const geometry_msgs::Vector3 &m, Eigen::Vector3d &e);
void tf::wrenchMsgToEigen(const geometry_msgs::Wrench &m, Eigen::Matrix<double,6,1> &e);
FIT_POINTS_TO_PARABOLA
----------------------
// find a,b,c such that y = axx + bx + c fits the points
// x values are the indices into vals (i.e. 0.0, 1.0, 2.0, ...)
void fitParabola(
const std::vector<double>& vals,
double* a,
double* b,
double* c)
{
int nvals = vals.size();
double s1 = (double)nvals;
double sx = 0.0;
double sxx = 0.0;
double sxxx = 0.0;
double sxxxx = 0.0;
double sy = 0.0;
double sxy = 0.0;
double sxxy = 0.0;
for (int i = 0 ; i < nvals ; ++i)
{
double y = vals[i];
double x = (double)i;
sy += y;
sx += x;
sxx += x*x;
sxxx += x*x*x;
sxxxx += x*x*x*x;
sxy += x*y;
sxxy += x*x*y;
}
Eigen::Matrix3d m;
m << s1, sx, sxx,
sx, sxx, sxxx,
sxx, sxxx, sxxxx;
Eigen::Vector3d r(sy, sxy, sxxy);
Eigen::Vector3d x = m.colPivHouseholderQr().solve(r);
*c = x(0);
*b = x(1);
*a = x(2);
}
FIT_POINTS_TO_PLANE
-------------------
// find a,b,c such that z = ax + by + c is a plane fitting points.
void fitPlane(
std::vector<cv::Point3d> points,
double* a,
double* b,
double* c)
{
int npts = points.size();
double s1 = (double)npts;
double sx = 0.0;
double sy = 0.0;
double sz = 0.0;
double sxx = 0.0;
double syy = 0.0;
double sxy = 0.0;
double sxz = 0.0;
double syz = 0.0;
for (int i = 0 ; i < npts ; ++i)
{
cv::Point3d& pt = points[i];
sx += pt.x;
sy += pt.y;
sz += pt.z;
sxx += pt.x * pt.x;
syy += pt.y * pt.y;
sxy += pt.x * pt.y;
sxz += pt.x * pt.z;
syz += pt.y * pt.z;
}
Eigen::Matrix3d m;
m << sxx, sxy, sx,
sxy, syy, sy,
sx, sy, s1;
Eigen::Vector3d r(sxz, syz, sz);
Eigen::Vector3d x = m.colPivHouseholderQr().solve(r);
*a = x(0);
*b = x(1);
*c = x(2);
}