This lecture mainly talks about several ways to describe the motion of rigid bodies in three-dimensional space.
Rotation matrix: redundant, with its own constraints (orthogonal matrix, and the determinant is 1), it is more difficult to further optimize in the next step.
Rotation Vectors: Compact, but Singular
Euler angles: compactness, gimbal lock problem, singularity.
Quaternion: no singularity, four degrees of freedom.
Rotation vector -> (Rodriguez formula) -> rotation matrix; otherwise, the rotation vector is solved by the trace of the rotation matrix and the eigenvector with an eigenvalue of 1;
Other conversions are no longer recorded. Anyway, it is all a series of formulas. Just know that they can be converted to each other.
Eigen test:
The code is clearly commented, it's just a library, and it's easy to use.
include <iostream>
using namespace std;
#include <ctime>
// Eigen part
#include <Eigen/Core>
// Algebraic operations on dense matrices (inverse, eigenvalues, etc.)
#include <Eigen/Dense>
#define MATRIX_SIZE 50
/ ****************************
* This program demonstrates the use of Eigen basic types
************ ****************/
int main( int argc, char** argv )
{
// All vectors and matrices in Eigen are Eigen::Matrix, which is a template class. Its first three parameters are: data type, row, column
// declare a 2*3 float matrix
Eigen::Matrix<float, 2, 3> matrix_23;
// At the same time, Eigen provides many built-in types through typedef, But the bottom layer is still Eigen::Matrix
// for example Vector3d is essentially Eigen::Matrix<double, 3, 1>, which is a three-dimensional vector
Eigen::Vector3d v_3d;
// this is the same
Eigen::Matrix<float,3 ,1> vd_3d;
// Matrix3d is essentially Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //initialized to zero
// if you are not sure about the matrix size, you can use dynamic size Eigen:: Matrix
< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// simpler
Eigen::MatrixXd matrix_x;
// there are many more of these types, we won't list them all // here
are the Operations on Eigen Matrix
// input data (initialization)
matrix_23 << 1, 2, 3, 4, 5, 6;
// output
cout << matrix_23 << endl;
// use () to access elements in the matrix
for ( int i=0; i<2; i++) {
for (int j=0; j<3; j++)
cout<<matrix_23(i,j)<<"\t";
cout<<endl;
}
// matrix Multiply with vectors (actually still matrices and matrices)
v_3d << 3, 2, 1;
vd_3d << 4,5,6;
// But in Eigen you can't mix two different types of matrices, something like this is wrong
// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// Eigen::Matrix should be converted explicitly <double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << result2 < < endl;
// Also you can't get the dimensions of the matrix wrong
// Try uncommenting the following and see what Eigen will report
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double> () * v_3d;
// Some matrix operations
// The four operations will not be demonstrated, just use +-*/ directly.
matrix_33 = Eigen::Matrix3d::Random(); // random number matrix
cout << matrix_33 << endl << endl;
cout << matrix_33.
cout << matrix_33.sum() << endl; // elements and
cout << matrix_33.trace() << endl; // trace
cout << 10*matrix_33 << endl; // multiply
cout << matrix_33 .inverse() << endl; // inverse
cout << matrix_33.determinant() << endl; // determinant
// eigenvalues
// real symmetric matrix can guarantee successful diagonalization
Eigen::SelfAdjointEigenSolver<Eigen:: Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors( ) << endl;
// Solve the equation
// We solve the equation matrix_NN * x = v_Nd
// The size of N is defined in the previous macro, which is generated by random numbers
// Direct inversion is naturally the most direct, but inversion requires a lot of computation
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt = clock(); // timing
// direct inverse
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
// usually use a matrix Decomposition to find, such as QR decomposition, will be much faster
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
return 0;
}
The CMakeLists.txt file is as follows:
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件 ，库的安装位置。
include_directories( "/usr/include/eigen3" )
# in osx and brew install
# include_directories( /usr/local/Cellar/eigen/3.3.3/include/eigen3 )
add_executable( eigenMatrix eigenMatrix.cpp )
Several representations are implemented using the Eigen geometry module:
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
// Eigen geometry module
#include <Eigen/Geometry>
/**************** ************
* This program demonstrates the use of the Eigen geometry module
**************************** */
int main ( int argc, char** argv )
{
// Eigen/Geometry module provides various representations of rotation and translation
// 3D rotation matrix directly uses Matrix3d or Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d: :Identity();
// The rotation vector uses AngleAxis, its underlying layer is not directly Matrix, but the operation can be used as a matrix (because the operator is overloaded)
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0, 0,1 ) ); //Angle, rotation axis: rotate 45 degrees along the Z axis
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //Use matrix() to convert to matrix
//You can also directly assign
rotation_matrix = rotation_vector.toRotationMatrix();
//Can be done with AngleAxis Coordinate transformation
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl ;
// Or use the rotation matrix
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// Euler angles: you can use the rotation matrix directly Convert to Euler angles
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX order, ie roll pitch yaw order
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
// The Euclidean transformation matrix uses Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // Although it is called 3d, it is actually a 4*4 matrix
T.rotate ( rotation_vector ); / / Rotate according to rotation_vector
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // Set the translation vector to (1,3,4)
cout << "Transform matrix = \n" << T. matrix() <<endl;
// Coordinate transformation with transformation matrix
Eigen::Vector3d v_transformed = T*v; // equivalent to R*v+t
cout<<"v tranformed = "<<v_transformed.transpose()< <endl; // For affine and projective transformation, use Eigen::Affine3d and Eigen::Projective3d, omit // Quaternion // You can directly assign AngleAxis to quaternion, and vice versa
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl; // Note that the order of coeffs is (x,y,z,w ), w is the real part, the first three are the imaginary part
// You can also assign the rotation matrix to it
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() < <endl;
// Use quaternion to rotate a vector, use overloaded multiplication to
v_rotated = q*v; // Note that mathematically it is qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
return 0;
}
The CMakeLists.txt file is as follows:
cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenGeometry eigenGeometry.cpp )
