PCD地图转换

为了得到合适的地图（与GNSS数据融合、地图拼接等），有时需要对生成的三维地图进行转换：平移、翻转等等

void transform_(const std::string& pcd_path)

{

    Eigen::Matrix4d transformation_;

    Eigen::Matrix4d trans;

    Eigen::Vector3d v1(axis_x1,axis_y1,axis_z1);

    Eigen::Vector3d v2(axis_x2,axis_y2,axis_z2);

    double angle = M_PI*(axis_angle/180.0);

    rotate_arbitrary_line(trans,v1,v2,angle);

    transformation_ = trans.transpose();

 

    Eigen::Matrix4d transform_1 = Eigen::Matrix4d::Identity();

    transform_1 (0,3) = t_x;

    transform_1 (1,3) = t_y;

    transform_1 (2,3) = t_z;

    transformation_ = transformation_*transform_1;

    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

    if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_path.c_str(), *source_cloud) == -1)

   {

        return ;

    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());

    pcl::transformPointCloud(*source_cloud, *transformed_cloud, transformation_);

    std::string output = pcd_path.substr(0,pcd_path.size()-4)+"_transformation.pcd";

    if(fout_path != "")

    {

        std::size_t found_idx = pcd_path.find_last_of("/");

        output = fout_path+"transformation_"+pcd_path.substr(found_idx+1);

        std::cout<<output<<std::endl;

    }

    std::cout<<output<<std::endl;

    pcl::io::savePCDFileBinary(output, *transformed_cloud);

}