ros消息同步和Eigen的使用

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 : []