重頭開始學ROS（6）---Eigen庫安裝與使用

`Eigen`庫

矩陣運算是一種非常重要的運算方式，在Matlab中，矩陣運算可以輕松的實現，但在C++這種偏底層的語言中，若不借助第三方庫，矩陣運算需要我們進行較為復雜的代碼設計。Eigen庫是一個用于線性運算的C++模板庫，它支持矩陣運算、矢量運算、數值分析以及相關的算法，安裝Eigen庫可以大大提高我們的開發效率。

安裝與配置`Eigen`庫

安裝

在ubuntu系統中，安裝Eigen庫較為簡單，只需在命令行中運行

sudo apt-get install libeigen3-dev

之后，我們可以在系統中找到/usr/include/eigen3，這證明我們安裝成功

在`Vscode`配置

使用Vscode可以很方便地配置Eigen庫的編譯，在c_cpp_properties.json的includepath中添加"/usr/include/**"即可，如果已經有了那就無需再添加。

{"configurations": [{"browse": {"databaseFilename": "${default}","limitSymbolsToIncludedHeaders": false},"includePath": ["/opt/ros/noetic/include/**","/usr/include/**","/home/aliang/vehicle_sim/devel/include/**"],"name": "ROS","intelliSenseMode": "gcc-x64","compilerPath": "/usr/bin/gcc","cStandard": "gnu11","cppStandard": "c++17"}],"version": 4
}

與`ROS`中配置

在ROS中，我們還需在功能包的CMakeList中添加：

include_directories("/usr/include/eigen3")

這樣我們就可以在程序中用這種方法引入包：

    #include <Eigen/Eigen>#include <Eigen/Dense>#include <Eigen/Geometry>#include <Eigen/Eigenvalues>

否則，需要使用這種方法引入包：

    #include <eigen3/Eigen/Eigen>#include <eigen3/Eigen/Dense>#include <eigen3/Eigen/Geometry>#include <eigen3/Eigen/Eigenvalues>

驗證是否可以正常使用

在ROS的功能包下新建一個cpp文件

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>int main(int argc, char *argv[]){// here is an arbitrary normal vector
// initialized to (1,2,3) upon instantiationEigen::Vector3d normal_vec(1,2,3); return 0;
}

在CMakeList中加入

add_executable(vec_compute src/compute.cpp)
target_link_libraries(vec_compute ${catkin_LIBRARIES})

編譯通過，證明可以正常使用

基于`Eigen`庫的RBF徑向基核函數的二維曲線擬合

RBF徑向基核函數的二維曲線擬合

RBF徑向基核函數的二維曲線擬合的核心在于，根據 $N$ 個擬合點求解出 $N$ 個徑向基核函數（ $N$ 個徑向基核函數分別對應 $N$ 個擬合點）的加權數。對于目標二維曲線 $F (x, y) = 1$ 即優化下列方程：
$\sum_{k=1}^N w_k*f(r) = 1$
展開為向量形式：
$\begin{pmatrix} f(||q-q_1||) & f(||q-q_2||) & \cdots & f(||q-q_N||) \end{pmatrix}* \begin{pmatrix} w_1\\ w_2\\ \vdots\\ w_N \end{pmatrix}=1$
其中 $q_N$ 表示被擬合的 $N$ 個點，由于每個被擬合點都滿足該公式，替換掉 $q$ ，可以再次展開為：
$\begin{pmatrix} f(||q_1-q_1||) & f(||q_1-q_2||) & \cdots & f(||q_1-q_N||)\\ f(||q_2-q_1||) & f(||q_2-q_2||) & \cdots & f(||q_2-q_N||)\\ \vdots & \vdots & \ddots & \vdots\\ f(||q_N-q_1||) & f(||q_N-q_2||) & \cdots & f(||q_N-q_N||)\\ \end{pmatrix}* \begin{pmatrix} w_1\\ w_2\\ \vdots\\ w_N \end{pmatrix}=1$
左邊的矩陣可以看為一個Gram相關矩陣形式，則可以求出我們想要的 $\vec{w}$ ：

$\vec{w} = GramMatrix^{-1}_{N \times N}*1_{N \times 1}$

選取徑向基核函數為 $f(r)=r^2ln(r+1)$

基于`Eigen`庫的實現

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <iostream>
#include <cmath>int main(int argc, char *argv[]){int num = 6;Eigen::MatrixXd p_x(1, num);Eigen::MatrixXd dis_x(num, num);Eigen::MatrixXd p_y(1, num);Eigen::MatrixXd dis_y(num, num);Eigen::MatrixXd dis_sqr(num, num);Eigen::MatrixXd dis_r(num, num);Eigen::MatrixXd w(num, 1);p_x << 1.5, 1.5, -0.75, -3, -0.75, 1.5;p_y << 0, 2.6, 1.3, 0, -1.3, -2.6;Eigen::MatrixXd mat_x = p_x.replicate(num, 1); // 該函數將p_x作為一個矩陣元素，填充出n*m的矩陣Eigen::MatrixXd mat_y = p_y.replicate(num, 1);mat_x = mat_x - p_x.transpose().replicate(1, num);mat_y = mat_y - p_y.transpose().replicate(1, num);dis_x = mat_x.cwiseProduct(mat_x); //對應位置相乘dis_y = mat_y.cwiseProduct(mat_y);dis_sqr = dis_x + dis_y;dis_r = dis_sqr.unaryExpr([](double x){return std::sqrt(x);});std::cout << "矩陣：\n" << dis_r << std::endl;dis_r = dis_r.unaryExpr([](double x){return x*x*log(x+1);});dis_r = dis_r.inverse();w = dis_r.rowwise().sum();std::cout << "矩陣：\n" << w << std::endl;return 0;
}

與Matlab計算結果相同

w=-0.04752690.0351389-0.04752050.0351453-0.04752050.0351389

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/pingmian/86850.shtml
繁體地址，請注明出處：http://hk.pswp.cn/pingmian/86850.shtml
英文地址，請注明出處：http://en.pswp.cn/pingmian/86850.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！