ros::spin() 和 ros::spinOnce() 区别及详解
https://www.cnblogs.com/liu-fa/p/5925381.html
ros消息时间同步与回调
https://blog.csdn.net/zyh821351004/article/details/47758433
ros 在同一个文件中发布和订阅
https://blog.csdn.net/heyijia0327/article/details/45567373
ROS中Eigen3使用
1安装:sudo apt-get install libeigen3-dev
2 调整,默认安装路径是:/usr/include/eigen3
cd /usr/include/eigen3
二维、三维向量操作:
#include<opencv2/opencv.hpp>
#include<iostream>
#include"stdio.h"
using namespace std;
using namespace cv;
int main()
{
vector<Point2f> vp2f;
vp2f.push_back(Point2f(2, 3));
cout << "【二维点向量】" << endl << vp2f << endl;
vector<Point3f> vp3f(20);
for (size_t i = 0; i < vp3f.size(); i++)
{
vp3f[i] = Point3f((float)(i + i), (float)(i * i), (float)(i + 1));
}
cout << "【三维点向量】" << endl << vp3f << endl;
system("pause");
return 0;
}
Eigen用法:
Eigen定义了一些类型
MatrixNt = Matrix<type, N, N> 特殊地有 MatrxXi = Matrix<int, Dynamic, Dynamic>
VectorNt = Matrix<type, N, 1> 比如 Vector2f = Matrix<float, 2, 1>
RowVectorNt = Matrix<type, 1, N> 比如 RowVector3d = Matrix<double, 1, 3>
// N可以是2,3,4或X(Dynamic)
// t可以是i(int)、f(float)、d(double)、cf(complex)、cd(complex)等。
Eigen教程:
https://dritchie.github.io/csci2240/assignments/eigen_tutorial.pdf
学习笔记:
https://www.cnblogs.com/houkai/p/6347648.html
官网例程:
https://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html
更全面的字典:
https://zhuanlan.zhihu.com/p/42718881
Eigen实现最小二乘法:
#include <opencv2/highgui.hpp>
#include <opencv2/aruco.hpp>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <Eigen/Cholesky>
using namespace std;
using namespace cv;
using namespace Eigen;
vector<Point3f> rPoint3, cPoint3;
vector<Point2f> rPoint2, cPoint2;
// 定义输出的结果
Mat mH;
// 最小二乘法
void computMatrix1(vector<Point3f>& radar_image, vector<Point3f>& camera_image, Mat& mH) {
if (camera_image.size() != radar_image.size() )
cout << " Program error !" << endl;
// 变量定义
VectorXf Uc(camera_image.size());
VectorXf Vc(camera_image.size());
VectorXf Ic(camera_image.size());
MatrixXf Pr(radar_image.size(),3);
Vector3f Tw1(1, 2, 3);
Vector3f Tw2(3, 4, 5);
// 打印输出
cout << "radar size : " << radar_image.size() << endl;
cout << "【雷达三维点向量】" << endl << rPoint3 << endl;
cout << "camera size : " << camera_image.size() << endl;
cout << "【相机三维点向量】" << endl << cPoint3 << endl;
// const int rowM = radar_image.size();
for (int i = 0; i < camera_image.size(); i++) {
Uc(i) = cPoint3[i].x;
Vc(i) = cPoint3[i].y;
Ic(i) = 1;
//
Pr(i, 0) = radar_image[i].x;
Pr(i, 1) = radar_image[i].y;
Pr(i, 2) = radar_image[i].z;
}
cout << "【相机三维点向量】" << endl;
cout << "Eigen Vector U : " << endl << Uc << endl;
cout << "Eigen Vector V : " << endl << Vc << endl;
cout << "Eigen Vector I : " << endl << Ic << endl;
cout << "【雷达三维点向量】" << endl;
cout << "Eigen Martix Pr : " << endl << Pr << endl;
// Matrix<float, Dynamic, 3> martix_P;
// Matrix<float, Dynamic, 1> martix_X;
// Matrix<float, 3, 1> x_tmp;
// // Solve Ax = b. Result stored in x. Matlab: x = A \ b.
// x_tmp = martix_P.ldlt().solve(martix_X); // A sym. p.s.d. #include <Eigen/Cholesky>
//
// Transposition
cout << "转置矩阵 " << endl << Pr.transpose() << endl;
// Inversion ( #include <Eigen/Dense > )
// Generates NaNs if the matrix is not invertible
cout << "求逆矩阵 " << endl << (Pr.transpose()*Pr).inverse() << endl;
Tw1 = (((Pr.transpose()*Pr).inverse())*Pr.transpose())*Uc;
Tw2 = (((Pr.transpose()*Pr).inverse())*Pr.transpose())*Vc;
cout << "Output 1 : " << endl << Tw1 << endl;
cout << "Output 2 : " << endl << Tw2 << endl;
cout << "mH_lsm : " << mH << endl;
}
int main(int argc, char *argv[]) {
cPoint3.push_back(Point3f( -0.0073, 6.2800, 1.0000));
cPoint3.push_back(Point3f( -0.4499, 6.2600, 1.0000));
cPoint3.push_back(Point3f( -1.5666, 6.5600, 1.0000));
cPoint3.push_back(Point3f( -2.6398, 7.0500, 1.0000));
cPoint3.push_back(Point3f( 0.4815, 9.2300, 1.0000));
cPoint3.push_back(Point3f( 2.3204, 9.8600, 1.0000));
cPoint3.push_back(Point3f( 0.0688, 6.5900, 1.0000));
cPoint3.push_back(Point3f( -0.7212, 3.0200, 1.0000));
cPoint3.push_back(Point3f( -0.9556, 6.4400, 1.0000));
cPoint3.push_back(Point3f( -1.7087, 6.5800, 1.0000));
cPoint3.push_back(Point3f( -2.6555, 7.0700, 1.0000));
cPoint3.push_back(Point3f( -1.6653, 8.5000, 1.0000));
rPoint3.push_back(Point3f( -0.2969, 6.0781, 1.0000));
rPoint3.push_back(Point3f( -0.7891, 6.0156, 1.0000));
rPoint3.push_back(Point3f( -1.7813, 6.2969, 1.0000));
rPoint3.push_back(Point3f( -2.5078, 6.7422, 1.0000));
rPoint3.push_back(Point3f( 0.1250, 8.7344, 1.0000));
rPoint3.push_back(Point3f( 1.7969, 9.2813, 1.0000));
rPoint3.push_back(Point3f( -0.2030, 6.2109, 1.0000));
rPoint3.push_back(Point3f( -0.8281, 3.1719, 1.0000));
rPoint3.push_back(Point3f( -1.0234, 6.2500, 1.0000));
rPoint3.push_back(Point3f( -1.6953, 6.3203, 1.0000));
rPoint3.push_back(Point3f( -2.6563, 6.7188, 1.0000));
rPoint3.push_back(Point3f( -1.7031, 8.3594, 1.0000));
// new method
computMatrix1(rPoint3, cPoint3, mH);
return 0;
}
输出结果:
radar size : 12
【雷达三维点向量】
[-0.2969, 6.0781002, 1;
-0.78909999, 6.0156002, 1;
-1.7812999, 6.2968998, 1;
-2.5078001, 6.7421999, 1;
0.125, 8.7343998, 1;
1.7969, 9.2812996, 1;
-0.20299999, 6.2108998, 1;
-0.82810003, 3.1719, 1;
-1.0233999, 6.25, 1;
-1.6953, 6.3203001, 1;
-2.6563001, 6.7188001, 1;
-1.7031, 8.3593998, 1]
camera size : 12
【相机三维点向量】
[-0.0073000002, 6.2800002, 1;
-0.4499, 6.2600002, 1;
-1.5666, 6.5599999, 1;
-2.6398001, 7.0500002, 1;
0.4815, 9.2299995, 1;
2.3204, 9.8599997, 1;
0.068800002, 6.5900002, 1;
-0.72119999, 3.02, 1;
-0.95560002, 6.4400001, 1;
-1.7086999, 6.5799999, 1;
-2.6554999, 7.0700002, 1;
-1.6653, 8.5, 1]
【相机三维点向量】
Eigen Vector U :
-0.0073
-0.4499
-1.5666
-2.6398
0.4815
2.3204
0.0688
-0.7212
-0.9556
-1.7087
-2.6555
-1.6653
Eigen Vector V :
6.28
6.26
6.56
7.05
9.23
9.86
6.59
3.02
6.44
6.58
7.07
8.5
Eigen Vector I :
1
1
1
1
1
1
1
1
1
1
1
1
【雷达三维点向量】
Eigen Martix Pr :
-0.2969 6.0781 1
-0.7891 6.0156 1
-1.7813 6.2969 1
-2.5078 6.7422 1
0.125 8.7344 1
1.7969 9.2813 1
-0.203 6.2109 1
-0.8281 3.1719 1
-1.0234 6.25 1
-1.6953 6.3203 1
-2.6563 6.7188 1
-1.7031 8.3594 1
转置矩阵
-0.2969 -0.7891 -1.7813 -2.5078 0.125 1.7969 -0.203 -0.8281 -1.0234 -1.6953 -2.6563 -1.7031
6.0781 6.0156 6.2969 6.7422 8.7344 9.2813 6.2109 3.1719 6.25 6.3203 6.7188 8.3594
1 1 1 1 1 1 1 1 1 1 1 1
求逆矩阵
0.0668061 -0.0175856 0.18187
-0.0175855 0.0408547 -0.289921
0.18187 -0.289921 2.19572
Output 1 :
1.13441
0.00581865
0.262563
Output 2 :
0.020811
1.08805
-0.296611
mH_lsm : []