目录
- 思路
- 源码一
- 源码二
- 效果
思路
16线激光雷达与图像融合时容易出现点云稀疏的问题，因此需要将点云密集化处理。
思路：将点云转入初始坐标系进行下采样（降低线密度），然后转入雷达坐标系，并根据图像大小保留图像范围内的点云（这样做的目的是根据当前点云位姿保留下当前视野范围内的点云），然后将能用于下一帧点云的历史点云保存下来，用于与下一帧点云拼接。
源码一
通过message_filters同步话题
（发现运行较慢）
//*************************************************************************************************
// 获取密集点云，迭代发布：cloud_dense
// source ~/catkin_ws/devel/setup.bash && rosrun my_lidar_cam_fusion cloud_dense
//
//****************************************************************************************************
#include "ros/ros.h"
#include <math.h>
#include <iostream>
#include <cmath>
#include "std_msgs/String.h"
#include <boost/thread.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <geometry_msgs/TransformStamped.h>
#include "tf/transform_datatypes.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#define PI 3.1415926
#define rQ 40 //雷达旋转角度
#define TX 0.18//0.14 //平移0.18
#define TY 0
#define TZ 0.24//0.30 //0.24
using namespace sensor_msgs;
using namespace std;
using namespace cv;
class cloud_dense
{
public:
cloud_dense()
{
cout<< "获取密集点云，迭代发布：cloud_dense "<<endl;
k=true;
cloud_pub = n.advertise<sensor_msgs::PointCloud2>("cloud_cut", 1);//拼接后的点云发布
imu_pub = n.advertise<sensor_msgs::Imu>("imu_RT", 1);//发布RT
//image_pub = n.advertise<sensor_msgs::Image>("image_raw", 1);
//sub = n.subscribe<sensor_msgs::Image>("/image_raw", 30, &ThreadListener::myCallback5,this,ros::TransportHints().tcpNoDelay());
sub1.subscribe(n,"points_raw",10);//订阅激光雷达
sub2.subscribe(n,"/gps_raw",10);
sub3.subscribe(n,"/quat_raw",100);
sync.connectInput( sub1, sub2, sub3 );
sync.registerCallback( boost::bind(&cloud_dense::chatterCallback, this,_1, _2, _3 ) );
}
void chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pc2,const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg);
void Down_sampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z);
void lidar_to_imu(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg);
void cloud_to_Initial_coord(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud );
void Initial_to_lidar_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar );
void Cloud_Cut(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut);
void Effective_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut);
void Publishe_msg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut );
private:
ros::NodeHandle n;
message_filters::Subscriber<sensor_msgs::PointCloud2> sub1;
message_filters::Subscriber<sbg_driver::SbgEkfNav> sub2;
message_filters::Subscriber<sbg_driver::SbgEkfQuat> sub3;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sbg_driver::SbgEkfNav,sbg_driver::SbgEkfQuat> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> sync{MySyncPolicy(2000)};
ros::Publisher cloud_pub;
ros::Publisher imu_pub;
//ros::Publisher image_pub;
//ros::Subscriber sub;
bool k;//调试
double gx,gy,gz ;//转到初始坐标系的平移基准
Eigen::Matrix4d r_y;//4x4矩阵绕Y轴
Eigen::Matrix4d r_lidartoimu;//4x4矩阵雷达到贯导
Eigen::Matrix4d lidartoimu;
Eigen::Matrix4d l2i_ni;
Eigen::Matrix4d T_matrix;//4x4旋转矩阵赋值
Eigen::Matrix4d T_ni;
pcl::PointCloud<pcl::PointXYZ> cloud_new1;
ros::Time time;
cv::Mat M;
cv::Mat RT;
int row; //行
int col;
};
void cloud_dense::Down_sampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_tmp);
sor.setLeafSize(0.15f, 0.15f, 0.15f); //体素滤波 20cm
sor.filter(*cloud);
}
// 把经纬坐标投影到Web墨卡托
void cloud_dense::GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z)
{
*x=(destLon-srcLon)*111000*cos(srcLat/180*PI);
*y=(destLat-srcLat)*111000;
*z=tz-gz;
}
void cloud_dense::lidar_to_imu(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg)
{
//cout<<" 将发布密集点云：cloud_lidar "<<endl;
double RQ=rQ*PI/180;
gx = gpsmsg->position.y;//把gps值放到这里
gy = gpsmsg->position.x;
gz = gpsmsg->position.z;
r_y(0,0)=cos(RQ);
r_y(0,1)=0;
r_y(0,2)=sin(RQ);
r_y(0,3)=0;
r_y(1,0)=0;
r_y(1,1)=1;
r_y(1,2)=0;
r_y(1,3)=0;
r_y(2,0)=-sin(RQ);
r_y(2,1)=0;
r_y(2,2)=cos(RQ);
r_y(2,3)=0;
r_y(3,0)=0;
r_y(3,1)=0;
r_y(3,2)=0;
r_y(3,3)=1;
r_lidartoimu(0,0)=0;
r_lidartoimu(0,1)=0;
r_lidartoimu(0,2)=1;
r_lidartoimu(0,3)=TX;
r_lidartoimu(1,0)=0;
r_lidartoimu(1,1)=-1;
r_lidartoimu(1,2)=0;
r_lidartoimu(1,3)=TY;
r_lidartoimu(2,0)=1;
r_lidartoimu(2,1)=0;
r_lidartoimu(2,2)=0;
r_lidartoimu(2,3)=TZ;
r_lidartoimu(3,0)=0;
r_lidartoimu(3,1)=0;
r_lidartoimu(3,2)=0;
r_lidartoimu(3,3)=1;
lidartoimu=r_y*r_lidartoimu;
l2i_ni=lidartoimu.inverse();
M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);
cout << "M=" << M << endl;
RT= (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );
cout << "RT=" << RT << endl;
row = 480; //行
col = 640;
k=false;
}
void cloud_dense::cloud_to_Initial_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud )
{
for (int i = 0; i <cloud->size(); i++)
{
if(3<cloud->points[i].x || cloud->points[i].x<-3)
{
if(-20<cloud->points[i].y && cloud->points[i].y<20)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud->points[i].x;
pointxyz(1,0)=cloud->points[i].y;
pointxyz(2,0)=cloud->points[i].z;
pointxyz(3,0)=1;
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, 1);
uv = M * RT*point;
if (uv.at<double>(2) == 0)
{
cout << "uv.at<double>(2)=0" << endl;
break;
}
double u = uv.at<double>(0) / uv.at<double>(2);
double v = uv.at<double>(1) / uv.at<double>(2);
int px = int(u + 0.5);
int py = int(v + 0.5);
//cout << "(u,v)=" << px << " "<< py << endl;
//注意：image.at<Vec3b>(row,rol) 但是像素坐标为(x,y)，即(u,v),即(rol,row)
if (0 <= px && px < col && 0 <= py && py < row)
{
Eigen::MatrixXd n=T_matrix*lidartoimu*pointxyz; //4X4
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new1.push_back(point_1);
}
}//if
}//if
}//for
//降采样
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
Down_sampling(cloud_new1.makeShared(), cloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar(new pcl::PointCloud<pcl::PointXYZ>);
Initial_to_lidar_coord( cloud2, cloud_lidar );
}
void cloud_dense::Initial_to_lidar_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar )
{
for (int i = 0; i <cloud2->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud2->points[i].x;
pointxyz(1,0)=cloud2->points[i].y;
pointxyz(2,0)=cloud2->points[i].z;
pointxyz(3,0)=1;
Eigen::MatrixXd n=l2i_ni*T_ni*pointxyz;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_lidar->push_back(point_1);
}
//裁剪
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
Cloud_Cut(cloud_lidar,cloud_cut);
//保存有效点用于迭代
Effective_point(cloud_cut);
Publishe_msg( cloud_cut );
}
void cloud_dense::Cloud_Cut(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut)
{
/*
//x轴直通滤波处理
pcl::PassThrough<pcl::PointXYZ> pass2;
pass2.setInputCloud(cloud_lidar);
pass2.setFilterFieldName("x");
if(forget_x<3){ forget_x=3;}
pass2.setFilterLimits(forget_x-1,forget_x+30);
pass2.setFilterLimitsNegative(false);
pass2.filter(*cloud_cut);
*/
for (int i = 0; i <cloud_lidar->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud_lidar->points[i].x;
pointxyz(1,0)=cloud_lidar->points[i].y;
pointxyz(2,0)=cloud_lidar->points[i].z;
pointxyz(3,0)=1;
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << cloud_lidar->points[i].x, cloud_lidar->points[i].y, cloud_lidar->points[i].z, 1);
uv = M * RT*point;
if (uv.at<double>(2) == 0)
{
cout << "uv.at<double>(2)=0" << endl;
break;
}
double u = uv.at<double>(0) / uv.at<double>(2);
double v = uv.at<double>(1) / uv.at<double>(2);
int px = int(u + 0.5);
int py = int(v + 0.5);
//cout << "(u,v)=" << px << " "<< py << endl;
//注意：image.at<Vec3b>(row,rol) 但是像素坐标为(x,y)，即(u,v),即(rol,row)
if (0 <= px && px < col && 0 <= py && py < row)
{
cloud_cut->push_back(cloud_lidar->points[i]);
}
}
}
void cloud_dense::Effective_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut)
{
pcl::PointCloud<pcl::PointXYZ> cloud_new2;
for (int i = 0; i <cloud_cut->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud_cut->points[i].x;
pointxyz(1,0)=cloud_cut->points[i].y;
pointxyz(2,0)=cloud_cut->points[i].z;
pointxyz(3,0)=1;
Eigen::MatrixXd n=T_matrix*lidartoimu*pointxyz;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new2.push_back(point_1);
}
cloud_new1=cloud_new2;
}
void cloud_dense::Publishe_msg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut )
{
//发布点云
sensor_msgs::PointCloud2 output;//发布点云话题消息
pcl::toROSMsg(*cloud_cut, output);
output.header.stamp = time;
output.header.frame_id = "map";
cloud_pub.publish(output);
}
// 回调函数
void cloud_dense::chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pc2,const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{
time = pc2->header.stamp;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);//点云对象
pcl::fromROSMsg (*pc2, *cloud_tmp);
// 下采样
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
Down_sampling(cloud_tmp, cloud);
// 雷达与惯导间的旋转平移
if(k)
{
lidar_to_imu( gpsmsg );
}
else
{
// 提取出四元数与gps
double x,y,z,w,tx,ty,tz;
x = quatmsg->quaternion.x;//四元数
y = quatmsg->quaternion.y;
z = quatmsg->quaternion.z;
w = quatmsg->quaternion.w;
tx = gpsmsg->position.y;//把gps值放到这里
ty = gpsmsg->position.x;
tz = gpsmsg->position.z;
// 平移计算
double nx,ny,nz;
GetDirectDistance(gx,gy,gz,tx,ty,tz,&nx,&ny,&nz);//计算GPS变化量
// 四元数转换为旋转矩阵
Eigen::Quaterniond quaternion(w,x,y,z);
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
T_matrix(0,0)=rotation_matrix(0,0);
T_matrix(1,0)=rotation_matrix(1,0);
T_matrix(2,0)=rotation_matrix(2,0);
T_matrix(3,0)=0;
T_matrix(0,1)=rotation_matrix(0,1);
T_matrix(1,1)=rotation_matrix(1,1);
T_matrix(2,1)=rotation_matrix(2,1);
T_matrix(3,1)=0;
T_matrix(0,2)=rotation_matrix(0,2);
T_matrix(1,2)=rotation_matrix(1,2);
T_matrix(2,2)=rotation_matrix(2,2);
T_matrix(3,2)=0;
T_matrix(0,3)=ny;//x->jin y->wei z-> -h
T_matrix(1,3)=nx;
T_matrix(2,3)=-nz;
T_matrix(3,3)=1;
//cout<< T_matrix << endl;
T_ni=T_matrix.inverse();
//点云处理:转到初始坐标系下
cloud_to_Initial_coord(cloud);
//降采样
//pcl::PointCloud<pcl::PointXYZ> cloud2;
//Down_sampling(cloud_new1, cloud2);
//转回lidar系
//pcl::PointCloud<pcl::PointXYZ> cloud_lidar;
//Initial_to_lidar_coord( T_ni, cloud2, cloud_lidar );
//直通滤波处理quatmsg->header.stamp;
//pcl::PointCloud<pcl::PointXYZ> cloud_cut;
//Straight_through_filter(cloud_lidar,cloud_cut);
//将有效点云转回全局坐标系用于迭代
//Effective_point(cloud_cut,T_matrix);
//发布位姿
sensor_msgs::Imu outimu;
outimu.header.stamp = time;
outimu.header.frame_id = "lidar_RT";
outimu.orientation.x=x;
outimu.orientation.y=y;
outimu.orientation.y=y;
outimu.orientation.w=w;
outimu.angular_velocity.x=ny;
outimu.angular_velocity.y=nx;
outimu.angular_velocity.z=-nz;
imu_pub.publish(outimu);
} //else
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cloud_dense");
cloud_dense cf;
ros::MultiThreadedSpinner spinner(3);
spinner.spin();
//ros::spin();
return 0;
}源码二
分别订阅话题，然后自己同步
//*************************************************************************************************
// 获取密集点云，迭代发布：cloud_dense
// source ~/catkin_ws/devel/setup.bash && rosrun my_lidar_cam_fusion cloud_dense_new
//
//****************************************************************************************************
#include "ros/ros.h"
#include <math.h>
#include <iostream>
#include <iomanip>
#include <thread>
#include <mutex>
#include <unistd.h>
#include "std_msgs/String.h"
#include <boost/thread.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <sbg_driver/SbgImuData.h>
#include <sbg_driver/SbgEkfQuat.h>
#include <sbg_driver/SbgGpsPos.h>
#include <sbg_driver/SbgEkfNav.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/time_synchronizer.h>
#include <geometry_msgs/TransformStamped.h>
#include "tf/transform_datatypes.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#define PI 3.1415926
#define QUEUE 2000
#define rQ 40 //雷达旋转角度
#define TX 0.18//0.14 //平移0.18
#define TY 0
#define TZ 0.24//0.30 //0.24
using namespace sensor_msgs;
using namespace std;
using namespace cv;
class cloud_dense
{
public:
cloud_dense()
{
cout<< "获取密集点云，迭代发布：cloud_dense "<<endl;
k=true;
cloud_pub = n.advertise<sensor_msgs::PointCloud2>("cloud_cut", 1);//拼接后的点云发布
imu_pub = n.advertise<sensor_msgs::Imu>("imu_RT", 1);//发布RT
sub1 = n.subscribe<sensor_msgs::PointCloud2>("points_raw", 10, &cloud_dense::myCallback1,this); //,ros::TransportHints().tcpNoDelay());
sub2 = n.subscribe<sbg_driver::SbgEkfNav>("/gps_raw", 1000, &cloud_dense::myCallback2,this );
sub3 = n.subscribe<sbg_driver::SbgEkfQuat>("/quat_raw", 1000, &cloud_dense::myCallback3,this );
}
void myCallback1(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);
void myCallback2(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg);
void myCallback3(const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg);
bool cloud_Queue(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg);
void syn_gps_quat_lidar();
bool get_gps_quat();
void imu_to_word();
//void chatterCallback(const boost::shared_ptr<const sensor_msgs::PointCloud2>& pc2,const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg,const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg);
//降采样
void Down_sampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
void GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z);
void lidar_to_imu(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg);
void cloud_to_Initial_coord(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud );
//转回lidar系
void Initial_to_lidar_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar );
void Cloud_Cut(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut);
//将有效点云转回全局坐标系用于迭代
void Effective_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut);
void Publishe_msg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut );
private:
ros::NodeHandle n;
ros::Subscriber sub1;
ros::Subscriber sub2;
ros::Subscriber sub3;
ros::Publisher cloud_pub;
ros::Publisher imu_pub;
std::mutex gpsLock;
std::mutex quatLock;
std::mutex cloudLock;
std::deque<sbg_driver::SbgEkfNav> gpsQueue;
std::deque<sbg_driver::SbgEkfQuat> quatQueue;
std::deque<sensor_msgs::PointCloud2> cloudQueue;
sensor_msgs::PointCloud2 currentCloudMsg;
bool k;//调试
bool bool_gps;
bool bool_quat;
double gx,gy,gz ;//转到初始坐标系的平移基准
double x,y,z,w,tx,ty,tz;
double nx,ny,nz;// 平移计算
Eigen::Matrix4d r_y;//4x4矩阵绕Y轴
Eigen::Matrix4d r_lidartoimu;//4x4矩阵雷达到贯导
Eigen::Matrix4d lidartoimu;
Eigen::Matrix4d l2i_ni;
Eigen::Matrix4d T_matrix;//4x4旋转矩阵赋值
Eigen::Matrix4d T_ni;
pcl::PointCloud<pcl::PointXYZ> cloud_new1;
ros::Time time;
double currentCloudtime;
double currentQuattime;
cv::Mat M;
cv::Mat RT;
int row; //行
int col;
};
void cloud_dense::myCallback1(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
if (!cloud_Queue(cloudmsg))
return;
if (!get_gps_quat())
return;
syn_gps_quat_lidar();
}
void cloud_dense::myCallback2(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg)
{
sbg_driver::SbgEkfNav thisgps ;
thisgps.header.stamp=gpsmsg->header.stamp;
thisgps.position=gpsmsg->position;
std::lock_guard<std::mutex> lock2(gpsLock);
gpsQueue.push_back(thisgps);
if(k)
{
lidar_to_imu( gpsmsg );
}
}
void cloud_dense::myCallback3(const boost::shared_ptr<const sbg_driver::SbgEkfQuat>& quatmsg)
{
sbg_driver::SbgEkfQuat thisquat;
thisquat.header.stamp = quatmsg->header.stamp;
thisquat.quaternion=quatmsg->quaternion;
std::lock_guard<std::mutex> lock3(quatLock);
quatQueue.push_back(thisquat);
}
void cloud_dense::syn_gps_quat_lidar()
{
//cout<<"currentCloudtime="<<setprecision(16)<<currentCloudtime<<endl;
//cout<<"currentQuattime="<<setprecision(16)<<currentQuattime<<endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp(new pcl::PointCloud<pcl::PointXYZ>);//点云对象
pcl::fromROSMsg (currentCloudMsg, *cloud_tmp);
// 下采样
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
Down_sampling(cloud_tmp, cloud);
GetDirectDistance(gx,gy,gz,tx,ty,tz,&nx,&ny,&nz);//计算GPS变化量
// 四元数转换为旋转矩阵
imu_to_word();
//点云处理:转到初始坐标系下
cloud_to_Initial_coord(cloud);
}
bool cloud_dense::cloud_Queue(const boost::shared_ptr<const sensor_msgs::PointCloud2>& cloudmsg)
{
std::lock_guard<std::mutex> lock1(cloudLock);
cloudQueue.push_back(*cloudmsg);
if (cloudQueue.size() <= 2)
return false;
currentCloudMsg = std::move(cloudQueue.front());
cloudQueue.pop_front();
time = currentCloudMsg.header.stamp;
currentCloudtime= currentCloudMsg.header.stamp.toSec();
return true;
}
//提取出四元数与gps
bool cloud_dense::get_gps_quat()
{
std::lock_guard<std::mutex> lock2(gpsLock);
std::lock_guard<std::mutex> lock3(quatLock);
bool_gps=false;
bool_quat=false;
while (!gpsQueue.empty())
{
if (gpsQueue.front().header.stamp.toSec() < currentCloudtime - 0.01)
gpsQueue.pop_front();
else
break;
}while (!quatQueue.empty())
{
if (quatQueue.front().header.stamp.toSec() < currentCloudtime - 0.01)
quatQueue.pop_front();
else
break;
}
for (int i = 0; i < (int)gpsQueue.size(); ++i)
{
sbg_driver::SbgEkfNav thisGpsMsg = gpsQueue[i];
double currentGpsTime = thisGpsMsg.header.stamp.toSec();
if (currentGpsTime <= currentCloudtime)
{
tx = thisGpsMsg.position.y;//把gps值放到这里
ty = thisGpsMsg.position.x;
tz = thisGpsMsg.position.z;
bool_gps=true;
break;
}
if (currentGpsTime > currentCloudtime + 0.01)
break;
}
for (int i = 0; i < (int)quatQueue.size(); ++i)
{
sbg_driver::SbgEkfQuat thisQuatMsg = quatQueue[i];
double currentQuatTime = thisQuatMsg.header.stamp.toSec();
if (currentQuatTime <= currentCloudtime)
{
x = thisQuatMsg.quaternion.x;//四元数
y = thisQuatMsg.quaternion.y;
z = thisQuatMsg.quaternion.z;
w = thisQuatMsg.quaternion.w;
currentQuattime=thisQuatMsg.header.stamp.toSec();
bool_quat=true;
break;
}
if (currentQuatTime > currentCloudtime + 0.01)
break;
}
if(bool_quat && bool_gps)
return true;
return false;
}
void cloud_dense::imu_to_word()
{
Eigen::Quaterniond quaternion(w,x,y,z);
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
T_matrix(0,0)=rotation_matrix(0,0);
T_matrix(1,0)=rotation_matrix(1,0);
T_matrix(2,0)=rotation_matrix(2,0);
T_matrix(3,0)=0;
T_matrix(0,1)=rotation_matrix(0,1);
T_matrix(1,1)=rotation_matrix(1,1);
T_matrix(2,1)=rotation_matrix(2,1);
T_matrix(3,1)=0;
T_matrix(0,2)=rotation_matrix(0,2);
T_matrix(1,2)=rotation_matrix(1,2);
T_matrix(2,2)=rotation_matrix(2,2);
T_matrix(3,2)=0;
T_matrix(0,3)=ny;//x->jin y->wei z-> -h
T_matrix(1,3)=nx;
T_matrix(2,3)=-nz;
T_matrix(3,3)=1;
//cout<< T_matrix << endl;
T_ni=T_matrix.inverse();
}
//---------------------------------------------------------------------------------------------------------------------------------
void cloud_dense::Down_sampling(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tmp,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_tmp);
sor.setLeafSize(0.15f, 0.15f, 0.15f); //体素滤波 20cm
sor.filter(*cloud);
}
// 把经纬坐标投影到Web墨卡托
void cloud_dense::GetDirectDistance(double srcLon, double srcLat,double gz, double destLon, double destLat,double tz,double *x,double *y,double *z)
{
*x=(destLon-srcLon)*111000*cos(srcLat/180*PI);
*y=(destLat-srcLat)*111000;
*z=tz-gz;
}
void cloud_dense::lidar_to_imu(const boost::shared_ptr<const sbg_driver::SbgEkfNav>& gpsmsg)
{
//cout<<" 将发布密集点云：cloud_lidar "<<endl;
double RQ=rQ*PI/180;
gx = gpsmsg->position.y;//把gps值放到这里
gy = gpsmsg->position.x;
gz = gpsmsg->position.z;
r_y(0,0)=cos(RQ);
r_y(0,1)=0;
r_y(0,2)=sin(RQ);
r_y(0,3)=0;
r_y(1,0)=0;
r_y(1,1)=1;
r_y(1,2)=0;
r_y(1,3)=0;
r_y(2,0)=-sin(RQ);
r_y(2,1)=0;
r_y(2,2)=cos(RQ);
r_y(2,3)=0;
r_y(3,0)=0;
r_y(3,1)=0;
r_y(3,2)=0;
r_y(3,3)=1;
r_lidartoimu(0,0)=0;
r_lidartoimu(0,1)=0;
r_lidartoimu(0,2)=1;
r_lidartoimu(0,3)=TX;
r_lidartoimu(1,0)=0;
r_lidartoimu(1,1)=-1;
r_lidartoimu(1,2)=0;
r_lidartoimu(1,3)=TY;
r_lidartoimu(2,0)=1;
r_lidartoimu(2,1)=0;
r_lidartoimu(2,2)=0;
r_lidartoimu(2,3)=TZ;
r_lidartoimu(3,0)=0;
r_lidartoimu(3,1)=0;
r_lidartoimu(3,2)=0;
r_lidartoimu(3,3)=1;
lidartoimu=r_y*r_lidartoimu;
l2i_ni=lidartoimu.inverse();
M= (Mat_<double>(3, 3) << 1120.793247366556, 0, 404.6240125739656, 0, 1126.958397947255, 248.9162767468943, 0, 0, 1);
cout << "M=" << M << endl;
RT= (Mat_<double>(3, 4) << -0.0209971, - 0.999327, 0.0300679 , 0.01784, - 0.0238557 ,- 0.0295651, - 0.999278, - 0.0963614, 0.999495 ,- 0.0216992, - 0.0232189, 0.00614244 );
cout << "RT=" << RT << endl;
row = 480; //行
col = 640;
k=false;
}
void cloud_dense::cloud_to_Initial_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud )
{
for (int i = 0; i <cloud->size(); i++)
{
if(3<cloud->points[i].x || cloud->points[i].x<-3)
{
if(-20<cloud->points[i].y && cloud->points[i].y<20)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud->points[i].x;
pointxyz(1,0)=cloud->points[i].y;
pointxyz(2,0)=cloud->points[i].z;
pointxyz(3,0)=1;
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, 1);
uv = M * RT*point;
if (uv.at<double>(2) == 0)
{
cout << "uv.at<double>(2)=0" << endl;
break;
}
double u = uv.at<double>(0) / uv.at<double>(2);
double v = uv.at<double>(1) / uv.at<double>(2);
int px = int(u + 0.5);
int py = int(v + 0.5);
//cout << "(u,v)=" << px << " "<< py << endl;
//注意：image.at<Vec3b>(row,rol) 但是像素坐标为(x,y)，即(u,v),即(rol,row)
if (0 <= px && px < col && 0 <= py && py < row)
{
Eigen::MatrixXd n=T_matrix*lidartoimu*pointxyz; //4X4
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new1.push_back(point_1);
}
}//if
}//if
}//for
//降采样
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
Down_sampling(cloud_new1.makeShared(), cloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar(new pcl::PointCloud<pcl::PointXYZ>);
Initial_to_lidar_coord( cloud2, cloud_lidar );
}
void cloud_dense::Initial_to_lidar_coord( pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar )
{
for (int i = 0; i <cloud2->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud2->points[i].x;
pointxyz(1,0)=cloud2->points[i].y;
pointxyz(2,0)=cloud2->points[i].z;
pointxyz(3,0)=1;
Eigen::MatrixXd n=l2i_ni*T_ni*pointxyz;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_lidar->push_back(point_1);
}
//裁剪
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
Cloud_Cut(cloud_lidar,cloud_cut);
//保存有效点用于迭代
Effective_point(cloud_cut);
Publishe_msg( cloud_cut );
}
void cloud_dense::Cloud_Cut(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_lidar,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut)
{
/*
//x轴直通滤波处理
pcl::PassThrough<pcl::PointXYZ> pass2;
pass2.setInputCloud(cloud_lidar);
pass2.setFilterFieldName("x");
if(forget_x<3){ forget_x=3;}
pass2.setFilterLimits(forget_x-1,forget_x+30);
pass2.setFilterLimitsNegative(false);
pass2.filter(*cloud_cut);
*/
for (int i = 0; i <cloud_lidar->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud_lidar->points[i].x;
pointxyz(1,0)=cloud_lidar->points[i].y;
pointxyz(2,0)=cloud_lidar->points[i].z;
pointxyz(3,0)=1;
Mat uv(3, 1, CV_64F);
Mat point = (Mat_<double>(4, 1) << cloud_lidar->points[i].x, cloud_lidar->points[i].y, cloud_lidar->points[i].z, 1);
uv = M * RT*point;
if (uv.at<double>(2) == 0)
{
cout << "uv.at<double>(2)=0" << endl;
break;
}
double u = uv.at<double>(0) / uv.at<double>(2);
double v = uv.at<double>(1) / uv.at<double>(2);
int px = int(u + 0.5);
int py = int(v + 0.5);
//cout << "(u,v)=" << px << " "<< py << endl;
//注意：image.at<Vec3b>(row,rol) 但是像素坐标为(x,y)，即(u,v),即(rol,row)
if (0 <= px && px < col && 0 <= py && py < row)
{
cloud_cut->push_back(cloud_lidar->points[i]);
}
}
}
void cloud_dense::Effective_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut)
{
pcl::PointCloud<pcl::PointXYZ> cloud_new2;
for (int i = 0; i <cloud_cut->size(); i++)
{
Eigen::MatrixXd pointxyz(4,1);//点云坐标
pointxyz(0,0)=cloud_cut->points[i].x;
pointxyz(1,0)=cloud_cut->points[i].y;
pointxyz(2,0)=cloud_cut->points[i].z;
pointxyz(3,0)=1;
Eigen::MatrixXd n=T_matrix*lidartoimu*pointxyz;
pcl::PointXYZ point_1( n(0,0), n(1,0), n(2,0) );
cloud_new2.push_back(point_1);
}
cloud_new1=cloud_new2;
}
void cloud_dense::Publishe_msg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut )
{
//发布点云
sensor_msgs::PointCloud2 output;//发布点云话题消息
pcl::toROSMsg(*cloud_cut, output);
output.header.stamp = time;
output.header.frame_id = "map";
cloud_pub.publish(output);
//发布位姿
sensor_msgs::Imu outimu;
outimu.header.stamp = time;
outimu.header.frame_id = "lidar_RT";
outimu.orientation.x=x;
outimu.orientation.y=y;
outimu.orientation.y=y;
outimu.orientation.w=w;
outimu.angular_velocity.x=ny;
outimu.angular_velocity.y=nx;
outimu.angular_velocity.z=-nz;
imu_pub.publish(outimu);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cloud_dense");
cloud_dense cf;
//ros::MultiThreadedSpinner spinner(3);
//spinner.spin();
ros::spin();
return 0;
}效果
（哈哈，左上角不知道为什么少了一点…）
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
