KITTI 数据集解析、下载、融合
1、Rawdata数据集下载
数据集下载地址
Download - DL数据集/KITTI-raw | 格物钛，非结构化数据平台
2、硬件配置
3、标定文件说明
-
calib_cam_to_cam
S_00: 1.392000e+03 5.120000e+02
K_00: 9.842439e+02 0.000000e+00 6.900000e+02 0.000000e+00 9.808141e+02 2.331966e+02 0.000000e+00 0.000000e+00 1.000000e+00
D_00: -3.728755e-01 2.037299e-01 2.219027e-03 1.383707e-03 -7.233722e-02
R_00: 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00
T_00: 2.573699e-16 -1.059758e-16 1.614870e-16
S_rect_00: 1.242000e+03 3.750000e+02
R_rect_00: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01
P_rect_00: 7.215377e+02 0.000000e+00 6.095593e+02 0.000000e+00 0.000000e+00 7.215377e+02 1.728540e+02 0.000000e+00 0.000000e+00 0.000000e+00 1.000000e+00 0.000000e+00
S————原始照片尺寸
K————相机内参（3x3）
D————校正矩阵（1x5）
R————到相机0的旋转矩阵
T————到相机0的平移矩阵
S_rect————纠正后的照片尺寸
R_rect————纠正后的旋转矩阵
P_rect————纠正后的投影矩阵
-
calib_velo_to_cam
R: 7.533745e-03 -9.999714e-01 -6.166020e-04 1.480249e-02 7.280733e-04 -9.998902e-01 9.998621e-01 7.523790e-03 1.480755e-02
T: -4.069766e-03 -7.631618e-02 -2.717806e-01
delta_f: 0.000000e+00 0.000000e+00
delta_c: 0.000000e+00 0.000000e+00
R————旋转矩阵
T————平移矩阵
-
calib_imu_to_velo
R: 9.999976e-01 7.553071e-04 -2.035826e-03 -7.854027e-04 9.998898e-01 -1.482298e-02 2.024406e-03 1.482454e-02 9.998881e-01
T: -8.086759e-01 3.195559e-01 -7.997231e-014、激光图像融合
//
// Created by cai on 2021/8/26.
//
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
//#include "fssim_common/Cmd.h"
#include
// 稠密矩阵的代数运算（逆，特征值等）
#include
#include
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace Eigen;
using namespace cv;
using namespace std;
#include "colored_pointcloud/colored_pointcloud.h"
#include
#include
cv::Mat P_rect_00(3,4,cv::DataType::type);//3×4 projection matrix after rectification
cv::Mat R_rect_00(4,4,cv::DataType::type);//3×3 rectifying rotation to make image planes co-planar
cv::Mat RT(4,4,cv::DataType::type);//rotation matrix and translation vector
class RsCamFusion
{
//**********************************************************************************************************
//1、定义成员变量
private:
typedef message_filters::sync_policies::ApproximateTime slamSyncPolicy;
message_filters::Synchronizer* sync_;
message_filters::Subscriber* camera_sub1;
message_filters::Subscriber* lidar_sub;
pcl::PointCloud::Ptr colored_cloud_toshow;
pcl::PointCloud::Ptr input_cloud;
pcl::PointCloud::Ptr input_cloud_ptr;
pcl::PointCloud::Ptr raw_cloud;
cv::Mat input_image;
cv::Mat image_to_show,image_to_show1;
int frame_count = 0;
static cv::Size imageSize;
static ros::Publisher pub;
//store calibration data in Opencv matrices
image_transport::Publisher depth_pub ;
sensor_msgs::ImagePtr depth_msg;
ros::NodeHandle nh;
ros::Publisher colored_cloud_showpub;
ros::Subscriber sub;
ros::Publisher fused_image_pub1;
public:
//构造函数
RsCamFusion():
nh("~"){
RT.at(0,0) = 7.533745e-03;RT.at(0,1) = -9.999714e-01;RT.at(0,2) = -6.166020e-04;RT.at(0,2) = -4.069766e-03;
RT.at(1,0) = 1.480249e-02;RT.at(1,1) = 7.280733e-04;RT.at(1,2) = -9.998902e-01;RT.at(1,3) = -7.631618e-02;
RT.at(2,0) = 9.998621e-01;RT.at(2,1) = 7.523790e-03;RT.at(2,2) = 1.480755e-02;RT.at(2,3) = -2.717806e-01;
RT.at(3,0) = 0.0;RT.at(3,1) = 0.0;RT.at(3,2) = 0.0;RT.at(3,3) = 1.0;
R_rect_00.at(0,0) = 9.999239e-01;R_rect_00.at(0,1) = 9.837760e-03;R_rect_00.at(0,2) = -7.445048e-03;R_rect_00.at(0,3) = 0.0;
R_rect_00.at(1,0) = -9.869795e-03;R_rect_00.at(1,1) = 9.999421e-01;R_rect_00.at(1,2) = -4.278459e-03;R_rect_00.at(1,3) = 0.0;
R_rect_00.at(2,0) = 7.402527e-03;R_rect_00.at(2,1) = 4.351614e-03;R_rect_00.at(2,2) = 9.999631e-01;R_rect_00.at(2,3) = 0.0;
R_rect_00.at(3,0) = 0.0;R_rect_00.at(3,1) = 0.0;R_rect_00.at(3,2) = 0.0;R_rect_00.at(3,3) = 1.0;
P_rect_00.at(0,0) = 7.215377e+02;P_rect_00.at(0,1) = 0.000000e+00;P_rect_00.at(0,2) = 6.095593e+02;P_rect_00.at(0,3) = 0.000000e+00;
P_rect_00.at(1,0) = 0.000000e+00;P_rect_00.at(1,1) = 7.215377e+02;P_rect_00.at(1,2) = 1.728540e+02;P_rect_00.at(1,3) = 0.000000e+00;
P_rect_00.at(2,0) = 0.000000e+00;P_rect_00.at(2,1) = 0.000000e+00;P_rect_00.at(2,2) = 1.000000e+00;P_rect_00.at(2,3) = 0.000000e+00;
camera_sub1 = new message_filters::Subscriber(nh, "/forward",300);
lidar_sub = new message_filters::Subscriber(nh, "/kitti/velo/pointcloud",100);
sync_ = new message_filters::Synchronizer(slamSyncPolicy(100), *camera_sub1,*lidar_sub);
sync_->registerCallback(boost::bind(&RsCamFusion::callback,this, _1, _2));
cout<<"waite_image"<());
colored_cloud_toshow.reset(new pcl::PointCloud());
input_cloud.reset(new pcl::PointCloud());
input_cloud_ptr.reset(new pcl::PointCloud());
}
void resetParameters(){
raw_cloud->clear();
input_cloud_ptr->clear();
input_cloud->clear();
colored_cloud_toshow->clear();
}
void callback(const sensor_msgs::ImageConstPtr input_image_msg1,
const sensor_msgs::PointCloud2ConstPtr input_cloud_msg)
{
resetParameters();
cv::Mat input_image1;
cv_bridge::CvImagePtr cv_ptr1;
std_msgs::Header image_header1 = input_image_msg1->header;
std_msgs::Header cloud_header = input_cloud_msg->header;
//数据获取
//图像ROS消息转化
cv_ptr1 = cv_bridge::toCvCopy(input_image_msg1,sensor_msgs::image_encodings::BGR8);
input_image1 = cv_ptr1->image;
//获取点云
pcl::fromROSMsg(*input_cloud_msg, *input_cloud_ptr);//把input_cloud_msg放
std::vector indices;
pcl::removeNaNFromPointCloud(*input_cloud_ptr, *input_cloud_ptr, indices);//去除无效点
transformpoint(input_cloud_ptr,input_image1,P_rect_00,R_rect_00,RT);
colored_cloud_showpub = nh.advertise("colored_cloud_toshow",10);
publishCloudtoShow(colored_cloud_showpub, cloud_header, colored_cloud_toshow);
fused_image_pub1 = nh.advertise("image_to_show",10);
publishImage(fused_image_pub1, image_header1, image_to_show1);
frame_count = frame_count + 1;
cout<<"*************************************************"<::ConstPtr& input_cloud, const cv::Mat input_image, cv::Mat &P_rect_00,cv::Mat &R_rect_00,cv::Mat &RT)
{
cv::Mat X(4,1,cv::DataType::type);
cv::Mat Y(4,1,cv::DataType::type);
cv::Point pt;
std::vector rawPoints;
*raw_cloud = *input_cloud;
image_to_show = input_image.clone();
for(int i=0;isize();i++) {
// convert each 3D point into homogeneous coordinates and store it in a 4D variable X
X.at(0, 0) = raw_cloud->points[i].x;
X.at(1, 0) = raw_cloud->points[i].y;
X.at(2, 0) = raw_cloud->points[i].z;
X.at(3, 0) = 1;
//apply the projection equation to map X onto the image plane of the camera. Store the result in Y
//计算矩阵
Y=P_rect_00*R_rect_00*RT*X;
pt.x=Y.at(0, 0) / Y.at(2, 0);
pt.y=Y.at(1, 0) / Y.at(2, 0);
// transform Y back into Euclidean coordinates and store the result in the variable pt
float d = Y.at(2, 0)*1000.0;
float val = raw_cloud->points[i].x;
float maxVal = 20.0;
int red = min(255, (int) (255 * abs((val - maxVal) / maxVal)));
int green = min(255, (int) (255 * (1 - abs((val - maxVal) / maxVal))));
if(pt.x<1240 &&pt.x>0 &&pt.y<375 &&pt.y>0 &&d>0)
{
cv::circle(image_to_show, pt, 1, cv::Scalar(0, green, red), cv::FILLED);
image_to_show1 = image_to_show;
pcl::PointXYZRGB point;
point.x = raw_cloud->points[i].x;
point.y = raw_cloud->points[i].y; //to create colored point clouds
point.z = raw_cloud->points[i].z;
//point.intensity = raw_cloud->points[i].intensity; PointXYZRGB颜色会正常，自定义点云颜色不正常
point.g = input_image.at(pt.y, pt.x)[1];
point.b = input_image.at(pt.y, pt.x)[0];
point.r = input_image.at(pt.y, pt.x)[2];
colored_cloud_toshow->points.push_back(point);
}
}
}
void publishCloudtoShow(const ros::Publisher& cloudtoshow_pub, const std_msgs::Header& header,
const pcl::PointCloud::ConstPtr& cloud)
{
sensor_msgs::PointCloud2 output_msg;
pcl::toROSMsg(*cloud, output_msg);
output_msg.header = header;
cloudtoshow_pub.publish(output_msg);
}
void publishImage(const ros::Publisher& image_pub, const std_msgs::Header& header, const cv::Mat image)
{
cv_bridge::CvImage output_image;
output_image.header = header;
output_image.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
output_image.image = image;
image_pub.publish(output_image);
}
};
//*****************************************************************************************************
//
// 程序入口
//
//×××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××××
int main(int argc, char** argv)
{
//1、节点初始化 及定义参数
ros::init(argc, argv, "kitti3D2");
RsCamFusion RF;
ros::spin();
return 0;
}
cmake_minimum_required(VERSION 2.8.3)
project(kittivelo_cam)
add_compile_options(-std=c++11)
set(CMAKE_BUILD_TYPE Release)
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
message_filters
roscpp
rospy
std_msgs
tf
pcl_conversions
tf2
tf2_ros
tf2_geometry_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# opencv
find_package(OpenCV 3 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# pcl
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
# boost
find_package(Boost REQUIRED)
include_directories(${Boost_INCLUDE_DIRS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
add_executable(kittivelo_cam src/kittivelo_cam.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(kittivelo_cam ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES})