此代码合并了每一帧的激光点云数据,构成了激光点云地图.
ground truth，即真值，在本文简写成GT。
在kitti数据集中，GT代表了世界坐标系下相机的位姿真值，也就是Tcam2world
经过我的一番推导，
Pworld = Tcam2world * Tlidar2cam * Plidar
（其实不一定对hhh）
最终写成了以下代码

src/sum_liadrCloud.cpp

//合并每一帧的激光点云数据,构成GT激光点云地图.
#include <iostream>
#include <pcl/io/pcd_io.h>  
#include <pcl/point_types.h>  
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>	//	pcl::transformPointCloud 用到这个头文件


#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

string GT_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/groundtruth/07.txt";
string LidarCloud_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/velodyne_points/pcd_data";
string out_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/velodyne_points/out.pcd";



int main(int argc, char *argv[])
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);//世界lidar cloud

    ifstream GT; //读取ground truth的数值,一行12个值,组成3*4矩阵;拼接上0001后得4*4的变换矩阵
    std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> vT_c2w;

    GT.open(GT_path);    
    while (!GT.eof()){
        Eigen::Matrix4d mT_c2w = Eigen::Matrix4d::Zero(); //camera to world ,store ground truth
        string s;
        getline(GT,s);
        double gt11=0,gt12=0,gt13=0,gt14=0,gt21=0,gt22=0,gt23=0,gt24=0,gt31=0,gt32=0,gt33=0,gt34=0;
        if(!s.empty()){
            stringstream ss;
            ss << s;

            ss >> gt11;
            ss >> gt12;
            ss >> gt13;
            ss >> gt14;

            ss >> gt21;
            ss >> gt22;
            ss >> gt23;
            ss >> gt24;

            ss >> gt31;
            ss >> gt32;
            ss >> gt33;
            ss >> gt34;

            mT_c2w << gt11, gt12, gt13, gt14, gt21, gt22, gt23, gt24, gt31, gt32, gt33, gt34, 0, 0, 0, 1;

        }
        vT_c2w.push_back(mT_c2w);

    }
        // for (int i=0; i<4; ++i){
        //     for (int j=0; j<4; ++j){
        //         cout << vT_c2w[1100](i,j) << "\t";
        //     }
        //     cout << endl;
        // }
    // cout << vT_c2w.size() << endl;

    Eigen::Matrix4d mT_l2c = Eigen::Matrix4d::Zero(); //T_l2c矩阵,lidar to camera的变换矩阵,外参
    mT_l2c << 7.027555e-03, -9.999753e-01, 2.599616e-05, -7.137748e-03,
             -2.254837e-03, -4.184312e-05, -9.999975e-01, -7.482656e-02,
             9.999728e-01, 7.027479e-03, -2.255075e-03, -3.336324e-01,
             0,0,0,1;
    
    // Eigen::Matrix4d mT = Eigen::Matrix4d::Zero();
    // mT = vT_c2w[1] * mT_l2c;

    for (int k=1; k<vT_c2w.size(); ++k){ //遍历每一个pcd文件,对其做变换,转到世界坐标系下.
        string LidarCloud_path_k = LidarCloud_path + "/" + to_string(k) + ".pcd";
        
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloud
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k_transformed(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloud
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());
        //  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);

        if (pcl::io::loadPCDFile<pcl::PointXYZI>(LidarCloud_path_k, *cloud_k) == -1)
        {
            PCL_ERROR("Couldn't read file k.pcd\n");
            return (-1);
        }
        cout << "cloud " << k << " is loaded !" << endl;

        Eigen::Matrix4d mT_final = vT_c2w[k] * mT_l2c;
        Eigen::Affine3d aT_final(mT_final);
        std::cout << aT_final.matrix() << std::endl;
        pcl::transformPointCloud (*cloud_k, *cloud_k_transformed, aT_final);

        pcl::VoxelGrid<pcl::PointXYZI> sor;   // 创建滤波器对象
        sor.setInputCloud (cloud_k_transformed);   //给滤波器对象设置需要过滤的点云
        sor.setLeafSize (0.10f, 0.10f, 0.10f); //设置滤波时创建的体素大小为1cm立方体
        sor.filter (*cloud_filtered); //执行滤波处理

        *cloud = *cloud + *cloud_filtered;

        
    }
    pcl::PCDWriter writer;
    // Save DoN features
    writer.write<pcl::PointXYZI> (out_path, *cloud, false);
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

set(CMAKE_BUILD_TYPE "Release")
# 添加c++ 11标准支持
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")

# Eigen
include_directories("/usr/include/eigen3")

find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})

add_executable (sum_liadrCloud src/sum_liadrCloud.cpp) 
target_link_libraries(sum_liadrCloud ${PCL_LIBRARIES})

就这样~有问题可在评论区留言