├── .gitignore ├── 1_Intro_Installation.md ├── 2_Matrix_Array_Vector_Class.md ├── 3_Matrix_Operations.md ├── 4_Advanced_Eigen_Operations.md ├── 5_Dense_Linear_Problems_And_Decompositions.md ├── 6_Sparse_Matrices.md ├── 7_Geometry_Transformation.md ├── 8_Differentiation.md ├── 9_Numerical_Optimization.md ├── CMakeLists.txt ├── CMakePresets.json ├── LICENSE ├── README.md ├── images ├── 3D_rotation_converter.png ├── Angle_axis_vector.svg ├── RPY_angles_of_airplanes.png ├── RPY_angles_of_cars.png ├── Singular-Value-Decomposition.svg ├── angulare_velocity_2.jpg ├── exponential_coordinates_for_rigid-body_motions.png ├── exponential_coordinates_for_rigid-body_motions2.png ├── exponential_coordinates_for_rotation.png ├── gram_schmidt1.png ├── gram_schmidt2.png ├── gram_schmidt3.png ├── gram_schmidt4.png ├── gram_schmidt5.png ├── gram_schmidt6.png ├── gram_schmidt7.png ├── gram_schmidt8.png ├── linear_velocity.png ├── quaternions.online.png ├── ref0.svg ├── row_echelon_form.svg ├── screw_1.jpg ├── transformtion_1.jpg ├── transformtion_2.jpg └── transformtion_3.jpg ├── src ├── angle_axis_rodrigues.cpp ├── auto_diff.cpp ├── calculate_range_of_matrix_column_space.cpp ├── check_matrixsimilarity.cpp ├── compute_basis_of_nullspace.cpp ├── create_transformation_matrices.cpp ├── eigen_functor.cpp ├── eigen_value_eigen_vector.cpp ├── euler_angle.cpp ├── geometry_transformation.cpp ├── glm.cpp ├── gram_schmidt_orthogonalization.cpp ├── levenberg_marquardt.cpp ├── main.cpp ├── matrix_array_vector.cpp ├── matrix_broadcasting.cpp ├── matrix_condition_numerical_stability.cpp ├── matrix_decomposition.cpp ├── memory_mapping.cpp ├── non_linear_least_squares.cpp ├── non_linear_regression.cpp ├── null_space_kernel_rank.cpp ├── numerical_diff.cpp ├── numerical_methods.cpp ├── quaternion.cpp ├── rotation_matrices.cpp ├── singular_value_decomposition.cpp ├── solving_system_of_linear_equations.cpp ├── sparse_matrices.cpp └── unaryExpr.cpp └── vidoes ├── gimbal_locl_beta_pi_2.mp4 └── rotation_translation.mp4 /.gitignore: -------------------------------------------------------------------------------- 1 | build/* 2 | sandbox/* 3 | *.user 4 | *.kdev4 5 | narration_scripts/* 6 | -------------------------------------------------------------------------------- /1_Intro_Installation.md: -------------------------------------------------------------------------------- 1 | # Chapter 1 Introduction and Installation 2 | - [About Eigen](#about-eigen) 3 | - [Instillation](#instillation) 4 | - [Adding Eigen to Your Project](#adding-eigen-to-your-project) 5 | 6 | 7 | # About Eigen 8 | Eigen is a pure header library meaning everything is in the `.hpp` files. Eigen is implemented using the expression templates technique, which builds structures at compile time, where expressions are evaluated only as needed. This means it builds expression trees at compile time and generates custom code to evaluate them 9 | This technique enables the programmer to bypass the normal order of expression evaluation in the C++ and achieve optimizations such as loop fusion and loop unrolling. 10 | 11 | # Instillation 12 | Installation is fairly straightforward, on debian based OS: 13 | ``` 14 | sudo apt-get install libeigen3-dev 15 | ``` 16 | or if you want to build it from source code just clone it: 17 | ``` 18 | git clone https://gitlab.com/libeigen/eigen.git 19 | ``` 20 | make the the build directory: 21 | ``` 22 | cd eigen 23 | mkdir build 24 | cd build 25 | ``` 26 | Now build and install it, 27 | ``` 28 | cmake -DCMAKE_CXX_FLAGS=-std=c++1z -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX:PATH=~/usr .. && make -j8 all install 29 | ``` 30 | please note that I have set: 31 | ``` 32 | -DCMAKE_INSTALL_PREFIX:PATH=~/usr 33 | ``` 34 | This way the Eigen will be installed `in home//usr` and not in the `/usr` so no root privilege is needed. 35 | 36 | 37 | # Adding Eigen to Your Project 38 | 39 | Now in your CMake file you have to set the `Eigen3_DIR`: 40 | 41 | 42 | ``` 43 | set(Eigen3_DIR "$ENV{HOME}/usr/share/eigen3/cmake") 44 | 45 | find_package (Eigen3 REQUIRED NO_MODULE) 46 | 47 | MESSAGE("EIGEN3_FOUND: " ${EIGEN3_FOUND}) 48 | MESSAGE("EIGEN3_INCLUDE_DIR: " ${EIGEN3_INCLUDE_DIR}) 49 | MESSAGE("EIGEN3_VERSION: " ${EIGEN3_VERSION}) 50 | MESSAGE("EIGEN3_VERSION_STRING: " ${EIGEN3_VERSION_STRING}) 51 | 52 | 53 | add_executable (example example.cpp) 54 | target_link_libraries (example Eigen3::Eigen) 55 | ``` 56 | 57 | [Home](README.md) [Next >>](2_Matrix_Array_Vector_Class.md) 58 | 59 | 60 | -------------------------------------------------------------------------------- /2_Matrix_Array_Vector_Class.md: -------------------------------------------------------------------------------- 1 | # Chapter 2 Matrix, Array and Vector Class 2 | - [Matrix Class](#matrix-class) 3 | - [Vector Class](#vector-class) 4 | - [Array Class](#array-class) 5 | * [Converting Array to Matrix](#converting-array-to-matrix) 6 | * [Converting Matrix to Array](#converting-matrix-to-array) 7 | - [Initialization](#initialization) 8 | - [Accessing Elements (Coefficient)](#accessing-elements--coefficient-) 9 | * [Accessing via parenthesis](#accessing-via-parenthesis) 10 | * [Accessing via pointer to data](#accessing-via-pointer-to-data) 11 | * [Row Major Access](#row-major-access) 12 | * [Accessing a block of data](#accessing-a-block-of-data) 13 | - [Reshaping, Resizing, Slicing](#reshaping--resizing--slicing) 14 | - [Tensor Module](#tensor-module) 15 | 16 | # Matrix Class 17 | The Matrix class takes six template parameters, the first mandatory three first parameters are: 18 | 19 | ``` 20 | Eigen::Matrix 21 | ``` 22 | 23 | For instance: 24 | ``` 25 | Eigen::Matrix matrix; 26 | 27 | ``` 28 | If dimensions are known at compile time, you can use `Eigen::Dynamic` as the template parameter. for instance 29 | ``` 30 | Eigen::Matrix matrix; 31 | ``` 32 | 33 | Eigen offers a lot of convenience `typedefs` to cover the usual cases, the definition of these matrices (also vectors and arrays) in Eigen comes in the following form: 34 | ``` 35 | Eigen::MatrixSizeType 36 | Eigen::VectorSizeType 37 | Eigen::ArraySizeType 38 | ``` 39 | Type can be: 40 | - i for integer, 41 | - f for float, 42 | - d for double, 43 | - c for complex, 44 | - cf for complex float, 45 | - cd for complex double. 46 | 47 | Size can be 2,3,4 for fixed size square matrices or `X` for dynamic size 48 | 49 | For example, `Matrix4f` is a `4x4` matrix of floats. Here is how it is defined by Eigen: 50 | ``` 51 | typedef Matrix Matrix4f; 52 | ``` 53 | or 54 | ``` 55 | typedef Matrix MatrixXd; 56 | ``` 57 | 58 | 59 | Here are more examples: 60 | 61 | ``` 62 | Eigen::Matrix4d m; // 4x4 double 63 | 64 | Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex 65 | 66 | 67 | //a is a 3x3 matrix, with a static float[9] array of uninitialized coefficients, 68 | Eigen::Matrix3f a; 69 | 70 | //b is a dynamic-size matrix whose size is currently 0x0, and whose array of coefficients hasn't yet been allocated at all. 71 | Eigen::MatrixXf b; 72 | 73 | //A is a 10x15 dynamic-size matrix, with allocated but currently uninitialized coefficients. 74 | Eigen::MatrixXf A(10, 15); 75 | ``` 76 | 77 | 78 | 79 | # Vector Class 80 | Vectors are single row/ single column matrices. For instance: 81 | ``` 82 | // Vector3f is a fixed column vector of 3 floats: 83 | Eigen::Vector3f objVector3f; 84 | 85 | // RowVector2i is a fixed row vector of 3 integer: 86 | Eigen::RowVector2i objRowVector2i; 87 | 88 | // VectorXf is a column vector of size 10 floats: 89 | Eigen::VectorXf objv(10); 90 | 91 | //V is a dynamic-size vector of size 30, with allocated but currently uninitialized coefficients. 92 | Eigen::VectorXf V(30); 93 | ``` 94 | You get/ set each row or column of marix with/ from a vector: 95 | 96 | ``` 97 | Eigen::Matrix2d mat; 98 | mat<<1,2,3,4; 99 | Eigen::RowVector2d firstRow=mat.row(0); 100 | Eigen::Vector2d firstCol=mat.col(0); 101 | 102 | 103 | firstRow = Eigen::RowVector2d::Random(); 104 | firstCol = Eigen::Vector2d::Random(); 105 | 106 | 107 | mat.row(0) =firstRow; 108 | mat.col(0) = firstCol; 109 | ``` 110 | 111 | 112 | # Array Class 113 | Eigen Matrix class is intended for linear algebra. The Array class is the general-purpose arrays, 114 | which has coefficient-wise operations, such as adding a constant to every coefficient, multiplying two arrays coefficient-wise, etc. 115 | 116 | ``` 117 | // ArrayXf 118 | Eigen::Array a1; 119 | // Array3f 120 | Eigen::Array a2; 121 | // ArrayXXd 122 | Eigen::Array a3; 123 | // Array33d 124 | Eigen::Array a4; 125 | Eigen::Matrix3d matrix_from_array = a4.matrix(); 126 | ``` 127 | 128 | ## Converting Array to Matrix 129 | ``` 130 | Eigen::Array array1=Eigen::ArrayXd::Random(4,4); 131 | Eigen::Array mat1=array1.matrix(); 132 | ``` 133 | 134 | ## Converting Matrix to Array 135 | ``` 136 | Eigen::Matrix mat1=Eigen::MatrixXd::Random(4,4); 137 | Eigen::Array array1=mat1.array(); 138 | ``` 139 | 140 | 141 | 142 | # Initialization 143 | You can initialize your matrix with comma 144 | ``` 145 | Eigen::Matrix matrix; 146 | matrix << 1, 2, 3, 4, 5, 6; 147 | ``` 148 | 149 | There are various out of the box APIs for special matrics, for intance 150 | 151 | ``` 152 | Eigen::Matrix2d rndMatrix; 153 | rndMatrix.setRandom(); 154 | 155 | Eigen::Matrix2d constantMatrix; 156 | constantMatrix.setConstant(4.3); 157 | 158 | Eigen::MatrixXd identity=Eigen::MatrixXd::Identity(6,6); 159 | 160 | Eigen::MatrixXd zeros=Eigen::MatrixXd::Zero(3, 3); 161 | 162 | Eigen::ArrayXXf table(10, 4); 163 | table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90); 164 | ``` 165 | 166 | 167 | # Accessing Elements (Coefficient) 168 | ## Accessing via parenthesis 169 | Eigen has overloaded parenthesis operators, that means you can access the elements with row and column index: `matrix(row,col)`. 170 | 171 | All Eigen matrices default to column-major storage order. That means, `matrix(2)` is the the third element of first column matix(2,0): 172 | 173 | 174 | ``` 175 | Eigen::MatrixXf matrix(4, 4); 176 | matrix << 1, 2, 3, 4, 177 | 5, 6, 7, 8, 178 | 9, 10, 11, 12, 179 | 13, 14, 15, 16; 180 | matrix(2): "< matrixRowMajor(4, 4); 199 | ``` 200 | 201 | ## Accessing a block of data 202 | 203 | You can have access to a block in a matrix. 204 | ``` 205 | int starting_row,starting_column,number_rows_in_block,number_cols_in_block; 206 | 207 | starting_row=1; 208 | starting_column=1; 209 | number_rows_in_block=2; 210 | number_cols_in_block=2; 211 | matrix.block(starting_row,starting_column,number_rows_in_block,number_cols_in_block); 212 | ``` 213 | 214 | 215 | ## Casing Matrices 216 | 217 | ```cpp 218 | Eigen::Matrix matrix_23; 219 | // Eigen::Matrix vd_3d; 220 | Eigen::Vector3d v_3d; 221 | 222 | // wrong Eigen::Matrix result_wrong_type = matrix_23 ∗ v_3d; 223 | 224 | Eigen::Matrix result = matrix_23.cast() * v_3d; 225 | ``` 226 | 227 | 228 | # Reshaping, Resizing, Slicing 229 | The current size of a matrix can be retrieved by `rows()`, `cols()` and `size()` method. Resizing a dynamic-size matrix is done by the resize() method, which is a no-operation if the actual matrix size doesn't change, i.e. chaning from `3x4` to `6x2` 230 | 231 | ``` 232 | int rows, cols; 233 | rows=3; 234 | cols=4; 235 | Eigen::Matrix dynamicMatrix; 236 | 237 | dynamicMatrix.resize(rows,cols); 238 | dynamicMatrix=Eigen::MatrixXd::Random(rows,cols); 239 | dynamicMatrix.resize(2,6); 240 | ``` 241 | 242 | If your new size is different than matrix size, and you want a conservative your data, you should use `conservativeResize()` method 243 | ``` 244 | dynamicMatrix.conservativeResize(dynamicMatrix.rows(), dynamicMatrix.cols()+1); 245 | dynamicMatrix.col(dynamicMatrix.cols()-1) = Eigen::Vector2d(1, 4); 246 | ``` 247 | 248 | 249 | # Tensor Module 250 | 251 | 252 | [<< Previous ](1_Intro_Installation.md) [Home](README.md) [ Next >>](3_Matrix_Operations.md) 253 | 254 | 255 | 256 | -------------------------------------------------------------------------------- /3_Matrix_Operations.md: -------------------------------------------------------------------------------- 1 | # Chapter 3 Matrix Operations 2 | - [Matrix Arithmetic](#matrix-arithmetic) 3 | * [Addition/Subtraction Matrices/ Scalar](#addition-subtraction-matrices--scalar) 4 | * [Scalar Multiplication/ Division](#scalar-multiplication--division) 5 | * [Multiplication, Dot And Cross Product](#multiplication--dot-and-cross-product) 6 | * [Transposition and Conjugation](#transposition-and-conjugation) 7 | - [Coefficient-Wise Operations](#coefficient-wise-operations) 8 | * [Multiplication](#multiplication) 9 | * [Absolute](#absolute) 10 | * [Power, Root](#power--root) 11 | * [Log, Exponential](#log--exponential) 12 | * [Min, Mix of Two Matrices](#min--mix-of-two-matrices) 13 | * [Check Matrices Similarity](#check-matrices-similarity) 14 | * [Finite, Inf, NaN](#finite--inf--nan) 15 | * [Sinusoidal](#sinusoidal) 16 | * [Floor, Ceil, Round](#floor--ceil--round) 17 | * [Masking Elements](#masking-elements) 18 | - [Reductions](#reductions) 19 | * [Minimum/ Maximum Element In The Matrix](#minimum--maximum-element-in-the-matrix) 20 | * [Minimum/ Maximum Element Row-wise/Col-wise in the Matrix](#minimum--maximum-element-row-wise-col-wise-in-the-matrix) 21 | * [Sum Of All Elements](#sum-of-all-elements) 22 | * [Mean Of The Matrix](#mean-of-the-matrix) 23 | * [Mean Of The Matrix Row-wise/Col-wise](#mean-of-the-matrix-row-wise-col-wise) 24 | * [The Trace Of The Matrix](#the-trace-of-the-matrix) 25 | * [The Multiplication Of All Elements](#the-multiplication-of-all-elements) 26 | * [Norm 2 of The Matrix](#norm-2-of-the-matrix) 27 | * [Norm Infinity Of The Matrix](#norm-infinity-of-the-matrix) 28 | * [Checking If All Elements Are Positive](#checking-if-all-elements-are-positive) 29 | * [Checking If Any Elements Is Positive](#checking-if-any-elements-is-positive) 30 | * [Counting Elements](#counting-elements) 31 | * [Matrix Condition Number and Numerical Stability](#matrix-condition-number) 32 | * [Matrix Rank](#matrix-rank) 33 | - [Broadcasting](#broadcasting) 34 | 35 | 36 | # Matrix Arithmetic 37 | ## Addition/Subtraction Matrices/ Scalar 38 | 39 | 40 | ``` 41 | matrix.array() - 2; 42 | ``` 43 | 44 | ## Scalar Multiplication/ Division 45 | 46 | 47 | 48 | ## Multiplication, Dot And Cross Product 49 | 50 | ## Transposition and Conjugation 51 | 52 | # Coefficient-Wise Operations 53 | ## Multiplication 54 | ## Absolute 55 | .abs() 56 | 57 | ## Power, Root 58 | sqrt() 59 | array1.sqrt() sqrt(array1) 60 | array1.square() 61 | array1.cube() 62 | array1.pow(array2) pow(array1,array2) 63 | array1.pow(scalar) pow(array1,scalar) 64 | pow(scalar,array2) 65 | 66 | 67 | ## Log, Exponential 68 | 69 | array1.log() log(array1) 70 | array1.log10() log10(array1) 71 | array1.exp() exp(array1) 72 | 73 | ## Min, Mix of Two Matrices 74 | .min(.) 75 | max 76 | 77 | If you have two arrays of the same size, you can call .min(.) to construct the array whose coefficients are the minimum of the corresponding coefficients of the two given arrays. 78 | 79 | ## Check Matrices Similarity 80 | 81 | ## Finite, Inf, NaN 82 | 83 | 84 | array1.isFinite() isfinite(array1) 85 | array1.isInf() isinf(array1) 86 | array1.isNaN() 87 | 88 | ## Sinusoidal 89 | 90 | array1.sin() sin(array1) 91 | array1.cos() cos(array1) 92 | array1.tan() tan(array1) 93 | array1.asin() asin(array1) 94 | array1.acos() acos(array1) 95 | array1.atan() atan(array1) 96 | array1.sinh() sinh(array1) 97 | array1.cosh() cosh(array1) 98 | array1.tanh() tanh(array1) 99 | array1.arg() arg(array1) 100 | 101 | ## Floor, Ceil, Round 102 | array1.floor() floor(array1) 103 | array1.ceil() ceil(array1) 104 | array1.round() round(aray1) 105 | 106 | 107 | 108 | ## Masking Elements 109 | In the following, we want to replace the element of our matrix, with an element from the either matrices `P` or `Q`. If the value in our matrix is smaller than a threshold, we replace it with an element from `P` otherwise from `Q`. 110 | 111 | ``` 112 | int cols, rows; 113 | cols=2; rows=3; 114 | Eigen::MatrixXf R=Eigen::MatrixXf::Random(rows, cols); 115 | 116 | Eigen::MatrixXf Q=Eigen::MatrixXf::Zero(rows, cols); 117 | Eigen::MatrixXf P=Eigen::MatrixXf::Constant(rows, cols,1.0); 118 | 119 | double threshold=0.5; 120 | Eigen::MatrixXf masked=(R.array() < threshold).select(P,Q ); // (R < threshold ? P : Q) 121 | ``` 122 | 123 | # Reductions 124 | 125 | ## Minimum/ Maximum Element In The Matrix 126 | 127 | ``` 128 | int min_element_row_index,min_element_col_index; 129 | matrix.minCoeff(&min_element_row_index,&min_element_col_index); 130 | ``` 131 | ## Minimum/ Maximum Element Row-wise/Col-wise in the Matrix 132 | ``` 133 | matrix.rowwise().maxCoeff(); 134 | ``` 135 | ## Sum Of All Elements 136 | ``` 137 | matrix.sum(); 138 | ``` 139 | ## Mean Of The Matrix 140 | ``` 141 | matrix.mean() 142 | ``` 143 | ## Mean Of The Matrix Row-wise/Col-wise 144 | ``` 145 | matrix.colwise().mean(); 146 | ``` 147 | 148 | ## The Trace Of The Matrix 149 | ``` 150 | matrix.trace(); 151 | ``` 152 | 153 | ## The Multiplication Of All Elements 154 | ``` 155 | matrix.prod(); 156 | ``` 157 | 158 | ## Norm 2 of The Matrix 159 | ``` 160 | matrix.lpNorm<2>() 161 | ``` 162 | 163 | ## Norm Infinity Of The Matrix 164 | ``` 165 | matrix.lpNorm() 166 | ``` 167 | ## Checking If All Elements Are Positive 168 | ``` 169 | (matrix.array()>0).all(); 170 | ``` 171 | ## Checking If Any Elements Is Positive 172 | ``` 173 | (matrix.array()>0).any(); 174 | ``` 175 | ## Counting Elements 176 | ``` 177 | (matrix.array()>1).count(); 178 | ``` 179 | ## Matrix Condition Number and Numerical Stability 180 | 181 | ## Matrix Rank 182 | The column rank of a matrix is the maximal number of linearly independent columns of that matrix. The column rank of a matrix is in the dimension of the column space, while the row rank of A is the dimension of the row space. It can be proven that https://latex.codecogs.com/svg.latex? \text{column rank} =\text{row rank} 183 | 184 | Full rank: if its rank equals the largest possible for a matrix of the same dimensions, which is the lesser of the number of rows and columns 185 | 186 | A matrix is said to be rank-deficient if it does not have full rank. The rank deficiency of a matrix is the difference between the lesser the number of rows and columns, and the rank. 187 | 188 | 189 | Decomposing the matrix is the most common way to get the rank 190 | 191 | Gaussian Elimination (row reduction): 192 | 193 | This method can also be used to compute the rank of a matrix 194 | the determinant of a square matrix 195 | the inverse of an invertible matrix 196 | 197 | Row echelon form: means that Gaussian elimination has operated on the rows 198 | Column echelon form 199 | # Broadcasting 200 | 201 | 202 | [<< Previous ](2_Matrix_Array_Vector_Class.md) [Home](README.md) [ Next >>](4_Advanced_Eigen_Operations.md) 203 | 204 | -------------------------------------------------------------------------------- /4_Advanced_Eigen_Operations.md: -------------------------------------------------------------------------------- 1 | # Chapter 4 Advanced Eigen Operations 2 | - [Memory Alignment](#memory-alignment) 3 | - [Passing Eigen objects by value to functions](#passing-eigen-objects-by-value-to-functions) 4 | - [Aliasing](#aliasing) 5 | - [Memory Mapping](#memory-mapping) 6 | - [Unary Expression](#unary-expression) 7 | - [Eigen Functor](#eigen-functor) 8 | 9 | # Memory Alignment 10 | # Passing Eigen objects by value to functions 11 | 12 | Refs: [1](https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html) 13 | 14 | # Aliasing 15 | 16 | # Memory Mapping 17 | In many applications, your data has been stored in different data structures and you need to perform some operations on the data. Suppose you have the following class for presenting your points and you have shape which is `std::vector` of such point and now you need to perform an affine matrix transformation on your shape. 18 | ``` 19 | struct point { 20 | double a; 21 | double b; 22 | }; 23 | ``` 24 | One costly approach would be to iterate over your container and copy your data to some Eigen matrix. But there is a better approach. Eigen enables you to map the memory (of your existing data) into your matrices without copying. 25 | 26 | 27 | ## Eigen matrix from std::vector 28 | 29 | ```cpp 30 | std::vector data = {1, 2, 3, 4, 5, 6, 7, 8, 9}; 31 | ``` 32 | 33 | The default alignment is column major: 34 | 35 | ```cpp 36 | auto einMatColMajor = Eigen::Map>(data.data()); 37 | auto einMatRowMajor = Eigen::Map>(data.data()); 38 | ``` 39 | 40 | 41 | 42 | # Unary Expression 43 | # Eigen Functor 44 | 45 | [<< Previous ](3_Matrix_Operations.md) [Home](README.md) [ Next >>](5_Dense_Linear_Problems_And_Decompositions.md) 46 | -------------------------------------------------------------------------------- /6_Sparse_Matrices.md: -------------------------------------------------------------------------------- 1 | # Chapter 6 Sparse Matrices 2 | - [Sparse Matrix Manipulations](#sparse-matrix-manipulations) 3 | * [Compressed Sparse Row](#compressed-sparse-row) 4 | - [Solving Sparse Linear Systems](#solving-sparse-linear-systems) 5 | - [Matrix Free Solvers](#matrix-free-solvers) 6 | 7 | 8 | # Sparse Matrix Manipulations 9 | ## Compressed Sparse Row 10 | 11 | # Solving Sparse Linear Systems 12 | # Matrix Free Solvers 13 | 14 | 15 | [<< Previous ](5_Dense_Linear_Problems_And_Decompositions.md) [Home](README.md) [ Next >>](7_Geometry_Transformation.md) 16 | -------------------------------------------------------------------------------- /8_Differentiation.md: -------------------------------------------------------------------------------- 1 | # Chapter 8 Differentiation 2 | - [Jacobian](#jacobian) 3 | - [Hessian Matrix](#hessian-matrix) 4 | - [Automatic Differentiation](#automatic-differentiation) 5 | - [Numerical Differentiation](#numerical-differentiation) 6 | 7 | # Jacobian 8 | 9 | Suppose https://latex.codecogs.com/svg.latex?f : R_{n} \rightarrow R_{m}. This function takes a point https://latex.codecogs.com/svg.latex?x \in R_{n} as input and produces the vector 10 | https://latex.codecogs.com/svg.latex?f(x) \in R_{m} as output. Then the Jacobian matrix of 11 | https://latex.codecogs.com/svg.latex?f is defined to be an m×n matrix, denoted by https://latex.codecogs.com/svg.latex?J , whose https://latex.codecogs.com/svg.latex?(i,j)_{th} entry is: https://latex.codecogs.com/svg.latex?{\textstyle \mathbf {J} _{ij}={\frac {\partial f_{i}}{\partial x_{j}}}} or explicitly: 12 | 13 | https://latex.codecogs.com/svg.latex?{\displaystyle \mathbf {J} ={\begin{bmatrix}{\dfrac {\partial \mathbf {f} }{\partial x_{1}}}&\cdots &{\dfrac {\partial \mathbf {f} }{\partial x_{n}}}\end{bmatrix}}={\begin{bmatrix}\nabla ^{\mathrm {T} }f_{1}\\\vdots \\\nabla ^{\mathrm {T} }f_{m}\end{bmatrix}}={\begin{bmatrix}{\dfrac {\partial f_{1}}{\partial x_{1}}}&\cdots &{\dfrac {\partial f_{1}}{\partial x_{n}}}\\\vdots &\ddots &\vdots \\{\dfrac {\partial f_{m}}{\partial x_{1}}}&\cdots &{\dfrac {\partial f_{m}}{\partial x_{n}}}\end{bmatrix}}} 14 | 15 | 16 | 17 | 18 | # Hessian Matrix 19 | The Hessian matrix 20 | https://latex.codecogs.com/svg.latex?H of f is a square https://latex.codecogs.com/svg.latex?n \times n matrix, usually defined and arranged as follows: 21 | 22 | https://latex.codecogs.com/svg.latex?{\displaystyle \mathbf {H} _{f}={\begin{bmatrix}{\dfrac {\partial ^{2}f}{\partial x_{1}^{2}}}&{\dfrac {\partial ^{2}f}{\partial x_{1}\,\partial x_{2}}}&\cdots &{\dfrac {\partial ^{2}f}{\partial x_{1}\,\partial x_{n}}}\\[2.2ex]{\dfrac {\partial ^{2}f}{\partial x_{2}\,\partial x_{1}}}&{\dfrac {\partial ^{2}f}{\partial x_{2}^{2}}}&\cdots &{\dfrac {\partial ^{2}f}{\partial x_{2}\,\partial x_{n}}}\\[2.2ex]\vdots &\vdots &\ddots &\vdots \\[2.2ex]{\dfrac {\partial ^{2}f}{\partial x_{n}\,\partial x_{1}}}&{\dfrac {\partial ^{2}f}{\partial x_{n}\,\partial x_{2}}}&\cdots &{\dfrac {\partial ^{2}f}{\partial x_{n}^{2}}}\end{bmatrix}}} 23 | 24 | 25 | We can approximation Hessian Matrix by 26 | https://latex.codecogs.com/svg.latex?H\approx J^TJ 27 | 28 | Example: suppose you have the following function: 29 | 30 | https://latex.codecogs.com/svg.latex?\left\{\begin{matrix} 31 | y_0 = 10\times(x_0+3)^2 + (x_1-5)^2 \\ 32 | y_1 = (x_0+1)\times x_1\\ 33 | y_2 = sin(x_0)\times x_1 34 | \end{matrix}\right. 35 | 36 | 37 | In the next you will see how can we compute the Jacobian matrix with `Automatic Differentiation` and `Numerical Differentiation` 38 | 39 | Refs: [1](https://stats.stackexchange.com/questions/71154/when-an-analytical-jacobian-is-available-is-it-better-to-approximate-the-hessia) 40 | 41 | # Automatic Differentiation 42 | First let's create a generic functor, 43 | ``` 44 | template 45 | struct VectorFunction { 46 | 47 | typedef _Scalar Scalar; 48 | 49 | enum 50 | { 51 | InputsAtCompileTime = NX, 52 | ValuesAtCompileTime = NY 53 | }; 54 | 55 | // Also needed by Eigen 56 | typedef Eigen::Matrix InputType; 57 | typedef Eigen::Matrix ValueType; 58 | 59 | // Vector function 60 | template 61 | void operator()(const Eigen::Matrix& x, Eigen::Matrix* y ) const 62 | { 63 | (*y)(0,0) = 10.0*pow(x(0,0)+3.0,2) +pow(x(1,0)-5.0,2) ; 64 | (*y)(1,0) = (x(0,0)+1)*x(1,0); 65 | (*y)(2,0) = sin(x(0,0))*x(1,0); 66 | } 67 | }; 68 | ``` 69 | 70 | Now lets prepare the input and out matrices and compute the jacobian: 71 | ``` 72 | Eigen::Matrix x; 73 | Eigen::Matrix y; 74 | Eigen::Matrix fjac; 75 | 76 | Eigen::AutoDiffJacobian< VectorFunction > JacobianDerivatives; 77 | 78 | // Set values in x, y and fjac... 79 | 80 | 81 | x(0,0)=2.0; 82 | x(1,0)=3.0; 83 | 84 | JacobianDerivatives(x, &y, &fjac); 85 | 86 | std::cout << "jacobian of matrix at "<< x(0,0)<<","< 98 | struct Functor 99 | { 100 | // Information that tells the caller the numeric type (eg. double) and size (input / output dim) 101 | typedef _Scalar Scalar; 102 | enum { 103 | InputsAtCompileTime = NX, 104 | ValuesAtCompileTime = NY 105 | }; 106 | // Tell the caller the matrix sizes associated with the input, output, and jacobian 107 | typedef Eigen::Matrix InputType; 108 | typedef Eigen::Matrix ValueType; 109 | typedef Eigen::Matrix JacobianType; 110 | 111 | // Local copy of the number of inputs 112 | int m_inputs, m_values; 113 | 114 | // Two constructors: 115 | Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 116 | Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 117 | 118 | // Get methods for users to determine function input and output dimensions 119 | int inputs() const { return m_inputs; } 120 | int values() const { return m_values; } 121 | 122 | }; 123 | ``` 124 | Then inherit from the above generic functor and override the `operator()` and implement your function: 125 | 126 | ``` 127 | struct simpleFunctor : Functor 128 | { 129 | simpleFunctor(void): Functor(2,3) 130 | { 131 | 132 | } 133 | // Implementation of the objective function 134 | // y0 = 10*(x0+3)^2 + (x1-5)^2 135 | // y1 = (x0+1)*x1 136 | // y2 = sin(x0)*x1 137 | int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const 138 | { 139 | fvec(0) = 10.0*pow(x(0)+3.0,2) +pow(x(1)-5.0,2) ; 140 | fvec(1) = (x(0)+1)*x(1); 141 | fvec(2) = sin(x(0))*x(1); 142 | return 0; 143 | } 144 | }; 145 | ``` 146 | Now you can easily use it: 147 | ``` 148 | 149 | simpleFunctor functor; 150 | Eigen::NumericalDiff numDiff(functor); 151 | Eigen::MatrixXd fjac(3,2); 152 | Eigen::VectorXd x(2); 153 | x(0) = 2.0; 154 | x(1) = 3.0; 155 | numDiff.df(x,fjac); 156 | std::cout << "jacobian of matrix at "<< x(0)<<","<>](9_Numerical_Optimization.md) 165 | -------------------------------------------------------------------------------- /9_Numerical_Optimization.md: -------------------------------------------------------------------------------- 1 | # Chapter 9 Numerical Optimization 2 | - [Newton's Method In Optimization](#newton-s-method-in-optimization) 3 | - [Gauss-Newton Algorithm](#gauss-newton-algorithm) 4 | + [Example of Gauss-Newton, Inverse Kinematic Problem](#example-of-gauss-newton--inverse-kinematic-problem) 5 | - [Curve Fitting](#curve-fitting) 6 | - [Non Linear Least Squares](#non-linear-least-squares) 7 | - [Non Linear Regression](#non-linear-regression) 8 | - [Levenberg Marquardt](#levenberg-marquardt) 9 | 10 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | # Newton's Method In Optimization 20 | Newton's method is an iterative method for finding the roots of a differentiable function https://latex.codecogs.com/svg.latex?F. Applying Newton's method to the derivative https://latex.codecogs.com/svg.latex?f^\prime of a twice-differentiable function https://latex.codecogs.com/svg.latex?f to find the roots of the derivative, (solutions to https://latex.codecogs.com/svg.latex?f^\prime(x)=0) will give us **critical points** (minima, maxima, or saddle points) 21 | 22 | The second-order Taylor expansion of https://latex.codecogs.com/svg.latex?{\displaystyle f:\mathbb {R} \to \mathbb {R} } around https://latex.codecogs.com/svg.latex?{\displaystyle x_{k}} is: 23 | 24 | 25 | 26 | https://latex.codecogs.com/svg.latex?{\displaystyle f(x_{k}+t)\approx f(x_{k})+f'(x_{k})t+{\frac {1}{2}}f''(x_{k})t^{2}.} 27 | 28 | 29 | setting the derivative to zero: 30 | 31 | 32 | https://latex.codecogs.com/svg.latex?{\displaystyle \displaystyle 0={\frac {\rm {d}}{{\rm {d}}t}}\left(f(x_{k})+f'(x_{k})t+{\frac {1}{2}}f''(x_{k})t^{2}\right)=f'(x_{k})+f''(x_{k})t,} 33 | 34 | This will give us: 35 | 36 | 37 | https://latex.codecogs.com/svg.latex?{\displaystyle t=-{\frac {f'(x_{k})}{f''(x_{k})}}.} 38 | 39 | We start the next point https://latex.codecogs.com/svg.latex?x_{k+1} at where the second order approximation is zero, so 40 | https://latex.codecogs.com/svg.latex?x_{k+1}=t+x_{k} 41 | 42 | By putting everything together: 43 | 44 | 45 | https://latex.codecogs.com/svg.latex?{\displaystyle x_{k+1}=x_{k}-{\frac {f'(x_{k})}{f''(x_{k})}}.} 46 | 47 | So we iterate this until the changes are small enough. 48 | 49 | # Gauss-Newton Algorithm 50 | The Gauss-Newton algorithm is a modification of Newton's method for finding a minimum of a function. 51 | Unlike Newton's method, second derivatives, (which can be challenging to compute), are not required. 52 | 53 | 54 | Starting with an initial guess https://latex.codecogs.com/svg.latex?{\displaystyle {\boldsymbol {\beta }}^{(0)}} for the minimum, the method proceeds by the iterations 55 | 56 | https://latex.codecogs.com/svg.latex?{\displaystyle {\boldsymbol {\beta }}^{(s+1)}={\boldsymbol {\beta }}^{(s)}-\left(\mathbf {J_{f}} ^{\mathsf {T}}\mathbf {J_{f}} \right)^{-1}\mathbf {J_{f}} ^{\mathsf {T}}\mathbf {f} \left({\boldsymbol {\beta }}^{(s)}\right)} 57 | 58 | 59 | The https://latex.codecogs.com/svg.latex?{\displaystyle \left(\mathbf {J_{f}} ^{\mathsf {T}}\mathbf {J_{f}} \right)^{-1}\mathbf {J_{f}} ^{\mathsf {T}}} is called Pseudoinverse. To compute this matrix we use the following property: 60 | 61 | Let https://latex.codecogs.com/svg.latex?A be a 62 | https://latex.codecogs.com/svg.latex?m\times n matrix with the SVD: 63 | 64 | https://latex.codecogs.com/svg.latex?A = U \Sigma V^T 65 | 66 | and 67 | 68 | https://latex.codecogs.com/svg.latex?A^+ = (A^T A)^{-1} A^T 69 | 70 | 71 | Then, we can write the pseudoinverse as: 72 | 73 | https://latex.codecogs.com/svg.latex?A^+ = V \Sigma^{-1} U^T 74 | 75 | 76 | Proof: 77 | 78 | https://latex.codecogs.com/svg.latex?\begin{align} 79 | A^+ &= (A^TA)^{-1}A^T\\ 80 | &=(V\Sigma U^TU\Sigma V^T)^{-1} V\Sigma U^T \\ 81 | &=(V\Sigma^2 V^T)^{-1} V\Sigma U^T \\ 82 | &=(V^T)^{-1} \Sigma^{-2} V^{-1} V\Sigma U^T \\ 83 | &= V \Sigma^{-2}\Sigma U^T \\ 84 | &= V\Sigma^{-1}U^T\\ 85 | \end{align} 86 | 87 | 88 | Refs: [1](https://math.stackexchange.com/questions/19948/pseudoinverse-matrix-and-svd) 89 | 90 | 91 | 92 | 93 | 94 | ### Example of Gauss-Newton, Inverse Kinematic Problem 95 | In the following we solve the inverse kinematic of 3 link planner robot. 96 | 97 | ``` 98 | vector_t q=q_start; 99 | vector_t delta_q(3); 100 | 101 | 102 | double epsilon=1e-3; 103 | 104 | int i=0; 105 | double gamma; 106 | double stepSize=10; 107 | 108 | while( (distanceError(goal,forward_kinematics(q)).squaredNorm()>epsilon) && (i<200) ) 109 | { 110 | Eigen::MatrixXd jacobian=numericalDifferentiationFK(q); 111 | Eigen::MatrixXd j_pinv=pseudoInverse(jacobian); 112 | Eigen::VectorXd delta_p=transformationMatrixToPose(goal)-transformationMatrixToPose(forward_kinematics(q) ); 113 | 114 | delta_q=j_pinv*delta_p; 115 | q=q+delta_q; 116 | i++; 117 | } 118 | ``` 119 | 120 | Full source code [here](src/3_link_planner_robot.cpp) 121 | 122 | 123 | # Quasi-Newton Method 124 | 125 | # Curve Fitting 126 | You have a function https://latex.codecogs.com/svg.latex?y = f(x, \boldsymbol \beta) with https://latex.codecogs.com/svg.latex?n unknown parameters, https://latex.codecogs.com/svg.latex?\boldsymbol \beta=(\beta_1,\beta_2,...\beta_n) and a set of sample data (possibly contaminated with noise) from 127 | that function and you are interested to find the unknown parameters such that the residual (difference between output of your function and sample data) https://latex.codecogs.com/svg.latex?\boldsymbol r=(r_1,r_2,...r_m) 128 | became minimum. We construct a new function, where it is sum square of all residuals: 129 | 130 | https://latex.codecogs.com/svg.latex?{\displaystyle S({\boldsymbol {\beta }})=\sum _{i=1}^{m}r_{i}({\boldsymbol {\beta }})^{2}.} 131 | 132 | 133 | 134 | residuals are: 135 | 136 | 137 | 138 | 139 | https://latex.codecogs.com/svg.latex?{\displaystyle r_{i}({\boldsymbol {\beta }})=y_{i}-f\left(x_{i},{\boldsymbol {\beta }}\right).} 140 | 141 | 142 | 143 | We start with an initial guess for https://latex.codecogs.com/svg.latex?\boldsymbol {\beta }^0 144 | 145 | Then we proceeds by the iterations: 146 | 147 | https://latex.codecogs.com/svg.latex?{\displaystyle {\boldsymbol {\beta }}^{(s+1)}={\boldsymbol {\beta }}^{(s)}-\left(\mathbf {J_{r}} ^{\mathsf {T}}\mathbf {J_{r}} \right)^{-1}\mathbf {J_{r}} ^{\mathsf {T}}\mathbf {r} \left({\boldsymbol {\beta }}^{(s)}\right)} 148 | 149 | 150 | 151 | ## Example of Substrate Concentration Cuver Fitting 152 | 153 | Lets say we have the following dataset: 154 | 155 | 156 | |i |1 |2 |3 |4 |5 |6 |7 157 | |--- |--- |--- |--- |--- |--- |--- |---| 158 | |[S] |0.038 |0.194 |0.425 |0.626 |1.253 |2.500 |3.740 159 | |Rate |0.050 |0.127 |0.094 |0.2122 |0.2729 |0.2665 |0.3317 160 | 161 | And we have the folliwng function to fit the data: 162 | 163 | https://latex.codecogs.com/svg.latex?y=\frac{\beta_1 \times x}{\beta_2+x} 164 | 165 | 166 | So our https://latex.codecogs.com/svg.latex?\mathbf r_{2\times7} is: 167 | 168 | 169 | https://latex.codecogs.com/svg.latex?\left\{\begin{matrix} 170 | r_1=y_1 - \frac{\beta_1 \times x_1}{\beta_2+x_1}\\ 171 | r_2=y_2 - \frac{\beta_1 \times x_2}{\beta_2+x_2}\\ 172 | r_3=y_3 - \frac{\beta_1 \times x_3}{\beta_2+x_3}\\ 173 | r_4=y_4 - \frac{\beta_1 \times x_4}{\beta_2+x_4}\\ 174 | r_5=y_5 - \frac{\beta_1 \times x_5}{\beta_2+x_5}\\ 175 | r_6=y_6 - \frac{\beta_1 \times x_6}{\beta_2+x_6}\\ 176 | r_7=y_7 - \frac{\beta_1 \times x_7}{\beta_2+x_7}\\ 177 | \end{matrix}\right. 178 | 179 | 180 | and the jacobian is https://latex.codecogs.com/svg.latex?\mathbf J_{7\times2}: 181 | 182 | 183 | https://latex.codecogs.com/svg.latex?\left\{\begin{matrix} 184 | \frac{\sigma r_i}{\sigma \beta_1}= \frac{-x_i}{\beta_2+x_i} 185 | \\ 186 | \frac{\sigma r_i}{\sigma \beta_2} = \frac{\beta_1*x_i}{(\beta_2 \times x_i)^2} 187 | \end{matrix}\right.: 188 | 189 | 190 | Now let's implement it with Eigen, First a functor that calculate the 191 | https://latex.codecogs.com/svg.latex?\mathbf r given https://latex.codecogs.com/svg.latex?\boldsymbol {\beta }: 192 | ``` 193 | struct SubstrateConcentrationFunctor : Functor 194 | { 195 | SubstrateConcentrationFunctor(Eigen::MatrixXd points): Functor(points.cols(),points.rows()) 196 | { 197 | this->Points = points; 198 | } 199 | 200 | int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &r) const 201 | { 202 | double x_i,y_i,beta1,beta2; 203 | for(unsigned int i = 0; i < this->Points.rows(); ++i) 204 | { 205 | y_i=this->Points.row(i)(1); 206 | x_i=this->Points.row(i)(0); 207 | beta1=z(0); 208 | beta2=z(1); 209 | r(i) =y_i-(beta1*x_i) /(beta2+x_i); 210 | } 211 | 212 | return 0; 213 | } 214 | Eigen::MatrixXd Points; 215 | 216 | int inputs() const { return 2; } // There are two parameters of the model, beta1, beta2 217 | int values() const { return this->Points.rows(); } // The number of observations 218 | }; 219 | ``` 220 | 221 | Now we have to set teh data: 222 | ``` 223 | //the last column in the matrix should be "y" 224 | Eigen::MatrixXd points(7,2); 225 | 226 | points.row(0)<< 0.038,0.050; 227 | points.row(1)<<0.194,0.127; 228 | points.row(2)<<0.425,0.094; 229 | points.row(3)<<0.626,0.2122; 230 | points.row(4)<<1.253,0.2729; 231 | points.row(5)<<2.500,0.2665; 232 | points.row(6)<<3.740,0.3317; 233 | ``` 234 | Let's create an instance of our functor and compute the derivatves numerically: 235 | ``` 236 | SubstrateConcentrationFunctor functor(points); 237 | Eigen::NumericalDiff numDiff(functor); 238 | ``` 239 | Set the initial values for beta: 240 | ``` 241 | Eigen::VectorXd beta(2); 242 | beta<<0.9,0.2; 243 | ``` 244 | And the main loop: 245 | ``` 246 | //βᵏ⁺¹=βᵏ- (JᵀJ)⁻¹Jᵀr(βᵏ) 247 | for(int i=0;i<10;i++) 248 | { 249 | numDiff.df(beta,J); 250 | std::cout<<"J: \n" << J< 2 | 3 | 4 | 5 | e 6 | 7 | 8 | 9 | 10 | = 11 | 12 | e 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /images/RPY_angles_of_airplanes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/RPY_angles_of_airplanes.png -------------------------------------------------------------------------------- /images/RPY_angles_of_cars.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/RPY_angles_of_cars.png -------------------------------------------------------------------------------- /images/Singular-Value-Decomposition.svg: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | Singular Value Decomposition 9 | 10 | 11 | 12 | This is the File 13 | http://commons.wikimedia.org/wiki/File:Singular-Value-Decomposition.svg 14 | 15 | Singular Value Decomposition of the 2-dimensional Shearing 16 | 17 | M = ( 1 1 ) 18 | ( 0 1 ) 19 | 20 | The Image shows: 21 | 22 | Upper Left: 23 | The unit Disc with the two canonical unit Vectors 24 | 25 | Upper Right: 26 | Unit Disc et al. transformed with M and signular Values 27 | sigma_1 and sigma_2 indicated 28 | 29 | Lower Left: 30 | The Action of V^* on the Unit disc. This is a just 31 | Rotation. 32 | 33 | Lower Right: 34 | The Action of Sigma * V^* on the Unit disc. Sigma scales in 35 | vertically and horizontally. 36 | 37 | The this special Case the singularValues are Phi and 1/Phi where 38 | Phi is the Golden Ratio. V^* is a (counter clockwise) Rotation by 39 | an angle alpha where alpha satisfies tan(alpha) = -Phi. 40 | U is a Rotation by beta with tan(beta) = Phi-1 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | -------------------------------------------------------------------------------- /images/angulare_velocity_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/angulare_velocity_2.jpg -------------------------------------------------------------------------------- /images/exponential_coordinates_for_rigid-body_motions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/exponential_coordinates_for_rigid-body_motions.png -------------------------------------------------------------------------------- /images/exponential_coordinates_for_rigid-body_motions2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/exponential_coordinates_for_rigid-body_motions2.png -------------------------------------------------------------------------------- /images/exponential_coordinates_for_rotation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/exponential_coordinates_for_rotation.png -------------------------------------------------------------------------------- /images/gram_schmidt1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt1.png -------------------------------------------------------------------------------- /images/gram_schmidt2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt2.png -------------------------------------------------------------------------------- /images/gram_schmidt3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt3.png -------------------------------------------------------------------------------- /images/gram_schmidt4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt4.png -------------------------------------------------------------------------------- /images/gram_schmidt5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt5.png -------------------------------------------------------------------------------- /images/gram_schmidt6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt6.png -------------------------------------------------------------------------------- /images/gram_schmidt7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt7.png -------------------------------------------------------------------------------- /images/gram_schmidt8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/gram_schmidt8.png -------------------------------------------------------------------------------- /images/linear_velocity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/linear_velocity.png -------------------------------------------------------------------------------- /images/quaternions.online.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/quaternions.online.png -------------------------------------------------------------------------------- /images/ref0.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /images/row_echelon_form.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /images/screw_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/screw_1.jpg -------------------------------------------------------------------------------- /images/transformtion_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/transformtion_1.jpg -------------------------------------------------------------------------------- /images/transformtion_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/transformtion_2.jpg -------------------------------------------------------------------------------- /images/transformtion_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/images/transformtion_3.jpg -------------------------------------------------------------------------------- /src/angle_axis_rodrigues.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch, 6 | double yaw) { 7 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 8 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 9 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 10 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 11 | Eigen::Matrix3d rotationMatrix = q.matrix(); 12 | return rotationMatrix; 13 | } 14 | 15 | 16 | int main() 17 | { 18 | //////////////////////////////////////// create angle axis (rodrigues) /////////////////////////////////////////// 19 | 20 | 21 | Eigen::Vector3d vector3d(2.3,3.1,1.7); 22 | Eigen::Vector3d vector3dNormalized=vector3d.normalized(); 23 | double theta=M_PI/7; 24 | Eigen::AngleAxisd angleAxisConversion(theta,vector3dNormalized); 25 | 26 | 27 | //////////////////////////////////////// angle axis (rodrigues) to rotation matrix /////////////////////////////////////////// 28 | Eigen::Matrix3d rotationMatrixConversion; 29 | rotationMatrixConversion=angleAxisConversion.toRotationMatrix(); 30 | 31 | 32 | 33 | //////////////////////////////////////// rotation matrix to angle axis (rodrigues) /////////////////////////////////////////// 34 | 35 | 36 | Eigen::Matrix3d rotationMatrix; 37 | 38 | double roll, pitch, yaw; 39 | roll=M_PI/2; 40 | pitch=M_PI/2; 41 | yaw=M_PI/6; 42 | 43 | rotationMatrix= eulerAnglesToRotationMatrix(roll, pitch,yaw); 44 | 45 | Eigen::AngleAxisd rodrigues(rotationMatrix ); 46 | std::cout<<"Rodrigues Angle:\n"< 6 | #include 7 | #include 8 | 9 | template 10 | T MultiVarFunction(const T &x,const T& y) 11 | { 12 | T z=x*cos(y); 13 | return z; 14 | } 15 | 16 | void MultiVarFunctionDerivativesExample() 17 | { 18 | Eigen::AutoDiffScalar x,y,z_derivative; 19 | 20 | x.value()=2 ; 21 | y.value()=M_PI/4; 22 | 23 | /* 24 | Eigen::VectorXd::Unit(2, 0) means a unit vector with first element be 1 25 | 26 | 0 27 | 1 28 | 29 | which means we want compute the derivates relative to the first element 30 | */ 31 | //we telling we want dz/dx 32 | x.derivatives() = Eigen::VectorXd::Unit(2, 0); 33 | 34 | //we telling we want dz/dy 35 | y.derivatives() = Eigen::VectorXd::Unit(2, 1); 36 | 37 | z_derivative = MultiVarFunction(x, y); 38 | 39 | std::cout << "AutoDiff:" << std::endl; 40 | std::cout << "Function output: " << z_derivative.value() << std::endl; 41 | std::cout << "Derivative: \n" << z_derivative.derivatives()<< std::endl; 42 | 43 | } 44 | 45 | template 46 | struct VectorFunction { 47 | 48 | typedef _Scalar Scalar; 49 | 50 | enum 51 | { 52 | InputsAtCompileTime = NX, 53 | ValuesAtCompileTime = NY 54 | }; 55 | 56 | // Also needed by Eigen 57 | typedef Eigen::Matrix InputType; 58 | typedef Eigen::Matrix ValueType; 59 | 60 | // Vector function 61 | template 62 | void operator()(const Eigen::Matrix& x, Eigen::Matrix* y ) const 63 | { 64 | (*y)(0,0) = 10.0*pow(x(0,0)+3.0,2) +pow(x(1,0)-5.0,2) ; 65 | (*y)(1,0) = (x(0,0)+1)*x(1,0); 66 | (*y)(2,0) = sin(x(0,0))*x(1,0); 67 | } 68 | }; 69 | 70 | 71 | 72 | void JacobianDerivativesExample() 73 | { 74 | 75 | Eigen::Matrix x; 76 | Eigen::Matrix y; 77 | Eigen::Matrix fjac; 78 | 79 | Eigen::AutoDiffJacobian< VectorFunction > JacobianDerivatives; 80 | 81 | // Set values in x, y and fjac... 82 | 83 | 84 | x(0,0)=2.0; 85 | x(1,0)=3.0; 86 | 87 | JacobianDerivatives(x, &y, &fjac); 88 | 89 | std::cout << "jacobian of matrix at "<< x(0,0)<<","< 2 | #include 3 | using namespace Eigen; 4 | 5 | 6 | template 7 | Eigen::Matrix 8 | CompleteOrthogonalDecomposition(const Eigen::Matrix &M) { 9 | Eigen::CompleteOrthogonalDecomposition< 10 | Eigen::Matrix> 11 | cod(M); 12 | const Eigen::Matrix Q = 13 | cod.householderQ(); 14 | return Q.leftCols(cod.rank()); 15 | } 16 | 17 | int main() {} 18 | -------------------------------------------------------------------------------- /src/check_matrixsimilarity.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | 6 | void checkMatrixsimilarity() 7 | { 8 | // EXPECT_NEAR should be used element wise 9 | // This could be also used 10 | // ASSERT_TRUE(((translation - expectedTranslation).norm() < precision); 11 | 12 | // Pointwise() matcher could be also used 13 | // EXPECT_THAT(result_array, Pointwise(NearWithPrecision(0.1), expected_array)); 14 | 15 | 16 | Eigen::VectorXd pose; 17 | 18 | Eigen::VectorXd expectedPose(3); 19 | expectedPose<<1.0,0.0,0.0; 20 | //ASSERT_TRUE(pose.isApprox(expectedPose)); 21 | } 22 | 23 | int main() 24 | { 25 | 26 | } 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/compute_basis_of_nullspace.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | using namespace Eigen; 4 | 5 | void computeBasisOfNullSpace() 6 | { 7 | 8 | Eigen::MatrixXd A(3,4); 9 | A<<1 ,1 ,2, 1 , 10 | 3,1,4,4, 11 | 4,-4,0,8; 12 | 13 | 14 | Eigen::FullPivLU lu(A); 15 | Eigen::MatrixXd A_null_space = lu.kernel(); 16 | 17 | std::cout< > cod; 23 | cod.compute(A); 24 | std::cout << "rank : " << cod.rank() << "\n"; 25 | // Find URV^T 26 | MatrixXd V = cod.matrixZ().transpose(); 27 | MatrixXd Null_space = V.block(0, cod.rank(),V.rows(), V.cols() - cod.rank()); 28 | MatrixXd P = cod.colsPermutation(); 29 | Null_space = P * Null_space; // Unpermute the columns 30 | // The Null space: 31 | std::cout << "The null space: \n" << Null_space << "\n" ; 32 | // Check that it is the null-space: 33 | std::cout << "A * Null_space = \n" << A * Null_space << '\n'; 34 | 35 | 36 | 37 | } 38 | 39 | 40 | int main() 41 | { 42 | computeBasisOfNullSpace(); 43 | } 44 | -------------------------------------------------------------------------------- /src/create_transformation_matrices.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/behnamasadi/EigenDemo/e05f0aa2560bd30254500372b566f8b21559f8b4/src/create_transformation_matrices.cpp -------------------------------------------------------------------------------- /src/eigen_functor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | #include 6 | #include 7 | ////////////////////////////C++ Functor//////////////////////////// 8 | /*** print the name of some types... ***/ 9 | 10 | template 11 | std::string name_of_type() 12 | { 13 | return "other"; 14 | } 15 | 16 | template<> 17 | std::string name_of_type() 18 | { 19 | return "int"; 20 | } 21 | 22 | template<> 23 | std::string name_of_type() 24 | { 25 | return "float"; 26 | } 27 | 28 | template<> 29 | std::string name_of_type() 30 | { 31 | return "double"; 32 | } 33 | 34 | template 35 | struct product_functor 36 | { 37 | product_functor(scalar a, scalar b) : m_a(a), m_b(b) 38 | { 39 | std::cout << "Type: " << name_of_type() << ". Computing the product of " << a << " and " << b << "."; 40 | } 41 | // the objective function a*b 42 | scalar f() const 43 | { 44 | return m_a * m_b; 45 | } 46 | 47 | private: 48 | scalar m_a, m_b; 49 | }; 50 | 51 | struct sum_of_ints_functor 52 | { 53 | sum_of_ints_functor(int a, int b) : m_a(a), m_b(b) 54 | { 55 | std::cout << "Type: int. Computing the sum of the two ints " << a << " and " << b << "."; 56 | } 57 | 58 | int f() const 59 | { 60 | return m_a + m_b; 61 | } 62 | 63 | private: 64 | int m_a, m_b; 65 | }; 66 | 67 | template 68 | void call_and_print_return_value(const functor_type& functor_object) 69 | { 70 | std::cout << " The result is: " << functor_object.f() << std::endl; 71 | } 72 | 73 | void functorExample() 74 | { 75 | call_and_print_return_value(sum_of_ints_functor(3,5)); 76 | call_and_print_return_value(product_functor(0.2f,0.4f)); 77 | } 78 | 79 | 80 | int main() 81 | { 82 | functorExample(); 83 | } 84 | 85 | -------------------------------------------------------------------------------- /src/eigen_value_eigen_vector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void eigenValueSolver() { 5 | // ComplexEigenSolver ces; 6 | Eigen::EigenSolver ces; 7 | Eigen::MatrixXd A(4, 4); 8 | A(0, 0) = 18; 9 | A(0, 1) = -9; 10 | A(0, 2) = -27; 11 | A(0, 2) = 24; 12 | A(1, 0) = -9; 13 | A(1, 1) = 4.5; 14 | A(1, 2) = 13.5; 15 | A(1, 3) = -12; 16 | A(2, 0) = -27; 17 | A(2, 1) = 13.5; 18 | A(2, 2) = 40.5; 19 | A(2, 3) = -36; 20 | A(3, 0) = 24; 21 | A(3, 1) = -12; 22 | A(3, 2) = -36; 23 | A(3, 3) = 32; 24 | ces.compute(A); 25 | std::cout << "The eigenvalues of A are:" << std::endl 26 | << ces.eigenvalues() << std::endl; 27 | std::cout << "The matrix of eigenvectors, V, is:" << std::endl 28 | << ces.eigenvectors() << std::endl 29 | << std::endl; 30 | // complex lambda = ces.eigenvalues()[0]; 31 | // cout << "Consider the first eigenvalue, lambda = " << lambda << endl; 32 | // VectorXcf v = ces.eigenvectors().col(0); 33 | // cout << "If v is the corresponding eigenvector, then lambda * v = " << 34 | // endl << lambda * v << endl; 35 | // cout << "... and A * v = " << endl << A * v << endl << endl; 36 | // 37 | // cout << "Finally, V * D * V^(-1) = " << endl 38 | // << ces.eigenvectors() * ces.eigenvalues().asDiagonal() * 39 | // ces.eigenvectors().inverse() << endl; 40 | } 41 | 42 | void selfAdjointEigenSolve() { 43 | 44 | // Real symmetric matrix can guarantee successful diagonalization 45 | 46 | Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Random(); // Random Number Matrix 47 | 48 | Eigen::SelfAdjointEigenSolver eigen_solver( 49 | matrix_33.transpose() * matrix_33); 50 | std::cout << "Eigen values = \n" << eigen_solver.eigenvalues() << std::endl; 51 | std::cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << std::endl; 52 | } 53 | 54 | int main() { selfAdjointEigenSolve(); } 55 | -------------------------------------------------------------------------------- /src/euler_angle.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch, 5 | double yaw) { 6 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 7 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 8 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 9 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 10 | Eigen::Matrix3d rotationMatrix = q.matrix(); 11 | return rotationMatrix; 12 | } 13 | 14 | Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d rotationMatrix) 15 | { 16 | Eigen::Vector3d euler_angles = rotationMatrix.eulerAngles(2, 1, 0); 17 | return euler_angles; 18 | } 19 | 20 | int main() 21 | { 22 | 23 | double roll, pitch, yaw; 24 | 25 | roll = M_PI / 8; 26 | pitch = M_PI / 4; 27 | yaw = M_PI / 9; 28 | 29 | 30 | std::cout << "set angles:" << std::endl; 31 | 32 | std::cout << "roll: Pi/" << M_PI / roll << " ,pitch: Pi/" << M_PI / pitch << " ,yaw : Pi/" << M_PI / yaw 33 | << std::endl; 34 | 35 | Eigen::Matrix3d rotationMatrix = eulerAnglesToRotationMatrix(roll, pitch, yaw); 36 | Eigen::Vector3d euler_angles = rotationMatrixToEulerAngles(rotationMatrix); 37 | 38 | std::cout << "retrieved angles:" << std::endl; 39 | 40 | std::cout << "roll: Pi/" << M_PI / euler_angles[2] << " ,pitch: Pi/" << M_PI / euler_angles[1] << " ,yaw : Pi/" 41 | << M_PI / euler_angles[0] << std::endl; 42 | 43 | } 44 | -------------------------------------------------------------------------------- /src/geometry_transformation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //Transform, Translation, Scaling, Rotation2D and 3D rotations (Quaternion, AngleAxis) 5 | 6 | #if KDL_FOUND==1 7 | #include 8 | #endif 9 | 10 | //The value of KDL_FOUND has been set via target_compile_definitions in CMake 11 | 12 | 13 | 14 | 15 | void transformExample() 16 | { 17 | /* 18 | The class Eigen::Transform represents either 19 | 1) an affine, or 20 | 2) a projective transformation 21 | using homogenous calculus. 22 | 23 | For instance, an affine transformation A is composed of a linear part L and a translation t such that transforming a point p by A is equivalent to: 24 | 25 | p' = L * p + t 26 | 27 | Using homogeneous vectors: 28 | 29 | [p'] = [L t] * [p] = A * [p] 30 | [1 ] [0 1] [1] [1] 31 | 32 | Ref: https://stackoverflow.com/questions/35416880/what-does-transformlinear-return-in-the-eigen-library 33 | 34 | Difference Between Projective and Affine Transformations 35 | 1) The projective transformation does not preserve parallelism, length, and angle. But it still preserves collinearity and incidence. 36 | 2) Since the affine transformation is a special case of the projective transformation, 37 | it has the same properties. However unlike projective transformation, it preserves parallelism. 38 | 39 | Ref: https://www.graphicsmill.com/docs/gm5/Transformations.htm 40 | */ 41 | 42 | float arrVertices [] = { -1.0 , -1.0 , -1.0 , 43 | 1.0 , -1.0 , -1.0 , 44 | 1.0 , 1.0 , -1.0 , 45 | -1.0 , 1.0 , -1.0 , 46 | -1.0 , -1.0 , 1.0 , 47 | 1.0 , -1.0 , 1.0 , 48 | 1.0 , 1.0 , 1.0 , 49 | -1.0 , 1.0 , 1.0}; 50 | Eigen::MatrixXf mVertices = Eigen::Map < Eigen::Matrix > ( arrVertices ) ; 51 | Eigen::Transform t = Eigen::Transform ::Identity(); 52 | t.scale ( 0.8f ) ; 53 | t.rotate ( Eigen::AngleAxisf (0.25f * M_PI , Eigen::Vector3f::UnitX () ) ) ; 54 | t.translate ( Eigen::Vector3f (1.5 , 10.2 , -5.1) ) ; 55 | std::cout << t * mVertices.colwise().homogeneous () << std::endl; 56 | } 57 | 58 | 59 | Eigen::Matrix4f createAffinematrix(float a, float b, float c, Eigen::Vector3f trans) 60 | { 61 | { 62 | Eigen::Transform t; 63 | t = Eigen::Translation(trans); 64 | t.rotate(Eigen::AngleAxis(a, Eigen::Vector3f::UnitX())); 65 | t.rotate(Eigen::AngleAxis(b, Eigen::Vector3f::UnitY())); 66 | t.rotate(Eigen::AngleAxis(c, Eigen::Vector3f::UnitZ())); 67 | return t.matrix(); 68 | } 69 | 70 | 71 | { 72 | /* 73 | The difference between the first implementation and the second is like the difference between "Fix Angle" and "Euler Angle", you can 74 | https://www.youtube.com/watch?v=09xVHo1JudY 75 | */ 76 | Eigen::Transform t; 77 | t = Eigen::AngleAxis(c, Eigen::Vector3f::UnitZ()); 78 | t.prerotate(Eigen::AngleAxis(b, Eigen::Vector3f::UnitY())); 79 | t.prerotate(Eigen::AngleAxis(a, Eigen::Vector3f::UnitX())); 80 | t.pretranslate(trans); 81 | return t.matrix(); 82 | } 83 | } 84 | 85 | 86 | 87 | 88 | int main() 89 | { 90 | 91 | } 92 | -------------------------------------------------------------------------------- /src/glm.cpp: -------------------------------------------------------------------------------- 1 | #include // glm::perspective 2 | #include // glm::translate, glm::rotate, glm::scale 3 | #include // glm::pi 4 | #include // glm::mat4 5 | #include // glm::vec3 6 | #include // glm::vec4 7 | 8 | glm::mat4 camera(float Translate, glm::vec2 const &Rotate) { 9 | glm::mat4 Projection = 10 | glm::perspective(glm::pi() * 0.25f, 4.0f / 3.0f, 0.1f, 100.f); 11 | glm::mat4 View = 12 | glm::translate(glm::mat4(1.0f), glm::vec3(0.0f, 0.0f, -Translate)); 13 | View = glm::rotate(View, Rotate.y, glm::vec3(-1.0f, 0.0f, 0.0f)); 14 | View = glm::rotate(View, Rotate.x, glm::vec3(0.0f, 1.0f, 0.0f)); 15 | glm::mat4 Model = glm::scale(glm::mat4(1.0f), glm::vec3(0.5f)); 16 | return Projection * View * Model; 17 | } 18 | 19 | int main() { 20 | /* 21 | eigen to glm 22 | EigenToGlmMat 23 | https://stackoverflow.com/questions/63429179/eigen-and-glm-products-produce-different-results 24 | 25 | https://gist.github.com/podgorskiy/04a3cb36a27159e296599183215a71b0 26 | 27 | 28 | Quaternion to Matrix using glm 29 | https://stackoverflow.com/questions/38145042/quaternion-to-matrix-using-glm 30 | 31 | 32 | glm::toQuat( 33 | glm::to_string(GLR).c_str() 34 | glm::toMat3 35 | 36 | 37 | */ 38 | 39 | 40 | 41 | float Translate=1.0; 42 | glm::vec2 Rotate; 43 | 44 | glm::mat4 v=camera( Translate, Rotate); 45 | } 46 | -------------------------------------------------------------------------------- /src/gram_schmidt_orthogonalization.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void gramSchmidtOrthogonalization(Eigen::MatrixXd &matrix,Eigen::MatrixXd &orthonormalMatrix) 5 | { 6 | /* 7 | In this method you make every column perpendicular to it's previous columns, 8 | here if a and b are representation vector of two columns, c=b-((b.a)/|a|).a 9 | ^ 10 | / 11 | b / 12 | / 13 | / 14 | ----------> 15 | a 16 | ^ 17 | /| 18 | b / | 19 | / | c 20 | / | 21 | ----------> 22 | a 23 | you just have to normilze every vector after make it perpendicular to previous columns 24 | so: 25 | q1=a.normalized(); 26 | q2=b-(b.q1).q1 27 | q2=q2.normalized(); 28 | q3=c-(c.q1).q1 - (c.q2).q2 29 | q3=q3.normalized(); 30 | Now we have Q, but we want A=QR so we just multiply both side by Q.transpose(), since Q is orthonormal, Q*Q.transpose() is I 31 | A=QR; 32 | Q.transpose()*A=R; 33 | */ 34 | Eigen::VectorXd col; 35 | for(int i=0;i qr(A); 91 | q = qr.householderQ(); 92 | thinQ.setIdentity(); 93 | thinQ = qr.householderQ() * thinQ; 94 | std::cout << "Q computed by Eigen" << "\n\n" << thinQ << "\n\n"; 95 | std::cout << q << "\n\n" << thinQ << "\n\n"; 96 | 97 | 98 | } 99 | 100 | void gramSchmidtOrthogonalizationExample() 101 | { 102 | Eigen::MatrixXd matrix(3,4),orthonormalMatrix(3,4) ; 103 | matrix=Eigen::MatrixXd::Random(3,4);////A.setRandom(); 104 | 105 | 106 | gramSchmidtOrthogonalization(matrix,orthonormalMatrix); 107 | } 108 | 109 | 110 | 111 | int main() 112 | { 113 | 114 | } 115 | -------------------------------------------------------------------------------- /src/levenberg_marquardt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // Generic functor 7 | template 8 | struct Functor 9 | { 10 | // Information that tells the caller the numeric type (eg. double) and size (input / output dim) 11 | typedef _Scalar Scalar; 12 | enum { 13 | InputsAtCompileTime = NX, 14 | ValuesAtCompileTime = NY 15 | }; 16 | // Tell the caller the matrix sizes associated with the input, output, and jacobian 17 | typedef Eigen::Matrix InputType; 18 | typedef Eigen::Matrix ValueType; 19 | typedef Eigen::Matrix JacobianType; 20 | 21 | // Local copy of the number of inputs 22 | int m_inputs, m_values; 23 | 24 | // Two constructors: 25 | Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 26 | Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 27 | 28 | // Get methods for users to determine function input and output dimensions 29 | int inputs() const { return m_inputs; } 30 | int values() const { return m_values; } 31 | 32 | }; 33 | 34 | ///////////////////////// Levenberg Marquardt Example (1) f(x) = a x + b /////////////////////////////// 35 | 36 | typedef std::vector > Point2DVector; 37 | 38 | Point2DVector GeneratePoints(); 39 | 40 | struct SimpleLineFunctor : Functor 41 | { 42 | int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const 43 | { 44 | // "a" in the model is z(0), and "b" is z(1) 45 | double x_i,y_i,a,b; 46 | for(unsigned int i = 0; i < this->Points.size(); ++i) 47 | { 48 | y_i=this->Points[i](1); 49 | x_i=this->Points[i](0); 50 | a=z(0); 51 | b=z(1); 52 | fvec(i) =y_i-(a*x_i +b); 53 | } 54 | 55 | return 0; 56 | } 57 | 58 | Point2DVector Points; 59 | 60 | int inputs() const { return 2; } // There are two parameters of the model 61 | int values() const { return this->Points.size(); } // The number of observations 62 | }; 63 | 64 | struct SimpleLineFunctorNumericalDiff : Eigen::NumericalDiff {}; 65 | 66 | Point2DVector GeneratePoints(const unsigned int numberOfPoints) 67 | { 68 | Point2DVector points; 69 | // Model y = 2*x + 5 with some noise (meaning that the resulting minimization should be about (2,5) 70 | for(unsigned int i = 0; i < numberOfPoints; ++i) 71 | { 72 | double x = static_cast(i); 73 | Eigen::Vector2d point; 74 | point(0) = x; 75 | point(1) = 2.0 * x + 5.0 + drand48()/10.0; 76 | points.push_back(point); 77 | } 78 | 79 | return points; 80 | } 81 | 82 | void testSimpleLineFunctor() 83 | { 84 | std::cout << "Testing f(x) = a x + b function..." << std::endl; 85 | 86 | unsigned int numberOfPoints = 50; 87 | Point2DVector points = GeneratePoints(numberOfPoints); 88 | 89 | Eigen::VectorXd x(2); 90 | x.fill(2.0f); 91 | 92 | SimpleLineFunctorNumericalDiff functor; 93 | functor.Points = points; 94 | Eigen::LevenbergMarquardt lm(functor); 95 | 96 | Eigen::LevenbergMarquardtSpace::Status status = lm.minimize(x); 97 | std::cout << "status: " << status << std::endl; 98 | std::cout << "x that minimizes the function: " << std::endl << x << std::endl; 99 | std::cout <<"lm.parameters.epsfcn: " < &x_values, std::vector &y_values,double a=-1, double b=0.6, double c=2, unsigned int numberOfPoints=50 ) 187 | { 188 | //point are x_start=-4, x_end=3 189 | double x_start=-4; 190 | double x_end=3; 191 | double x,y; 192 | 193 | for(unsigned int i = 0; i < numberOfPoints; ++i) 194 | { 195 | x = x_start+ static_cast(i)* ( x_end- x_start)/numberOfPoints; 196 | y = a*pow(x,2)+b*x+c + drand48()/10.0; 197 | x_values.push_back(x); 198 | y_values.push_back(y); 199 | } 200 | 201 | } 202 | 203 | void testQuadraticFunctor() 204 | { 205 | std::cout << "Testing the f(x) = ax² + bx + c function..." << std::endl; 206 | std::vector x_values; 207 | std::vector y_values; 208 | 209 | double a=-1; 210 | double b=0.6; 211 | double c=2; 212 | 213 | quadraticPointsGenerator(x_values, y_values,a,b,c); 214 | 215 | // 'm' is the number of data points. 216 | int m = x_values.size(); 217 | 218 | // Move the data into an Eigen Matrix. 219 | // The first column has the input values, x. The second column is the f(x) values. 220 | Eigen::MatrixXd measuredValues(m, 2); 221 | for (int i = 0; i < m; i++) { 222 | measuredValues(i, 0) = x_values[i]; 223 | measuredValues(i, 1) = y_values[i]; 224 | } 225 | 226 | // 'n' is the number of parameters in the function. 227 | // f(x) = a(x^2) + b(x) + c has 3 parameters: a, b, c 228 | int n = 3; 229 | 230 | // 'parameters' is vector of length 'n' containing the initial values for the parameters. 231 | // The parameters 'x' are also referred to as the 'inputs' in the context of LM optimization. 232 | // The LM optimization inputs should not be confused with the x input values. 233 | Eigen::VectorXd parameters(n); 234 | parameters(0) = 0.0; // initial value for 'a' 235 | parameters(1) = 0.0; // initial value for 'b' 236 | parameters(2) = 0.0; // initial value for 'c' 237 | 238 | // 239 | // Run the LM optimization 240 | // Create a LevenbergMarquardt object and pass it the functor. 241 | // 242 | 243 | QuadraticFunctor functor; 244 | functor.measuredValues = measuredValues; 245 | functor.m = m; 246 | functor.n = n; 247 | 248 | Eigen::LevenbergMarquardt lm(functor); 249 | int status = lm.minimize(parameters); 250 | std::cout << "LM optimization status: " << status << std::endl; 251 | 252 | // 253 | // Results 254 | // The 'x' vector also contains the results of the optimization. 255 | // 256 | std::cout << "Optimization results" << std::endl; 257 | std::cout << "\ta: " << parameters(0) << std::endl; 258 | std::cout << "\tb: " << parameters(1) << std::endl; 259 | std::cout << "\tc: " << parameters(2) << std::endl; 260 | 261 | std::cout << "Actual values are" << std::endl; 262 | std::cout << "\ta: " << a << std::endl; 263 | std::cout << "\tb: " << b << std::endl; 264 | std::cout << "\tc: " << c << std::endl; 265 | std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl; 266 | 267 | } 268 | 269 | 270 | ///////////////////////// Levenberg Marquardt Example (3) f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 /////////////////////////////// 271 | 272 | 273 | // https://en.wikipedia.org/wiki/Test_functions_for_optimization 274 | // Booth Function 275 | // Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 276 | struct BoothFunctor : Functor 277 | { 278 | // Simple constructor 279 | BoothFunctor(): Functor(2,2) {} 280 | 281 | // Implementation of the objective function 282 | int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const 283 | { 284 | double x = z(0); double y = z(1); 285 | /* 286 | * Evaluate the Booth function. 287 | * Important: LevenbergMarquardt is designed to work with objective functions that are a sum 288 | * of squared terms. The algorithm takes this into account: do not do it yourself. 289 | * In other words: objFun = sum(fvec(i)^2) 290 | */ 291 | fvec(0) = x + 2*y - 7; 292 | fvec(1) = 2*x + y - 5; 293 | return 0; 294 | } 295 | }; 296 | 297 | void testBoothFunctor() { 298 | std::cout << "Testing the f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2 function..." << std::endl; 299 | Eigen::VectorXd zInit(2); zInit << 1.87, 2.032; 300 | std::cout << "zInit: " << zInit.transpose() << std::endl; 301 | Eigen::VectorXd zSoln(2); zSoln << 1.0, 3.0; 302 | std::cout << "zSoln: " << zSoln.transpose() << std::endl; 303 | 304 | BoothFunctor functor; 305 | Eigen::NumericalDiff numDiff(functor); 306 | Eigen::LevenbergMarquardt,double> lm(numDiff); 307 | lm.parameters.maxfev = 1000; 308 | lm.parameters.xtol = 1.0e-10; 309 | std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl; 310 | std::cout << "x tol: " << lm.parameters.xtol << std::endl; 311 | 312 | Eigen::VectorXd z = zInit; 313 | int ret = lm.minimize(z); 314 | std::cout << "iter count: " << lm.iter << std::endl; 315 | std::cout << "return status: " << ret << std::endl; 316 | std::cout << "zSolver: " << z.transpose() << std::endl; 317 | std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl; 318 | } 319 | 320 | 321 | 322 | ///////////////////////// Levenberg Marquardt Example (4) f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 /////////////////////////////// 323 | 324 | 325 | // https://en.wikipedia.org/wiki/Test_functions_for_optimization 326 | // Himmelblau's Function 327 | // Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 328 | struct HimmelblauFunctor : Functor 329 | { 330 | // Simple constructor 331 | HimmelblauFunctor(): Functor(2,2) {} 332 | 333 | // Implementation of the objective function 334 | int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const 335 | { 336 | double x = z(0); double y = z(1); 337 | /* 338 | * Evaluate Himmelblau's function. 339 | * Important: LevenbergMarquardt is designed to work with objective functions that are a sum 340 | * of squared terms. The algorithm takes this into account: do not do it yourself. 341 | * In other words: objFun = sum(fvec(i)^2) 342 | */ 343 | fvec(0) = x * x + y - 11; 344 | fvec(1) = x + y * y - 7; 345 | return 0; 346 | } 347 | }; 348 | 349 | void testHimmelblauFunctor() 350 | { 351 | std::cout << "Testing f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2 function..." << std::endl; 352 | // Eigen::VectorXd zInit(2); zInit << 0.0, 0.0; // soln 1 353 | // Eigen::VectorXd zInit(2); zInit << -1, 1; // soln 2 354 | // Eigen::VectorXd zInit(2); zInit << -1, -1; // soln 3 355 | Eigen::VectorXd zInit(2); zInit << 1, -1; // soln 4 356 | std::cout << "zInit: " << zInit.transpose() << std::endl; 357 | std::cout << "soln 1: [3.0, 2.0]" << std::endl; 358 | std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl; 359 | std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl; 360 | std::cout << "soln 4: [3.584428, -1.848126]" << std::endl; 361 | 362 | HimmelblauFunctor functor; 363 | Eigen::NumericalDiff numDiff(functor); 364 | Eigen::LevenbergMarquardt,double> lm(numDiff); 365 | lm.parameters.maxfev = 1000; 366 | lm.parameters.xtol = 1.0e-10; 367 | std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl; 368 | std::cout << "x tol: " << lm.parameters.xtol << std::endl; 369 | 370 | Eigen::VectorXd z = zInit; 371 | int ret = lm.minimize(z); 372 | std::cout << "iter count: " << lm.iter << std::endl; 373 | std::cout << "return status: " << ret << std::endl; 374 | std::cout << "zSolver: " << z.transpose() << std::endl; 375 | std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl; 376 | } 377 | 378 | 379 | int main() 380 | { 381 | testSimpleLineFunctor(); 382 | testQuadraticFunctor(); 383 | testBoothFunctor(); 384 | testHimmelblauFunctor(); 385 | } 386 | 387 | 388 | /* 389 | 390 | Refs 391 | https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/CurveFitting.cpp 392 | https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation 393 | https://stackoverflow.com/questions/48213584/understanding-levenberg-marquardt-enumeration-returns 394 | https://www.ultimatepp.org/reference$Eigen_demo$en-us.html 395 | https://ethz-adrl.github.io/ct/ct_doc/doc/html/core_tut_linearization.html 396 | https://robotics.stackexchange.com/questions/20673/why-with-the-pseudo-inverse-it-is-possible-to-invert-the-jacobian-matrix-even-in 397 | http://users.ics.forth.gr/~lourakis/ 398 | https://mathoverflow.net/questions/257699/gauss-newton-vs-gradient-descent-vs-levenberg-marquadt-for-least-squared-method 399 | https://math.stackexchange.com/questions/1085436/gauss-newton-versus-gradient-descent 400 | https://stackoverflow.com/questions/34701160/how-to-set-levenberg-marquardt-damping-using-eigen 401 | http://www.netlib.org/minpack/lmder.f 402 | https://en.wikipedia.org/wiki/Test_functions_for_optimization 403 | https://github.com/daviddoria/Examples/blob/master/c%2B%2B/Eigen/LevenbergMarquardt/NumericalDerivative.cpp 404 | https://stackoverflow.com/questions/18509228/how-to-use-the-eigen-unsupported-levenberg-marquardt-implementation 405 | */ 406 | 407 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | double exp(double x) // the functor we want to apply 7 | { 8 | std::setprecision(5); 9 | return std::trunc(x); 10 | } 11 | 12 | void MatrixPowersPolynomials() { 13 | 14 | // Rectangular Diagonal 15 | // rectangular diagonal matrix is an n × d matrix in which each entry (i, j) 16 | // has a non-zero value if and only if i = j. diagonal matrix is a matrix in 17 | // which the entries outside the main diagonal are all zero; 18 | 19 | // A block diagonal matrix contains square blocks B 1…B r of (possibly) 20 | // nonzero entries along the diagonal. All other entries are zero. Although 21 | // each block is square, they need not be of the same size. 22 | 23 | // Upper and Lower Triangular Matrix) A square matrix is an upper triangular 24 | // matrix if all entries (i, j) below its main diagonal (i.e., satisfying 25 | // i > j) are zeros. 26 | 27 | // The product of uppertriangular matrices is upper triangular. 28 | // c(i,j)=0 if i>j c(i,j)=sum(a(i,k)*b(k,j)) 29 | // i>j 30 | // if i>k a(i,k)=0 31 | // if i j b(k,j)=0 32 | 33 | // Inverse of Triangular Matrix Is Triangula 34 | 35 | // Strictly Triangular Matrix) A matrix is said to be strictlytriangular if it 36 | // is triangularandall its diagonal elements are zeros. 37 | 38 | // The zeroth power of a matrix is defined to be the identity matrix 39 | // When a matrix satisfies A^k = 0 for some integer k, it is referred to as 40 | // nilpotent. all strictly triangular matrices (triangular with zero main 41 | // diagonal)of size d × d satisfy A^d = 0. product of two upper triangular is 42 | // a triangular matrix 43 | 44 | // polynomial function f(A) of a square matrix in much the same way as one 45 | // computes polynomials of scalars. f(x) = 3x^2 + 5x + 2 -> 46 | // f(A) = 3A^2 + 5A + 2 Two polynomials f(A) and g(A) of the same matrix A 47 | // will always commute f(A)g(A)=g(A)f(A) 48 | 49 | // Commutativity of Multiplication with Inverse if AB=I then BA=I 50 | // When the inverse of a matrix exists, it is always unique 51 | // Inverse of Triangular Matrix Is Triangular 52 | // Inv(A^n)=Inv(A)^n 53 | 54 | // An orthogonal matrix is a square matrix whose inverse is its transpose: 55 | // A*A^T=A^T*A=I all column columns/ rows are perpendicular 56 | 57 | // The multiplication of an n × d matrix A with a d-dimensional column 58 | // vector to create an n-dimensional column vector is often interpreted as 59 | // a linear transformation from d-dimensional space to n-dimensional space 60 | /* 61 | a11 a12 a11 a12 62 | a21 a22 x1 =x1 a21 + x2 a22 63 | a31 a32 x2 a31 a32 64 | 65 | Therefore, the n × d matrix A is occasionally represented in terms of its 66 | ordered set of ndimensional columns A nxd= [a1 a2 ... ad] 67 | 68 | low-rank update 69 | Linear regression least-squares classification, support-vector machines, and 70 | logistic regression Matrix factorization is an alternative term for matrix 71 | decomposition recommender systems 72 | 73 | Regression 74 | the only difference from classification is that the array contains numerical 75 | values (rather than categorical ones) The dependent variable is also referred 76 | to as a response variable, target variable, or regressand in the case of 77 | regression The independent variables are also referred to as regressors.more 78 | than two classes like {Red, Green, Blue} cannot be ordered, and are therefore 79 | different from regression 80 | 81 | 82 | convex objective functions like linear regression 83 | 84 | https://medium.com/@wisnutandaseru/proving-eulers-identity-using-taylor-series-2771089cd780 85 | Householder Reflections 86 | Directional derivative 87 | chain rule for multivariale derivative 88 | multivariant fourier transform 89 | 90 | 91 | 92 | inverse of I+A? 93 | https://math.stackexchange.com/questions/298616/what-is-inverse-of-ia/298623 94 | https://en.wikipedia.org/wiki/Woodbury_matrix_identity 95 | 96 | 97 | https://en.m.wikipedia.org/wiki/Computational_complexity_of_mathematical_operations#Matrix_algebra 98 | */ 99 | } 100 | 101 | int main(int argc, char *argv[]) { 102 | 103 | /* 104 | Machine Epsilon is the machine-dependent difference between 1.0 and the 105 | smallest representable value that is greater than 1.0. 106 | 107 | */ 108 | 109 | std::cout << std::fixed << std::setprecision(60); 110 | std::cout << std::numeric_limits::epsilon() << std::endl; 111 | std::cout << 1.0 - std::numeric_limits::epsilon() << std::endl; 112 | std::cout << 2.0 - std::numeric_limits::epsilon() << std::endl; 113 | std::cout << 4.0 - 2*std::numeric_limits::epsilon() << std::endl; 114 | std::cout << 8.0 - 2*std::numeric_limits::epsilon() << std::endl; 115 | std::cout << 16.0 - std::numeric_limits::epsilon() << std::endl; 116 | std::cout << 32.0 - std::numeric_limits::epsilon() << std::endl; 117 | std::cout << 64.0 - std::numeric_limits::epsilon() << std::endl; 118 | 119 | double from3 = std::nextafter(0.1, 0), to3 = 0.1; 120 | std::cout << "The number 0.1 lies between two valid doubles:\n" 121 | << std::setprecision(56) << " " << from3 122 | << std::hexfloat << " (" << from3 << ')' << std::defaultfloat 123 | << "\nand " << to3 << std::hexfloat << " (" << to3 << ")\n" 124 | << std::defaultfloat << std::setprecision(20); 125 | 126 | 127 | 128 | return 0; 129 | } 130 | -------------------------------------------------------------------------------- /src/matrix_array_vector.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void matrixCreation() { 5 | Eigen::Matrix4d m; // 4x4 double 6 | 7 | Eigen::Matrix4cd objMatrix4cd; // 4x4 double complex 8 | 9 | // a is a 3x3 matrix, with a static float[9] array of uninitialized 10 | // coefficients, 11 | Eigen::Matrix3f a; 12 | 13 | // b is a dynamic-size matrix whose size is currently 0x0, and whose array of 14 | // coefficients hasn't yet been allocated at all. 15 | Eigen::MatrixXf b; 16 | 17 | // A is a 10x15 dynamic-size matrix, with allocated but currently 18 | // uninitialized coefficients. 19 | Eigen::MatrixXf A(10, 15); 20 | } 21 | 22 | void arrayCreation() { 23 | // ArrayXf 24 | Eigen::Array a1; 25 | // Array3f 26 | Eigen::Array a2; 27 | // ArrayXXd 28 | Eigen::Array a3; 29 | // Array33d 30 | Eigen::Array a4; 31 | Eigen::Matrix3d matrix_from_array = a4.matrix(); 32 | } 33 | 34 | void vectorCreation() { 35 | // Vector3f is a fixed column vector of 3 floats: 36 | Eigen::Vector3f objVector3f; 37 | 38 | // RowVector2i is a fixed row vector of 3 integer: 39 | Eigen::RowVector2i objRowVector2i; 40 | 41 | // VectorXf is a column vector of size 10 floats: 42 | Eigen::VectorXf objv(10); 43 | 44 | // V is a dynamic-size vector of size 30, with allocated but currently 45 | // uninitialized coefficients. 46 | Eigen::VectorXf V(30); 47 | } 48 | 49 | void buildMatrixFromVector() { 50 | Eigen::Matrix2d mat; 51 | mat << 1, 2, 3, 4; 52 | 53 | std::cout << "matrix is:\n" << mat << std::endl; 54 | 55 | Eigen::RowVector2d firstRow = mat.row(0); 56 | Eigen::Vector2d firstCol = mat.col(0); 57 | 58 | std::cout << "First column of the matrix is:\n " << firstCol << std::endl; 59 | std::cout << "First column dims are: " << firstCol.rows() << "," 60 | << firstCol.cols() << std::endl; 61 | 62 | std::cout << "First row of the matrix is: \n" << firstRow << std::endl; 63 | std::cout << "First row dims are: " << firstRow.rows() << "," 64 | << firstRow.cols() << std::endl; 65 | 66 | firstRow = Eigen::RowVector2d::Random(); 67 | firstCol = Eigen::Vector2d::Random(); 68 | 69 | mat.row(0) = firstRow; 70 | mat.col(0) = firstCol; 71 | 72 | std::cout << "the new matrix is:\n" << mat << std::endl; 73 | } 74 | 75 | void initialization() { 76 | std::cout << "///////////////////Initialization//////////////////" 77 | << std::endl; 78 | 79 | Eigen::Matrix2d rndMatrix; 80 | rndMatrix.setRandom(); 81 | 82 | Eigen::Matrix2d constantMatrix; 83 | constantMatrix.setRandom(); 84 | constantMatrix.setConstant(4.3); 85 | 86 | Eigen::MatrixXd identity = Eigen::MatrixXd::Identity(6, 6); 87 | 88 | Eigen::MatrixXd zeros = Eigen::MatrixXd::Zero(3, 3); 89 | 90 | Eigen::ArrayXXf table(10, 4); 91 | table.col(0) = Eigen::ArrayXf::LinSpaced(10, 0, 90); 92 | } 93 | 94 | void elementAccess() { 95 | std::cout << "//////////////////Elements Access////////////////////" 96 | << std::endl; 97 | 98 | Eigen::MatrixXf matrix(4, 4); 99 | matrix << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16; 100 | 101 | std::cout << "matrix is:\n" << matrix << std::endl; 102 | 103 | std::cout << "All Eigen matrices default to column-major storage order. That " 104 | "means, matrix(2) is matix(2,0):" 105 | << std::endl; 106 | 107 | std::cout << "matrix(2): " << matrix(2) << std::endl; 108 | std::cout << "matrix(2,0): " << matrix(2, 0) << std::endl; 109 | 110 | std::cout << "//////////////////Pointer to data ////////////////////" 111 | << std::endl; 112 | 113 | for (int i = 0; i < matrix.size(); i++) { 114 | std::cout << *(matrix.data() + i) << " "; 115 | } 116 | std::cout << std::endl; 117 | 118 | std::cout << "//////////////////Row major Matrix////////////////////" 119 | << std::endl; 120 | Eigen::Matrix matrixRowMajor(4, 4); 121 | matrixRowMajor << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16; 122 | 123 | for (int i = 0; i < matrixRowMajor.size(); i++) { 124 | std::cout << *(matrixRowMajor.data() + i) << " "; 125 | } 126 | std::cout << std::endl; 127 | 128 | std::cout << "//////////////////Block Elements Access////////////////////" 129 | << std::endl; 130 | 131 | std::cout << "Block elements in the middle" << std::endl; 132 | 133 | int starting_row, starting_column, number_rows_in_block, number_cols_in_block; 134 | 135 | starting_row = 1; 136 | starting_column = 1; 137 | number_rows_in_block = 2; 138 | number_cols_in_block = 2; 139 | 140 | std::cout << matrix.block(starting_row, starting_column, number_rows_in_block, 141 | number_cols_in_block) 142 | << std::endl; 143 | 144 | for (int i = 1; i <= 3; ++i) { 145 | std::cout << "Block of size " << i << "x" << i << std::endl; 146 | std::cout << matrix.block(0, 0, i, i) << std::endl; 147 | } 148 | } 149 | 150 | void matrixReshaping() { 151 | // https://eigen.tuxfamily.org/dox/group__TutorialReshapeSlicing.html 152 | /* 153 | Eigen::MatrixXd m1(12,1); 154 | m1<<0,1,2,3,4,5,6,7,8,9,10,11; 155 | std::cout< m2(m1); 158 | //Eigen::Map m3(m2.data(),3,4); 159 | Eigen::Map m2(m1.data(),4,3); 160 | std::cout< dynamicMatrix; 174 | 175 | dynamicMatrix.resize(rows, cols); 176 | dynamicMatrix = Eigen::MatrixXd::Random(rows, cols); 177 | 178 | std::cout << "Matrix size is: " << dynamicMatrix.size() << std::endl; 179 | 180 | std::cout << "Matrix is:\n" << dynamicMatrix << std::endl; 181 | 182 | dynamicMatrix.resize(2, 6); 183 | std::cout << "New Matrix size is: " << dynamicMatrix.size() << std::endl; 184 | std::cout << "Matrix is:\n" << dynamicMatrix << std::endl; 185 | 186 | std::cout << "//////////////////Matrix conservativeResize////////////////////" 187 | << std::endl; 188 | 189 | dynamicMatrix.conservativeResize(dynamicMatrix.rows(), 190 | dynamicMatrix.cols() + 1); 191 | dynamicMatrix.col(dynamicMatrix.cols() - 1) = Eigen::Vector2d(1, 4); 192 | 193 | std::cout << dynamicMatrix << std::endl; 194 | } 195 | 196 | void convertingMatrixtoArray() { 197 | Eigen::Matrix mat1 = Eigen::MatrixXd::Random(4, 4); 198 | Eigen::Matrix mat2 = Eigen::MatrixXd::Random(4, 4); 199 | 200 | Eigen::Array array1 = mat1.array(); 201 | Eigen::Array array2 = mat2.array(); 202 | 203 | std::cout << "Matrix multipication:\n" << mat1 * mat2 << std::endl; 204 | std::cout << "Array multipication(coefficientsweise) :\n" 205 | << array1 * array2 << std::endl; 206 | std::cout << "Matrix coefficientsweise multipication :\n" 207 | << mat1.cwiseProduct(mat2) << std::endl; 208 | } 209 | 210 | void coefficientWiseOperations() { 211 | std::cout 212 | << "/////////////Matrix Coefficient Wise Operations///////////////////" 213 | << std::endl; 214 | Eigen::Matrix my_matrix; 215 | my_matrix << 1, 2, 3, 4, 5, 6; 216 | int i, j; 217 | std::cout << "The matrix is: \n" << my_matrix << std::endl; 218 | 219 | std::cout << "The matrix transpose is: \n" 220 | << my_matrix.transpose() << std::endl; 221 | 222 | std::cout << "The minimum element is: " << my_matrix.minCoeff(&i, &j) 223 | << " and its indices are:" << i << "," << j << std::endl; 224 | 225 | std::cout << "The maximum element is: " << my_matrix.maxCoeff(&i, &j) 226 | << " and its indices are:" << i << "," << j << std::endl; 227 | 228 | std::cout << "The multipication of all elements: " << my_matrix.prod() 229 | << std::endl; 230 | std::cout << "The sum of all elements: " << my_matrix.sum() << std::endl; 231 | std::cout << "The mean of all element: " << my_matrix.mean() << std::endl; 232 | std::cout << "The trace of the matrix is: " << my_matrix.trace() << std::endl; 233 | std::cout << "The means of columns: " << my_matrix.colwise().mean() 234 | << std::endl; 235 | std::cout << "The max of each columns: " << my_matrix.rowwise().maxCoeff() 236 | << std::endl; 237 | std::cout << "Norm 2 of the matrix is: " << my_matrix.lpNorm<2>() 238 | << std::endl; 239 | std::cout << "Norm infinty of the matrix is: " 240 | << my_matrix.lpNorm() << std::endl; 241 | std::cout << "If all elemnts are positive: " << (my_matrix.array() > 0).all() 242 | << std::endl; 243 | std::cout << "If any element is greater than 2: " 244 | << (my_matrix.array() > 2).any() << std::endl; 245 | std::cout << "Counting the number of elements greater than 1" 246 | << (my_matrix.array() > 1).count() << std::endl; 247 | std::cout << "subtracting 2 from all elements:\n" 248 | << my_matrix.array() - 2 << std::endl; 249 | std::cout << "abs of the matrix: \n" << my_matrix.array().abs() << std::endl; 250 | std::cout << "square of the matrix: \n" 251 | << my_matrix.array().square() << std::endl; 252 | 253 | std::cout << "exp of the matrix: \n" << my_matrix.array().exp() << std::endl; 254 | std::cout << "log of the matrix: \n" << my_matrix.array().log() << std::endl; 255 | std::cout << "square root of the matrix: \n" 256 | << my_matrix.array().sqrt() << std::endl; 257 | } 258 | 259 | void maskingArray() { 260 | std::cout << "//////////////////Maskin Matrix/Array ////////////////////" 261 | << std::endl; 262 | 263 | // Eigen::MatrixXf P, Q, R; // 3x3 float matrix. 264 | // (R.array() < s).select(P,Q ); // (R < s ? P : Q) 265 | // R = (Q.array()==0).select(P,R); // R(Q==0) = P(Q==0) 266 | int cols, rows; 267 | cols = 2; 268 | rows = 3; 269 | Eigen::MatrixXf R = Eigen::MatrixXf::Random(rows, cols); 270 | 271 | Eigen::MatrixXf Q = Eigen::MatrixXf::Zero(rows, cols); 272 | Eigen::MatrixXf P = Eigen::MatrixXf::Constant(rows, cols, 1.0); 273 | 274 | double s = 0.5; 275 | Eigen::MatrixXf masked = (R.array() < s).select(P, Q); // (R < s ? P : Q) 276 | 277 | std::cout << "R\n" << R << std::endl; 278 | std::cout << "masked\n" << masked << std::endl; 279 | std::cout << "P\n" << P << std::endl; 280 | std::cout << "Q\n" << Q << std::endl; 281 | } 282 | 283 | void AdditionSubtractionOfMatrices() {} 284 | 285 | void transpositionConjugation() {} 286 | 287 | void scalarMultiplicationDivision() {} 288 | 289 | void multiplicationDotCrossProduct() {} 290 | 291 | void casingMatrices() { 292 | 293 | Eigen::Matrix matrix_23; 294 | // Eigen::Matrix vd_3d; 295 | Eigen::Vector3d v_3d; 296 | 297 | // wrong Eigen::Matrix result_wrong_type = matrix_23 ∗ v_3d; 298 | 299 | Eigen::Matrix result = matrix_23.cast() * v_3d; 300 | } 301 | int main() { 302 | // buildMatrixFromVector(); 303 | // vectorCreation(); 304 | // elementAccess(); 305 | // matrixResizing(); 306 | // coefficientWiseOperations(); 307 | // convertingMatrixtoArray(); 308 | // maskingArray(); 309 | } 310 | -------------------------------------------------------------------------------- /src/matrix_broadcasting.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void broadcastingExample() 6 | { 7 | //https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html 8 | } 9 | 10 | 11 | 12 | int main() 13 | { 14 | 15 | } 16 | -------------------------------------------------------------------------------- /src/matrix_condition_numerical_stability.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | void matrixCondition() 5 | { 6 | //https://www.youtube.com/watch?v=GDYmWtWWtNQ 7 | //https://stackoverflow.com/questions/33575478/how-can-you-find-the-condition-number-in-eigen/33577450 8 | //https://math.stackexchange.com/questions/2817630/why-is-the-condition-number-of-a-matrix-given-by-these-eigenvalues 9 | 10 | } 11 | 12 | int main() 13 | { 14 | 15 | } 16 | -------------------------------------------------------------------------------- /src/matrix_decomposition.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void choleskyDecompositionExample() 6 | { 7 | /* 8 | Positive-definite matrix: 9 | Matrix Mnxn is said to be positive definite if the scalar zTMz is strictly positive for every non-zero 10 | column vector z n real numbers. zTMz>0 11 | 12 | 13 | Hermitian matrix: 14 | Matrix Mnxn is said to be positive definite if the scalar z*Mz is strictly positive for every non-zero 15 | column vector z n real numbers. z*Mz>0 16 | z* is the conjugate transpose of z. 17 | 18 | Positive semi-definite same as above except zTMz>=0 or z*Mz>=0 19 | 20 | 21 | Example: 22 | ┌2 -1 0┐ 23 | M= |-1 2 -1| 24 | |0 - 1 2| 25 | └ ┘ 26 | ┌ a ┐ 27 | z=| b | 28 | | c | 29 | └ ┘ 30 | zTMz=a^2 +c^2+ (a-b)^2+ (b-c)^2 31 | 32 | Cholesky decomposition: 33 | Cholesky decomposition of a Hermitian positive-definite matrix A is: 34 | A=LL* 35 | 36 | L is a lower triangular matrix with real and positive diagonal entries 37 | L* is the conjugate transpose of L 38 | */ 39 | 40 | Eigen::MatrixXd A(3,3); 41 | A << 6, 0, 0, 0, 4, 0, 0, 0, 7; 42 | Eigen::MatrixXd L( A.llt().matrixL() ); 43 | Eigen::MatrixXd L_T=L.adjoint();//conjugate transpose 44 | 45 | std::cout << "L" << std::endl; 46 | std::cout << L << std::endl; 47 | std::cout << "L_T" << std::endl; 48 | std::cout << L_T << std::endl; 49 | std::cout << "A" << std::endl; 50 | std::cout << A << std::endl; 51 | std::cout << "L*L_T" << std::endl; 52 | std::cout << L*L_T << std::endl; 53 | 54 | } 55 | 56 | 57 | void qRDecomposition(Eigen::MatrixXd &A,Eigen::MatrixXd &Q, Eigen::MatrixXd &R) 58 | { 59 | /* 60 | A=QR 61 | Q: is orthogonal matrix-> columns of Q are orthonormal 62 | R: is upper triangulate matrix 63 | this is possible when columns of A are linearly indipendent 64 | */ 65 | 66 | Eigen::MatrixXd thinQ(A.rows(),A.cols() ), q(A.rows(),A.rows()); 67 | 68 | Eigen::HouseholderQR householderQR(A); 69 | q = householderQR.householderQ(); 70 | thinQ.setIdentity(); 71 | Q = householderQR.householderQ() * thinQ; 72 | R=Q.transpose()*A; 73 | } 74 | 75 | void qRExample() 76 | { 77 | 78 | Eigen::MatrixXd A; 79 | A.setRandom(3,4); 80 | 81 | std::cout<<"A" < householderQR(A); 90 | q = householderQR.householderQ(); 91 | thinQ.setIdentity(); 92 | Q = householderQR.householderQ() * thinQ; 93 | 94 | std::cout << "HouseholderQR" <(); 100 | std::cout << R< colPivHouseholderQR(A.rows(), A.cols()); 114 | colPivHouseholderQR.compute(A); 115 | //R = colPivHouseholderQR.matrixR().template triangularView(); 116 | R = colPivHouseholderQR.matrixR(); 117 | Q = colPivHouseholderQR.matrixQ(); 118 | 119 | std::cout << "ColPivHouseholderQR" < fullPivHouseholderQR(A.rows(), A.cols()); 134 | fullPivHouseholderQR.compute(A); 135 | Q=fullPivHouseholderQR.matrixQ(); 136 | R=fullPivHouseholderQR.matrixQR().template triangularView(); 137 | 138 | std::cout << "Q" < matrix; 200 | matrix.resize(3,3); 201 | matrix<<1,2,1, 202 | 2,3,2, 203 | 1,2,3; 204 | 205 | } 206 | //http://eigen.tuxfamily.org/dox/group__DenseDecompositionBenchmark.html 207 | 208 | 209 | void denseDecompositions() 210 | { 211 | //LLT 212 | //LDLT 213 | //PartialPivLU 214 | //FullPivLU 215 | //HouseholderQR 216 | //ColPivHouseholderQR 217 | //CompleteOrthogonalDecomposition 218 | //FullPivHouseholderQR 219 | //JacobiSVD 220 | //BDCSVD 221 | } 222 | 223 | 224 | 225 | int main() 226 | { 227 | 228 | } 229 | 230 | -------------------------------------------------------------------------------- /src/memory_mapping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | void matrixFromStdVector() { 6 | std::vector data = {1, 2, 3, 4, 5, 6, 7, 8, 9}; 7 | 8 | auto einMatColMajor = Eigen::Map>(data.data()); 9 | 10 | data.clear(); 11 | std::cout << einMatColMajor << std::endl; 12 | 13 | auto einMatRowMajor = 14 | Eigen::Map>(data.data()); 15 | 16 | std::cout << einMatRowMajor << std::endl; 17 | } 18 | 19 | struct point { 20 | double a; 21 | double b; 22 | }; 23 | 24 | void eigenMapExample() { 25 | ////////////////////////////////////////First 26 | /// Example///////////////////////////////////////// 27 | Eigen::VectorXd solutionVec(12, 1); 28 | solutionVec << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12; 29 | Eigen::Map solutionColMajor(solutionVec.data(), 4, 3); 30 | 31 | Eigen::Map> solutionRowMajor( 32 | solutionVec.data()); 33 | 34 | std::cout << "solutionColMajor: " << std::endl; 35 | std::cout << solutionColMajor << std::endl; 36 | 37 | std::cout << "solutionRowMajor" << std::endl; 38 | std::cout << solutionRowMajor << std::endl; 39 | 40 | ////////////////////////////////////////Second 41 | /// Example///////////////////////////////////////// 42 | 43 | // https://stackoverflow.com/questions/49813340/stdvectoreigenvector3d-to-eigenmatrixxd-eigen 44 | 45 | int array[9]; 46 | for (int i = 0; i < 9; ++i) { 47 | array[i] = i; 48 | } 49 | 50 | Eigen::MatrixXi a(9, 1); 51 | a = Eigen::Map(array); 52 | std::cout << a << std::endl; 53 | 54 | std::vector pointsVec; 55 | point point1, point2, point3; 56 | 57 | point1.a = 1.0; 58 | point1.b = 1.5; 59 | 60 | point2.a = 2.4; 61 | point2.b = 3.5; 62 | 63 | point3.a = -1.3; 64 | point3.b = 2.4; 65 | 66 | pointsVec.push_back(point1); 67 | pointsVec.push_back(point2); 68 | pointsVec.push_back(point3); 69 | 70 | Eigen::Matrix2Xd pointsMatrix2d = Eigen::Map( 71 | reinterpret_cast(pointsVec.data()), 2, long(pointsVec.size())); 72 | 73 | Eigen::MatrixXd pointsMatrixXd = Eigen::Map( 74 | reinterpret_cast(pointsVec.data()), 2, long(pointsVec.size())); 75 | 76 | std::cout << pointsMatrix2d << std::endl; 77 | std::cout << "==============================" << std::endl; 78 | std::cout << pointsMatrixXd << std::endl; 79 | std::cout << "==============================" << std::endl; 80 | 81 | std::vector eigenPointsVec; 82 | eigenPointsVec.push_back(Eigen::Vector3d(2, 4, 1)); 83 | eigenPointsVec.push_back(Eigen::Vector3d(7, 3, 9)); 84 | eigenPointsVec.push_back(Eigen::Vector3d(6, 1, -1)); 85 | eigenPointsVec.push_back(Eigen::Vector3d(-6, 9, 8)); 86 | 87 | Eigen::MatrixXd pointsMatrix = Eigen::Map( 88 | eigenPointsVec[0].data(), 3, long(eigenPointsVec.size())); 89 | 90 | std::cout << pointsMatrix << std::endl; 91 | std::cout << "==============================" << std::endl; 92 | 93 | pointsMatrix = Eigen::Map( 94 | reinterpret_cast(eigenPointsVec.data()), 3, 95 | long(eigenPointsVec.size())); 96 | 97 | std::cout << pointsMatrix << std::endl; 98 | 99 | std::vector aa = {1, 2, 3, 4}; 100 | Eigen::VectorXd b = 101 | Eigen::Map(aa.data(), long(aa.size())); 102 | } 103 | 104 | int main() { 105 | matrixFromStdVector(); 106 | // eigenMapExample(); 107 | } 108 | -------------------------------------------------------------------------------- /src/non_linear_least_squares.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | /* 13 | 14 | https://en.wikipedia.org/wiki/Gauss%E2%80%93Newton_algorithm 15 | https://en.wikipedia.org/wiki/Non-linear_least_squares 16 | https://lingojam.com/SuperscriptGenerator 17 | https://lingojam.com/SubscriptGenerator 18 | 19 | consider m data points: 20 | (x₁,y₁),(x₂,y₂),...(xₘ,yₘ) 21 | 22 | and a functions which has n parameters: 23 | β=(β₁, β₂,..., βₙ) 24 | 25 | where m>=n 26 | 27 | this gives us m residuals: 28 | rᵢ=yᵢ-f(xᵢ,β) 29 | 30 | our objective is minimize the: 31 | 32 | S=Σ(rᵢ)² 33 | 34 | The minimum value of S occurs when the gradient is zero 35 | 36 | Lets say we have the following dataset: 37 | 38 | i 1 2 3 4 5 6 7 39 | x 0.038 0.194 0.425 0.626 1.253 2.500 3.740 40 | y 0.050 0.127 0.094 0.2122 0.2729 0.2665 0.3317 41 | 42 | and we have the folliwng function: 43 | 44 | y=(β₁*x)/(β₂+x) 45 | 46 | so our r is 47 | r₁=y₁ - (β₁*x₁)/(β₂+x₁) 48 | r₂=y₂ - (β₁*x₂)/(β₂+x₂) 49 | r₃=y₃ - (β₁*x₃)/(β₂+x₃) 50 | r₄=y₄ - (β₁*x₄)/(β₂+x₄) 51 | r₅=y₅ - (β₁*x₅)/(β₂+x₅) 52 | r₆=y₆ - (β₁*x₆)/(β₂+x₆) 53 | r₇=y₇ - (β₁*x₇)/(β₂+x₇) 54 | 55 | r(β)₂ₓ₇ -> J₇ₓ₂ 56 | 57 | 58 | σrᵢ/σβ₁=-xᵢ/(β₂+xᵢ) 59 | 60 | σrᵢ/σβ₂=(β₁*xᵢ)/(β₂xᵢ)² 61 | 62 | 63 | βᵏ⁺¹=βᵏ- (JᵀJ)⁻¹Jᵀr(βᵏ) 64 | */ 65 | 66 | 67 | 68 | // Generic functor 69 | template 70 | struct Functor 71 | { 72 | // Information that tells the caller the numeric type (eg. double) and size (input / output dim) 73 | typedef _Scalar Scalar; 74 | enum { 75 | InputsAtCompileTime = NX, 76 | ValuesAtCompileTime = NY 77 | }; 78 | // Tell the caller the matrix sizes associated with the input, output, and jacobian 79 | typedef Eigen::Matrix InputType; 80 | typedef Eigen::Matrix ValueType; 81 | typedef Eigen::Matrix JacobianType; 82 | 83 | // Local copy of the number of inputs 84 | int m_inputs, m_values; 85 | 86 | // Two constructors: 87 | Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 88 | Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 89 | 90 | // Get methods for users to determine function input and output dimensions 91 | int inputs() const { return m_inputs; } 92 | int values() const { return m_values; } 93 | 94 | }; 95 | 96 | typedef std::vector > Point2DVector; 97 | 98 | //Point2DVector GeneratePoints(const unsigned int numberOfPoints) 99 | 100 | struct SubstrateConcentrationFunctor : Functor 101 | { 102 | 103 | SubstrateConcentrationFunctor(Eigen::MatrixXd points): Functor(points.cols(),points.rows()) 104 | { 105 | this->Points = points; 106 | } 107 | 108 | int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &r) const 109 | { 110 | double x_i,y_i,beta1,beta2; 111 | for(unsigned int i = 0; i < this->Points.rows(); ++i) 112 | { 113 | y_i=this->Points.row(i)(1); 114 | x_i=this->Points.row(i)(0); 115 | beta1=z(0); 116 | beta2=z(1); 117 | r(i) =y_i-(beta1*x_i) /(beta2+x_i); 118 | } 119 | 120 | return 0; 121 | } 122 | Eigen::MatrixXd Points; 123 | 124 | int inputs() const { return 2; } // There are two parameters of the model, beta1, beta2 125 | int values() const { return this->Points.rows(); } // The number of observations 126 | }; 127 | 128 | 129 | 130 | void SubstrateConcentrationLeastSquare() 131 | { 132 | /* 133 | our data: 134 | 135 | x y 136 | 0.038,0.050; 137 | 0.194,0.127; 138 | 0.425,0.094; 139 | 0.626,0.2122; 140 | 1.253,0.2729; 141 | 2.500,0.2665; 142 | 3.740,0.3317; 143 | 144 | the last column in the matrix should be "y" 145 | 146 | */ 147 | 148 | 149 | Eigen::MatrixXd points(7,2); 150 | 151 | 152 | points.row(0)<< 0.038,0.050; 153 | points.row(1)<<0.194,0.127; 154 | points.row(2)<<0.425,0.094; 155 | points.row(3)<<0.626,0.2122; 156 | points.row(4)<<1.253,0.2729; 157 | points.row(5)<<2.500,0.2665; 158 | points.row(6)<<3.740,0.3317; 159 | 160 | SubstrateConcentrationFunctor functor(points); 161 | Eigen::NumericalDiff numDiff(functor); 162 | 163 | std::cout<<"functor.Points.size(): " < 5 | #include 6 | using namespace Eigen; 7 | 8 | void fullPivLU() 9 | { 10 | //https://stackoverflow.com/questions/31041921/how-to-get-rank-of-a-matrix-in-eigen-library 11 | 12 | Eigen::Matrix3d mat; 13 | mat<<2, 1, -1, 14 | -3, -1, 2, 15 | -2, 1, 2; 16 | 17 | Eigen::FullPivLU lu_decomp(mat); 18 | //Eigen::FullPivHouseholderQR 19 | auto rank = lu_decomp.rank(); 20 | std::cout<<"Rank:" < 2 | #include 3 | 4 | // Generic functor 5 | template 6 | struct Functor 7 | { 8 | // Information that tells the caller the numeric type (eg. double) and size (input / output dim) 9 | typedef _Scalar Scalar; 10 | enum { 11 | InputsAtCompileTime = NX, 12 | ValuesAtCompileTime = NY 13 | }; 14 | // Tell the caller the matrix sizes associated with the input, output, and jacobian 15 | typedef Eigen::Matrix InputType; 16 | typedef Eigen::Matrix ValueType; 17 | typedef Eigen::Matrix JacobianType; 18 | 19 | // Local copy of the number of inputs 20 | int m_inputs, m_values; 21 | 22 | // Two constructors: 23 | Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 24 | Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 25 | 26 | // Get methods for users to determine function input and output dimensions 27 | int inputs() const { return m_inputs; } 28 | int values() const { return m_values; } 29 | 30 | }; 31 | 32 | 33 | // y0 = 10*(x0+3)^2 + (x1-5)^2 34 | // y1 = (x0+1)*x1 35 | // y2 = sin(x0)*x1 36 | struct simpleFunctor : Functor 37 | { 38 | simpleFunctor(void): Functor(2,3) 39 | { 40 | 41 | } 42 | // Implementation of the objective function 43 | 44 | int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const 45 | { 46 | fvec(0) = 10.0*pow(x(0)+3.0,2) +pow(x(1)-5.0,2) ; 47 | fvec(1) = (x(0)+1)*x(1); 48 | fvec(2) = sin(x(0))*x(1); 49 | return 0; 50 | } 51 | }; 52 | 53 | 54 | void simpleNumericalDiffExample(Eigen::VectorXd &x) 55 | { 56 | simpleFunctor functor; 57 | Eigen::NumericalDiff numDiff(functor); 58 | Eigen::MatrixXd fjac(3,2); 59 | numDiff.df(x,fjac); 60 | std::cout << "jacobian of matrix at "<< x(0)<<","< 2 | #include 3 | 4 | // very good refrence: 5 | // https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html 6 | 7 | Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch, 8 | double yaw) { 9 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 10 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 11 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 12 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 13 | Eigen::Matrix3d rotationMatrix = q.matrix(); 14 | return rotationMatrix; 15 | } 16 | 17 | void quaternionFromRollPitchYaw() { 18 | 19 | //////////////// rotation matrix to quaternion //////////////// 20 | 21 | double roll, pitch, yaw; 22 | 23 | roll = M_PI / 4; 24 | pitch = M_PI / 3; 25 | yaw = M_PI / 6; 26 | 27 | Eigen::Matrix3d rotationMatrix = 28 | eulerAnglesToRotationMatrix(roll, pitch, yaw); 29 | 30 | Eigen::Quaterniond quaternionFromRotationMatrix(rotationMatrix); 31 | 32 | std::cout << "Quaternion X: " << quaternionFromRotationMatrix.x() 33 | << std::endl; 34 | std::cout << "Quaternion Y: " << quaternionFromRotationMatrix.y() 35 | << std::endl; 36 | std::cout << "Quaternion Z: " << quaternionFromRotationMatrix.z() 37 | << std::endl; 38 | std::cout << "Quaternion W: " << quaternionFromRotationMatrix.w() 39 | << std::endl; 40 | 41 | // // Rotation Matrix to Euler Angle (Proper) 42 | // Eigen::Vector3d euler_angles = rotationMatrix.eulerAngles(2, 0, 2); 43 | 44 | // // Eigen::Quaterniond 45 | // Eigen::Quaterniond tmp = 46 | // Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) * 47 | // Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitX()) * 48 | // Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitZ()); 49 | 50 | // Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 51 | 52 | //////////////// Quaternion to Rotation Matrix //////////////// 53 | Eigen::Matrix3d rotationMatrixFromQuaternion = 54 | quaternionFromRotationMatrix.matrix(); 55 | std::cout << "3x3 Rotation Matrix\n" 56 | << rotationMatrixFromQuaternion << std::endl; 57 | } 58 | 59 | struct Quaternion { 60 | double w, x, y, z; 61 | }; 62 | 63 | struct Point { 64 | double x, y, z; 65 | }; 66 | 67 | Quaternion quaternionMultiplication(Quaternion p, Quaternion q) { 68 | //(t0, t1, t2, t3) = (a1, b1, c1, d1) ✕ (a2, b2, c2, d2) 69 | 70 | // p=(a1 + b1*i, c1*j, d1*k)=(w,x,y,z) 71 | // q=(a2 + b2*i, c2*j, d2*k)=(w,x,y,z) 72 | double a1, b1, c1, d1, a2, b2, c2, d2; 73 | 74 | a1 = p.w; 75 | b1 = p.x; 76 | c1 = p.y; 77 | d1 = p.z; 78 | 79 | a2 = q.w; 80 | b2 = q.x; 81 | c2 = q.y; 82 | d2 = q.z; 83 | 84 | Quaternion t; 85 | 86 | // t0 = (a1a2 − b1b2 − c1c2 − d1d2) 87 | t.w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2; 88 | 89 | // t1 = (a1b2+b1a2+c1d2 -d1c2 ) 90 | t.x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2; 91 | 92 | // t2 = (a1c2 -b1d2 +c1a2+ d1b2 ) 93 | t.y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2; 94 | 95 | // t3 = (a1d2+ b1c2 -c1b2+ d1a2 ) 96 | t.z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2; 97 | 98 | return t; 99 | } 100 | 101 | Quaternion quaternionInversion(Quaternion q) { 102 | // The inverse of a quaternion is obtained by negating the imaginary 103 | // components: 104 | 105 | q.x = -q.x; 106 | q.y = -q.y; 107 | q.z = -q.z; 108 | return q; 109 | } 110 | 111 | Point QuaternionRotation(Quaternion q, Point p) { 112 | // Step 1:Convert the point to be rotated into a quaternion 113 | // p = (p0, p1, p2, p3) = ( 0, x, y, z ) 114 | Quaternion tmp; 115 | tmp.w = 0; 116 | tmp.x = p.x; 117 | tmp.y = p.y; 118 | tmp.z = p.z; 119 | 120 | // Step 2: Perform active rotation: when the point is rotated with respect to 121 | // the coordinate system p' = q−1 pq 122 | Quaternion p_prime = quaternionMultiplication( 123 | quaternionMultiplication(quaternionInversion(q), tmp), q); 124 | 125 | // Perform passive rotation: when the coordinate system is rotated with 126 | // respect to the point. The two rotations are opposite from each other. p' = 127 | // qpq−1 128 | 129 | // p_prime = quaternionMultiplication(quaternionMultiplication(q, tmp), 130 | // quaternionInversion(q)); 131 | 132 | // Step 3: Extract the rotated coordinates from p': 133 | Point rotate_d_p; 134 | rotate_d_p.x = p_prime.x; 135 | rotate_d_p.y = p_prime.y; 136 | rotate_d_p.z = p_prime.z; 137 | 138 | return rotate_d_p; 139 | } 140 | 141 | 142 | //rotate a vector3d with a quaternion 143 | Eigen::Vector3d QuaternionRotation(Eigen::Quaterniond q, Eigen::Vector3d p) 144 | { 145 | return q * p; 146 | } 147 | 148 | 149 | void QuaternionRotation() { 150 | 151 | Eigen::IOFormat HeavyFmt(Eigen::StreamPrecision, 2, ", ", ";\n", "[", "]", 152 | "[", "]"); 153 | 154 | // P = [0, p1, p2, p3] <-- point vector 155 | // alpha = angle to rotate 156 | //[x, y, z] = axis to rotate around (unit vector) 157 | // R = [cos(alpha/2), sin(alpha/2)*x, sin(alpha/2)*y, sin(alpha/2)*z] <-- rotation 158 | // R' = [w, -x, -y, -z] 159 | // P' = RPR' 160 | // P' = H(H(R, P), R') 161 | 162 | Eigen::Vector3d p(1, 0, 0); 163 | 164 | Quaternion P; 165 | P.w = 0; 166 | P.x = p(0); 167 | P.y = p(1); 168 | P.z = p(2); 169 | 170 | // rotation of 90 degrees about the y-axis 171 | double alpha = M_PI / 2; 172 | Quaternion R; 173 | Eigen::Vector3d r(0, 1, 0); 174 | r = r.normalized(); 175 | 176 | R.w = cos(alpha / 2); 177 | R.x = sin(alpha / 2) * r(0); 178 | R.y = sin(alpha / 2) * r(1); 179 | R.z = sin(alpha / 2) * r(2); 180 | 181 | std::cout << R.w << "," << R.x << "," << R.y << "," << R.z << std::endl; 182 | 183 | Quaternion R_prime = quaternionInversion(R); 184 | Quaternion P_prime = 185 | quaternionMultiplication(quaternionMultiplication(R, P), R_prime); 186 | 187 | /*rotation of 90 degrees about the y-axis for the point (1, 0, 0). The result 188 | is (0, 0, -1). (Note that the first element of P' will always be 0 and can 189 | therefore be discarded.) 190 | */ 191 | std::cout << P_prime.x << "," << P_prime.y << "," << P_prime.z << std::endl; 192 | } 193 | 194 | 195 | Quaternion rollPitchYawToQuaternion(double roll, double pitch, 196 | double yaw) // roll (x), pitch (Y), yaw (z) 197 | { 198 | // Abbreviations for the various angular functions 199 | 200 | double cr = cos(roll * 0.5); 201 | double sr = sin(roll * 0.5); 202 | double cp = cos(pitch * 0.5); 203 | double sp = sin(pitch * 0.5); 204 | double cy = cos(yaw * 0.5); 205 | double sy = sin(yaw * 0.5); 206 | 207 | Quaternion q; 208 | q.w = cr * cp * cy + sr * sp * sy; 209 | q.x = sr * cp * cy - cr * sp * sy; 210 | q.y = cr * sp * cy + sr * cp * sy; 211 | q.z = cr * cp * sy - sr * sp * cy; 212 | 213 | return q; 214 | } 215 | 216 | void convertAxisAngleToQuaternion() {} 217 | 218 | void ConvertQuaterniontoAxisAngle() {} 219 | 220 | void QuaternionRepresentingRotationFromOneVectortoAnother() { 221 | Quaternion q; 222 | Eigen::Vector3d v1, v2; 223 | Eigen::Vector3d a = v1.cross(v2); 224 | q.x = a(0); 225 | q.y = a(1); 226 | q.z = a(2); 227 | 228 | q.w = sqrt((pow(v1.norm(), 2)) * (pow(v1.norm(), 2))) + v1.dot(v2); 229 | } 230 | 231 | 232 | int main() { 233 | 234 | // double roll, pitch, yaw; 235 | 236 | // roll = M_PI / 4; 237 | // pitch = M_PI / 3; 238 | // yaw = M_PI / 6; 239 | 240 | // Eigen::Matrix3d rotationMatrix = 241 | // eulerAnglesToRotationMatrix(roll, pitch, yaw); 242 | 243 | //// Eigen::Quaterniond quaternionFromRotationMatrix(rotationMatrix); 244 | 245 | // Eigen::Matrix3d R_b_c, R_s_b; 246 | 247 | // R_b_c << 0, 0, -1, 0, 1, 0, 1, 0, 0; 248 | 249 | // std::cout << "R_b_c\n" << R_b_c << std::endl; 250 | 251 | // R_s_b << 0, -1, 0, 1, 0, 0, 0, 0, 1; 252 | 253 | // std::cout << "R_s_b\n" << R_s_b << std::endl; 254 | 255 | // Eigen::Matrix3d R_s_c = R_s_b * R_b_c; 256 | 257 | // std::cout << "R_s_c\n" << R_s_c << std::endl; 258 | 259 | // Eigen::Vector3d p_b; 260 | // p_b << -1, 0, 0; 261 | 262 | // Eigen::Vector3d p_s = R_s_b * p_b; 263 | 264 | // std::cout << p_s << std::endl; 265 | 266 | 267 | } 268 | -------------------------------------------------------------------------------- /src/rotation_matrices.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Transform, Translation, Scaling, Rotation2D and 3D rotations (Quaternion, 5 | // AngleAxis) 6 | 7 | #if KDL_FOUND == 1 8 | #include 9 | #endif 10 | 11 | Eigen::Matrix3d eulerAnglesToRotationMatrix(double roll, double pitch, 12 | double yaw) { 13 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 14 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 15 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 16 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 17 | Eigen::Matrix3d rotationMatrix = q.matrix(); 18 | return rotationMatrix; 19 | } 20 | 21 | void createRotationMatrix() { 22 | /* 23 | http://lavalle.pl/planning/node102.html 24 | http://euclideanspace.com/maths/geometry/rotations/conversions/index.htm 25 | 26 | Tait–Bryan angles: Z1Y2X3 in the wiki page: 27 | https://en.wikipedia.org/wiki/Euler_angles 28 | yaw: 29 | A yaw is a counterclockwise rotation of alpha about the z-axis. The 30 | rotation matrix is given by 31 | 32 | R_z 33 | 34 | |cos(alpha) -sin(alpha) 0| 35 | |sin(apha) cos(alpha) 0| 36 | | 0 0 1| 37 | 38 | pitch: 39 | R_y 40 | A pitch is a counterclockwise rotation of beta about the y-axis. The 41 | rotation matrix is given by 42 | 43 | |cos(beta) 0 sin(beta)| 44 | |0 1 0 | 45 | |-sin(beta) 0 cos(beta)| 46 | 47 | roll: 48 | A roll is a counterclockwise rotation of gamma about the x-axis. The 49 | rotation matrix is given by 50 | R_x 51 | |1 0 0| 52 | |0 cos(gamma) -sin(gamma)| 53 | |0 sin(gamma) cos(gamma)| 54 | 55 | 56 | 57 | It is important to note that R_z R_y R_x performs the roll first, then 58 | the pitch, and finally the yaw Roration matrix: R_z*R_y*R_x 59 | 60 | */ 61 | double roll, pitch, yaw, alpha, beta, gamma; 62 | roll = M_PI / 4; 63 | pitch = M_PI / 3; 64 | yaw = M_PI / 6; 65 | 66 | alpha = yaw; 67 | beta = pitch; 68 | gamma = roll; 69 | 70 | Eigen::Matrix3d R_z, R_y, R_x; 71 | 72 | // yaw: 73 | R_z << cos(alpha), -sin(alpha), 0, sin(alpha), cos(alpha), 0, 0, 0, 1; 74 | 75 | // pitch: 76 | R_y << cos(beta), 0, sin(beta), 0, 1, 0, -sin(beta), 0, cos(beta); 77 | 78 | // roll: 79 | 80 | R_x << 1, 0, 0, 0, cos(gamma), -sin(gamma), 0, sin(gamma), cos(gamma); 81 | 82 | std::cout << eulerAnglesToRotationMatrix(roll, pitch, yaw) << std::endl; 83 | 84 | // It is important to note that R_z R_y R_x performs the roll first, then 85 | // the pitch, and finally the yaw 86 | std::cout << R_z * R_y * R_x << std::endl; 87 | } 88 | 89 | void rotationMatrices() { 90 | 91 | //////////////////// Rotation Matrix (Tait–Bryan) //////////////////////// 92 | double roll, pitch, yaw; 93 | roll = M_PI / 2; 94 | pitch = M_PI / 2; 95 | yaw = M_PI / 6; 96 | std::cout << "Roll : " << roll << std::endl; 97 | std::cout << "Pitch : " << pitch << std::endl; 98 | std::cout << "Yaw : " << yaw << std::endl; 99 | 100 | // Roll, Pitch, Yaw to Rotation Matrix 101 | // Eigen::AngleAxis rollAngle(roll, Eigen::Vector3d(1,0,0)); 102 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 103 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 104 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 105 | 106 | ////////////////////////////////////////Comparing with 107 | /// KDL//////////////////////////////////////// 108 | #if KDL_FOUND == 1 109 | KDL::Frame F; 110 | F.M = F.M.RPY(roll, pitch, yaw); 111 | std::cout << F.M(0, 0) << " " << F.M(0, 1) << " " << F.M(0, 2) << std::endl; 112 | std::cout << F.M(1, 0) << " " << F.M(1, 1) << " " << F.M(1, 2) << std::endl; 113 | std::cout << F.M(2, 0) << " " << F.M(2, 1) << " " << F.M(2, 2) << std::endl; 114 | 115 | double x, y, z, w; 116 | F.M.GetQuaternion(x, y, z, w); 117 | std::cout << "KDL Frame Quaternion:" << std::endl; 118 | std::cout << "x: " << x << std::endl; 119 | std::cout << "y: " << y << std::endl; 120 | std::cout << "z: " << z << std::endl; 121 | std::cout << "w: " << w << std::endl; 122 | #endif 123 | 124 | Eigen::Matrix3d rotation; 125 | rotation = eulerAnglesToRotationMatrix(roll, pitch, yaw); 126 | 127 | double txLeft, tyLeft, tzLeft; 128 | txLeft = -1; 129 | tyLeft = 0.0; 130 | tzLeft = -4.0; 131 | 132 | Eigen::Affine3f t1; 133 | Eigen::Matrix4f M; 134 | Eigen::Vector3d translation; 135 | translation << txLeft, tyLeft, tzLeft; 136 | 137 | M << rotation(0, 0), rotation(0, 1), rotation(0, 2), translation(0, 0), 138 | rotation(1, 0), rotation(1, 1), rotation(1, 2), translation(1, 0), 139 | rotation(2, 0), rotation(2, 1), rotation(2, 2), translation(2, 0), 0, 0, 140 | 0, 1; 141 | 142 | t1 = M; 143 | 144 | Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity(); 145 | 146 | // Define a translation of 2.5 meters on the x axis. 147 | transform_2.translation() << 2.5, 1.0, 0.5; 148 | 149 | // The same rotation matrix as before; tetha radians arround Z axis 150 | transform_2.rotate(yawAngle * pitchAngle * rollAngle); 151 | std::cout << transform_2.matrix() << std::endl; 152 | std::cout << transform_2.translation() << std::endl; 153 | std::cout << transform_2.translation().x() << std::endl; 154 | std::cout << transform_2.translation().y() << std::endl; 155 | std::cout << transform_2.translation().z() << std::endl; 156 | } 157 | 158 | void getingRollPitchYawFromRotationMatrix() { 159 | /* http://planning.cs.uiuc.edu/node103.html 160 | 161 | |r11 r12 r13 | 162 | |r21 r22 r23 | 163 | |r31 r32 r33 | 164 | 165 | yaw: alpha=arctan(r21/r11) 166 | pitch: beta=arctan(-r31/sqrt( r32^2+r33^2 ) ) 167 | roll: gamma=arctan(r32/r33) 168 | */ 169 | double roll, pitch, yaw; 170 | roll = M_PI / 3; 171 | 172 | // these two will cause singularity 173 | // pitch = -M_PI / 2 174 | // pitch = M_PI / 2; 175 | pitch = M_PI / 4; 176 | yaw = M_PI / 6; 177 | Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX()); 178 | Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY()); 179 | Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ()); 180 | 181 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 182 | Eigen::Matrix3d rotationMatrix = q.matrix(); 183 | 184 | std::cout << "Rotation Matrix is:" << std::endl; 185 | std::cout << rotationMatrix << std::endl; 186 | 187 | std::cout << "roll is Pi/" 188 | << M_PI / atan2(rotationMatrix(2, 1), rotationMatrix(2, 2)) 189 | << std::endl; 190 | std::cout << "pitch: Pi/" 191 | << M_PI / atan2(-rotationMatrix(2, 0), 192 | std::pow( 193 | rotationMatrix(2, 1) * rotationMatrix(2, 1) + 194 | rotationMatrix(2, 2) * rotationMatrix(2, 2), 195 | 0.5)) 196 | << std::endl; 197 | 198 | std::cout << "yaw is Pi/" 199 | << M_PI / atan2(rotationMatrix(1, 0), rotationMatrix(0, 0)) 200 | << std::endl; 201 | 202 | // Rotation Matrix to Tait–Bryan angles 203 | Eigen::Vector3d euler_angles = rotationMatrix.eulerAngles(2, 1, 0); 204 | 205 | std::cout << " Pi/"< 2 | #include 3 | 4 | template 5 | T pseudoInverse(const T &a, double epsilon = std::numeric_limits::epsilon()) 6 | { 7 | //Eigen::DecompositionOptions flags; 8 | int flags; 9 | // For a non-square matrix 10 | if(a.cols()!=a.rows()) 11 | { 12 | flags=Eigen::ComputeThinU | Eigen::ComputeThinV; 13 | } 14 | else 15 | { 16 | flags=Eigen::ComputeFullU | Eigen::ComputeFullV; 17 | } 18 | Eigen::JacobiSVD< T > svd(a ,flags); 19 | 20 | double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0); 21 | return svd.matrixV() * (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint(); 22 | } 23 | 24 | 25 | 26 | template 27 | Eigen::Matrix 28 | pseudoinverse(const MatT &mat, typename MatT::Scalar tolerance = typename MatT::Scalar{1e-4}) // choose appropriately 29 | { 30 | typedef typename MatT::Scalar Scalar; 31 | auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV); 32 | const auto &singularValues = svd.singularValues(); 33 | Eigen::Matrix singularValuesInv(mat.cols(), mat.rows()); 34 | singularValuesInv.setZero(); 35 | for (unsigned int i = 0; i < singularValues.size(); ++i) { 36 | if (singularValues(i) > tolerance) 37 | { 38 | singularValuesInv(i, i) = Scalar{1} / singularValues(i); 39 | } 40 | else 41 | { 42 | singularValuesInv(i, i) = Scalar{0}; 43 | } 44 | } 45 | return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint(); 46 | } 47 | 48 | 49 | 50 | void SVD_Example() 51 | { 52 | /* 53 | 54 | AX=0; 55 | A, U, V=SVD(A); 56 | A* U(Index of last column)=0; 57 | 58 | 1) Full SVD 59 | A mxn 60 | U mxm 61 | Σ mxn 62 | V* nxn 63 | 64 | 65 | 2) Thin SVD 66 | A mxn 67 | U mxn 68 | Σ nxn 69 | V* nxn 70 | 71 | 3) Compact SVD 72 | 73 | 4) Truncated SVD 74 | Ref: https://en.wikipedia.org/wiki/Singular_value_decomposition#Thin_SVD 75 | 76 | */ 77 | 78 | std::cout<<"********************** 1) Full SVD ***********************************" < svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); 85 | 86 | 87 | std::cout<< "Size of original matrix:"<< A.rows()<<","< svd_thin( C, Eigen::ComputeThinU | Eigen::ComputeThinV); 130 | 131 | std::cout<< "Size of original matrix:"<< C.rows()<<","< 2 | // #include 3 | #include 4 | #include 5 | 6 | void solvingSystemOfLinearEquationsUsingSVD() { 7 | 8 | Eigen::MatrixXf A = Eigen::MatrixXf::Random(3, 2); 9 | std::cout << "Here is the matrix A:\n" << A << std::endl; 10 | Eigen::VectorXf b = Eigen::VectorXf::Random(3); 11 | std::cout << "Here is the right hand side b:\n" << b << std::endl; 12 | std::cout << "The least-squares solution is:\n" 13 | << A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b) 14 | << std::endl; 15 | } 16 | 17 | void solvingSystemOfLinearEquationsUsingQRDecomposition() { 18 | 19 | Eigen::MatrixXf A = Eigen::MatrixXf::Random(3, 2); 20 | Eigen::VectorXf b = Eigen::VectorXf::Random(3); 21 | std::cout << "The solution using the QR decomposition is:\n" 22 | << A.colPivHouseholderQr().solve(b) << std::endl; 23 | } 24 | 25 | void solvingSystemOfLinearEquationsUsingCompleteOrthogonalDecomposition() { 26 | Eigen::MatrixXf A = Eigen::MatrixXf::Random(3, 2); 27 | Eigen::VectorXf b = Eigen::VectorXf::Random(3); 28 | 29 | // Eigen::CompleteOrthogonalDecomposition> cod; 31 | // std::cout< matrix_NN = 50 | MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); 51 | matrix_NN = 52 | matrix_NN * matrix_NN.transpose(); // Guarantee semi−positive definite 53 | Matrix v_Nd = MatrixXd::Random(MATRIX_SIZE, 1); 54 | 55 | clock_t time_stt = clock(); // timing 56 | // Direct inversion 57 | Matrix x = matrix_NN.inverse() * v_Nd; 58 | cout << "======================= time of normal inverse is: " 59 | 60 | << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC 61 | << "ms ======================= " << endl; 62 | // cout << "x = " << x.transpose() << endl; 63 | 64 | // Usually solved by matrix decomposition, such as QR decomposition, the speed 65 | // will be much faster 66 | time_stt = clock(); 67 | x = matrix_NN.colPivHouseholderQr().solve(v_Nd); 68 | cout << "======================= time of Qr decomposition is: " 69 | 70 | << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC 71 | << "ms ======================= " << endl; 72 | // cout << "x = " << x.transpose() << endl; 73 | 74 | // For positive definite matrices, you can also use cholesky decomposition to 75 | // solve equations. 76 | time_stt = clock(); 77 | x = matrix_NN.ldlt().solve(v_Nd); 78 | cout << "======================= time of ldlt decomposition is " 79 | << 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC 80 | << "ms ======================= " << endl; 81 | // cout << "x = " << x.transpose() << endl; 82 | } 83 | 84 | int main() { 85 | // solvingSystemOfLinearEquationsSVD(); 86 | // solvingSystemOfLinearEquationsUsingCompleteOrthogonalDecomposition(); 87 | solver(); 88 | } 89 | -------------------------------------------------------------------------------- /src/sparse_matrices.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | //CRS 7 | void compressedSparseRow() 8 | { 9 | /* 10 | 0 1 2 3 4 5 6 7 8 11 | ┌ ┐ 12 | 0 |0 0 0 0 0 0 0 3 0| 13 | 1 |0 0 8 0 0 1 0 0 0| 14 | 2 |0 0 0 0 0 0 0 0 0| 15 | 3 |4 0 0 0 0 0 0 0 0| 16 | 4 |0 0 0 0 0 0 0 0 0| 17 | 5 |0 0 2 0 0 0 0 0 0| 18 | 6 |0 0 0 6 0 0 0 0 0| 19 | 7 |0 9 0 0 5 0 0 0 0| 20 | └ ┘ 21 | */ 22 | 23 | 24 | int rows=8; 25 | int cols=9; 26 | Eigen::Matrix dense_mat; 27 | dense_mat.resize(rows,cols); 28 | 29 | 30 | dense_mat<<0, 0, 0, 0, 0, 0, 0, 3, 0, 31 | 0, 0, 8, 0, 0, 1, 0, 0, 0, 32 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 33 | 4, 0, 0, 0, 0, 0, 0, 0, 0, 34 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 35 | 0, 0, 2, 0, 0, 0, 0, 0, 0, 36 | 0, 0, 0, 6, 0, 0, 0, 0, 0, 37 | 0, 9, 0, 0, 5, 0, 0, 0, 0; 38 | 39 | //specify a tolerance: 40 | //sparse_mat = dense_mat.sparseView(epsilon,reference); 41 | Eigen::SparseMatrix sparse_mat = dense_mat.sparseView(); 42 | 43 | std::cout<<"sparse matrix number of cols:"< SpMat; // declares a column-major sparse matrix type of double 93 | typedef Eigen::Triplet T;//structure to hold a non zero as a triplet (i,j,value). 94 | 95 | void buildProblem(std::vector& coefficients, Eigen::VectorXd& b, int n){} 96 | void saveAsBitmap(const Eigen::VectorXd& x, int n, const char* filename){} 97 | //https://eigen.tuxfamily.org/dox/classEigen_1_1Triplet.html 98 | int main(int argc, char** argv) 99 | { 100 | if(argc!=2) { 101 | std::cerr << "Error: expected one and only one argument.\n"; 102 | return -1; 103 | } 104 | 105 | int n = 300; // size of the image 106 | int m = n*n; // number of unknows (=number of pixels) 107 | 108 | // Assembly: 109 | std::vector coefficients; // list of non-zeros coefficients 110 | Eigen::VectorXd b(m); // the right hand side-vector resulting from the constraints 111 | buildProblem(coefficients, b, n); 112 | 113 | SpMat A(m,m); 114 | A.setFromTriplets(coefficients.begin(), coefficients.end()); 115 | 116 | // Solving: 117 | Eigen::SimplicialCholesky chol(A); // performs a Cholesky factorization of A 118 | Eigen::VectorXd x = chol.solve(b); // use the factorization to solve for the given right hand side 119 | 120 | // Export the result to a file: 121 | saveAsBitmap(x, n, argv[1]); 122 | 123 | return 0; 124 | } 125 | -------------------------------------------------------------------------------- /src/unaryExpr.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | //unaryExpr, Lambda Expression, function pointer ,in place update 6 | double ramp(double x) 7 | { 8 | if (x > 0) 9 | return x; 10 | else 11 | return 0; 12 | } 13 | 14 | void unaryExprExample() 15 | { 16 | //unaryExpr is a const function so it will not give you the possibility to modify values in place 17 | Eigen::ArrayXd x = Eigen::ArrayXd::Random(5); 18 | std::cout<