"Eigen is a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms."
The motivation of this page is to show some Eigen example calls.
Eigen::Vector3d pos;
pos << 0.0,-2.0,1.0;
Eigen::Vector3d pos2 = Eigen::Vector3d(1,-3,0.5);
pos = pos+pos2;
Eigen::Vector3d pos3 = Eigen::Vector3d::Zero();
pos.normalize();
pos.inverse();
//Dot and Cross Product
Eigen::Vector3d v(1, 2, 3);
Eigen::Vector3d w(0, 1, 2);
double vDotw = v.dot(w); // dot product of two vectors
Eigen::Vector3d vCrossw = v.cross(w); // cross product of two vectors
Eigen::Quaterniond rot;
rot.setFromTwoVectors(Eigen::Vector3d(0,1,0), pos);
Eigen::Matrix<double,3,3> rotationMatrix;
rotationMatrix = rot.toRotationMatrix();
Eigen::Quaterniond q(2, 0, 1, -3);
std::cout << "This quaternion consists of a scalar " << q.w()
<< " and a vector " << std::endl << q.vec() << std::endl;
q.normalize();
std::cout << "To represent rotation, we need to normalize it such
that its length is " << q.norm() << std::endl;
Eigen::Vector3d vec(1, 2, -1);
Eigen::Quaterniond p;
p.w() = 0;
p.vec() = vec;
Eigen::Quaterniond rotatedP = q * p * q.inverse();
Eigen::Vector3d rotatedV = rotatedP.vec();
std::cout << "We can now use it to rotate a vector " << std::endl
<< vec << " to " << std::endl << rotatedV << std::endl;
// convert a quaternion to a 3x3 rotation matrix:
Eigen::Matrix3d R = q.toRotationMatrix();
std::cout << "Compare with the result using an rotation matrix "
<< std::endl << R * vec << std::endl;
Eigen::Quaterniond a = Eigen::Quaterniond::Identity();
Eigen::Quaterniond b = Eigen::Quaterniond::Identity();
Eigen::Quaterniond c;
// Adding two quaternion as two 4x1 vectors is not supported by the Eigen API.
//That is, c = a + b is not allowed. The solution is to add each element:
c.w() = a.w() + b.w();
c.x() = a.x() + b.x();
c.y() = a.y() + b.y();
c.z() = a.z() + b.z();
// we use fixed axis rotation(rpy) to get rotation matrix
// roll, pitch, yaw = XYZ fixed axis rotation = Eurler angle ZYX
Eigen::Quaterniond q = AngleAxisd(0.1*M_PI, Vector3d::UnitX())
* AngleAxisd(0.2*M_PI, Vector3d::UnitY())
* AngleAxisd(0.3*M_PI, Vector3d::UnitZ());
Eigen::Matrix3d R = q.toRotationMatrix();
// roll pitch yaw to rotation matrix directly
Matrix3d R = AngleAxisd(PI, Vector3d::UnitX())
* AngleAxisd(0, Vector3d::UnitY())
* AngleAxisd(0, Vector3d::UnitZ());
// return euler angles as sequence XYZ Euler angle from rotatiton matrix
Vector3d euler_angles = R.eulerAngles(0,1,2); // 0: x axis, 1: y axis, 2: z axis
// recompute the rotation matrix from eurler angles
Matrix3d n;
n = AngleAxisd(euler_angles[0], Vector3d::UnitX())
*AngleAxisd(euler_angles[1], Vector3d::UnitY())
*AngleAxisd(euler_angles[2], Vector3d::UnitZ());
// return ZXZ euler angles from rotation matrix
Vector3d ea = R.eulerAngles(2, 0, 2); // 2,0,2 = ZXZ- Creating a rotation matrix with pitch, yaw, roll using Eigen
- Roll pitch and yaw from Rotation matrix with Eigen Library
- Eigen Geometry module
Affine3d is a Pose type message( contains a Vector 3d and Quaterniond/RotationMatrix). It is great for computing several subsequent transformations.
Eigen::Affine3d aff = Eigen::Affine3d::Identity();
aff.translation() = pos;
aff.translation() = Eigen::Vector3d(0.7,0.4,1.1);
aff.linear() = rot.toRotationMatrix();
aff.translate(Eigen::Vector3d(0,0.0,0.03));
aff.pretranslate(pos);
aff.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d(0,0,1)));
aff.prerotate(rot.toRotationMatrix());
Eigen::MatrixXd normalMatrix = aff.linear().inverse().transpose();
To get the translation and rotation portions:
pos = aff.translation();
rot = aff.linear();
aff.linear().col(0);//Transpose and inverse:
Eigen::MatrixXd A(3, 2);
A << 1, 2,
2, 3,
3, 4;
Eigen::MatrixXd B = A.transpose();// the transpose of A is a 2x3 matrix
// computer the inverse of BA, which is a 2x2 matrix:
Eigen::MatrixXd C = (B * A).inverse();
C.determinant(); //compute determinant
Eigen::Matrix3d D = Eigen::Matrix3d::Identity();
Eigen::Matrix3d m = Eigen::Matrix3d::Random();
m = (m + Eigen::Matrix3d::Constant(1.2)) * 50;
Eigen::Vector3d v2(1,2,3);
cout << "m =" << endl << m << endl;
cout << "m * v2 =" << endl << m * v2 << endl;
//Accessing matrices:
Eigen::MatrixXd A2 = Eigen::MatrixXd::Random(7, 9);
std::cout << "The fourth row and 7th column element is " << A2(3, 6) << std::endl;
Eigen::MatrixXd B2 = A2.block(1, 2, 3, 3);
std::cout << "Take sub-matrix whose upper left corner is A(1, 2)"
<< std::endl << B2 << std::endl;
Eigen::VectorXd a2 = A2.col(1); // take the second column of A
Eigen::VectorXd b2 = B2.row(0); // take the first row of B2
Eigen::VectorXd c2 = a2.head(3);// take the first three elements of a2
Eigen::VectorXd d2 = b2.tail(2);// take the last two elements of b2
Include some header files, Basic matrix manipulation, please see Modules and Header files.
#include <iostream>
#include <Eigen/Dense>
using namespace Eigen;
using namespace std;
int main()
{
// Matrix
MatrixXd m = MatrixXd::Random(3,3);
m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
cout << "m =" << endl << m << endl;
// vector
VectorXd v(3);
v << 1, 2, 3;
cout << "m * v =" << endl << m * v << endl;
}MatrixXd m(2,2);
m(0,0) = 3;
m(1,0) = 2.5;
m(0,1) = -1;
m(1,1) = m(1,0) + m(0,1);
// print matrix
std::cout << m << std::endl;// matrix basic operator
m2 = m + m1
m2 = m*m1- Eigen Webpage
- Eigen Space transformations
- Eigen Getting started
- Eigen Quick reference guide
- Eigen Advanced initialization
- Dense matrix and array manipulation
- Eigen Library Tutorial
- A simple quickref for Eigen
from tf.transformations import *
# all matrix output from tf.transformations is 4*4 Homogeneous Transformation Matrices
# define directly from numpy array
H = numpy.array([[1,0,0,0], [0,1,0,0], [0, 0, 1, 0], [0,0,0,1]])
# unit Homogeneous Transformation Matrix
I = identity_matrix()
# translation matrix (NOTE: actually is rotation matrix is unit 3*3 matrix, and position vector is (0,0,0,1))
Position = translation_matrix((0.1, 0.2, 0.3))
# general Homogeneous Transformation matrix with R and P
Rotation = I
M = concatenate_matrices(Position, Rotation)
# sequence transformation matrix muliplication
Result = concatenate_matrices(M, M1, M2, M3, M4, M5)Quaternion to Rotation Homogeneous matrix
NOTE: quaternion only have four number (x,y,z,w) = xi + yj + zk + w
# define quaternion directly from tuple, list, or numpy arrays.
q = [0,0,0,1]
# quaternion from angle-axis
alpha = 0.123
xaxis = (1, 0, 0)
qx = quaternion_about_axis(alpha, xaxis)
# quaternion multiply which align with rotation matrix multiply
q = quaternion_multiply(qx, qy)More details, please see tf/transformations in ROS/geometry github.
# transform homogeneous_matrix (4*4 numpy arrary) to PyKDL Frame
kdl_frame = fromMatrix(homogeneous_matrix)
# PyKDL Frame to ROS Pose message
pose_msg = toMsg(kdl_frame)More detail, please see tf_conversions/posemath, PyKDL - Frame transformations (Python).
KDL::Frame object_base_kdl_frame;
KDL::Frame base_kdl_frame;
KDL::Frame rel_kdl_frame;
rel_kdl_frame = KDL::Frame(KDL::Rotation::RPY(0, 0, 0),
KDL::Vector(0.060022, 0, 0));
object_base_kdl_frame = base_kdl_frame * rel_kdl_frame;
tf::poseKDLToMsg(object_base_kdl_frame, object_base_pose);References：
Transforms3d python package is a standalone and general space transform python package which can replace the ROS tf sub-module transformations about space transform. And it support python3 and python2, and it only depend on numpy.
pip install transforms3dimport transforms3d as tf
import numpy as np
T = [20, 30, 40] # translations
R = [[0, -1, 0], [1, 0, 0], [0, 0, 1]] # rotation matrix
Z = [2.0, 3.0, 4.0] # zooms
A = tf.affines.compose(T, R, Z)output:
array([[ 0., -3., 0., 20.],
[ 2., 0., 0., 30.],
[ 0., 0., 4., 40.],
[ 0., 0., 0., 1.]])# q = (w, x, y, z) = w+xi+yj+zk
q = [0, 1, 0, 0]
# quaternion to rotation matrix
M = tf.quaternions.quat2mat(q)