将点云坐标转换成世界坐标的demo

pcl_calib.cfg

#!/usr/bin/env python
PACKAGE = "depth_camera_calib"
NODENAME="depth_camera_calib"
from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()
# gen.add("int_param",    int_t,    0, "An Integer parameter", 50,  0, 100)
# gen.add("double_param", double_t, 0, "A double parameter",    .5, 0,   1)
# gen.add("str_param",    str_t,    0, "A string parameter",  "Hello World")
# gen.add("bool_param",   bool_t,   0, "A Boolean parameter",  True)
#
# # size_enum = gen.enum([ gen.const("Small",      int_t, 0, "A small constant"),
#                       gen.const("Medium",     int_t, 1, "A medium constant"),
#                       gen.const("Large",      int_t, 2, "A large constant"),
#                       gen.const("ExtraLarge", int_t, 3, "An extra large constant")], "An enum to set size")
#
# gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum)
#

gen.add("x_min",		double_t, 0, "x_min", -0.1, -0.5, 0.5)
gen.add("x_max",		double_t, 0, "x_max", 0.1, -0.5, 0.5)
gen.add("y_min",		double_t, 0, "y_min", -0.1, -0.5, 0.5)
gen.add("y_max",		double_t, 0, "y_max", 0.1, -0.5, 0.5)
gen.add("z_min",		double_t, 0, "z_min", 0.7, 0, 2)
gen.add("z_max",		double_t, 0, "z_max", 2, 0, 2)
gen.add("calib_xy_1",   bool_t,  0, "calib_xy_1",  False)
gen.add("calib_xy_2",   bool_t,  0, "calib_xy_2",  False)
gen.add("calib_z_1",   bool_t,  0, "calib_z_1",  False)
gen.add("calib_z_2",   bool_t,  0, "calib_z_2",  False)
gen.add("reset_calib",   bool_t,  0, "reset_calib",  False)
gen.add("prt_current_high_point_after_calib",   bool_t,  0, "prt_current_high_point",  False)

exit(gen.generate(PACKAGE, NODENAME, "pclCalib"))

pcl_calib_config.yaml

#red
world_x_1: "-15"
world_y_1: "25"

world_x_2: "15"
world_y_2: "5"

world_z_1: "0.9"
world_z_2: "28"
input_channel: /camera/depth_registered/points
output_channel: /filtered

cal_depth_camera_world.py

//
// Created by hao on 2022/5/21.
//
#include <ros/ros.h>
// PCL specific includes 。PCL 的相关的头文件
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
//滤波的头文件
#include <pcl/filters/voxel_grid.h>
#include <dynamic_reconfigure/server.h>

#include "depth_camera_calib/pclCalibConfig.h"
#include<cmath>
#include "tf_cloud_to_world.h"


//申明发布器
ros::Publisher pub;

double x_max=100;
double x_min=-100;
double y_max=100;
double y_min=-100;
double z_max=100;
double z_min=-100;


bool calib_xy_1= false;
bool calib_xy_2= false;

bool calib_z_1= false;
bool calib_z_2= false;



std::vector<double> cal_1_x;
std::vector<double> cal_1_y;
std::vector<double> cal_1_z;
std::vector<double> cal_2_x;
std::vector<double> cal_2_y;
std::vector<double> cal_2_z;

bool calib_xy_1_done= false;
bool calib_xy_2_done= false;
bool calib_z_1_done= false;
bool calib_z_2_done= false;
bool load_calib_txt_done= false;

const std::string CALIB_ID_XY_1="CALIB_ID_XY_1";
const std::string CALIB_ID_XY_2="CALIB_ID_XY_2";
const std::string CALIB_ID_Z_1="CALIB_ID_Z_1";
const std::string CALIB_ID_Z_2="CALIB_ID_Z_2";
const std::string CALIB_ID_NONE="CALIB_ID_NONE";

bool load_calib_txt= false;

bool prt_current_high_point_after_calib=false;

void do_prt_current_high_point(std::vector<pcl::PointXYZRGBA>* keep_points){
    double _x=0;
    double _y=0;
    double _z=100;

    for (auto &point:*keep_points){
//        选择距离相机最近的点,就是z最小的点
        if (point.z<_z){
            _x=point.x;
            _y=point.y;
            _z=point.z;
        }
    }
    double world_x=0;
    double world_y=0;
    double world_z=0;

    tf_cloud_to_world tf;
    tf.cloud_to_world(_x,_y,_z,world_x,world_y,world_z);

    ROS_INFO("highest point: world coordinate: %f %f %f",world_x,world_y,world_z);
    sleep(1);

}



void write_txt(const std::string& calib_id,int data_size)
{
    double sum_x=0;
    double sum_y=0;
    double sum_z=0;
    double avg_x=0;
    double avg_y=0;
    double avg_z=0;

    if (calib_id==CALIB_ID_XY_1){
        for (auto &value:cal_1_x)
            sum_x+=value;
        for (auto &value:cal_1_y)
            sum_y+=value;

        avg_x= round(sum_x/data_size*10000)/10000;
        avg_y= round(sum_y/data_size*10000)/10000;
        avg_z=-100;
    }
    else if (calib_id==CALIB_ID_XY_2){
        for (auto &value:cal_2_x)
            sum_x+=value;
        for (auto &value:cal_2_y)
            sum_y+=value;

        avg_x= round(sum_x/data_size*10000)/10000;
        avg_y= round(sum_y/data_size*10000)/10000;
        avg_z=-100;
    }

    else if (calib_id==CALIB_ID_Z_1){
        for (auto &value:cal_1_z)
            sum_z+=value;

        avg_x= -100;
        avg_y= -100;
        avg_z= round(sum_z/data_size*10000)/10000;
    }
    else if (calib_id==CALIB_ID_Z_2){
        for (auto &value:cal_2_z)
            sum_z+=value;

        avg_x= -100;
        avg_y= -100;
        avg_z= round(sum_z/data_size*10000)/10000;
    }
    else {
        avg_x= -100;
        avg_y= -100;
        avg_z=-100;
    }

    try {

        //世界坐标从档案里读取
        std::ofstream outfile("/home/hao/Documents/depth_camera_calib.txt", std::ios::out|std::ios::app);
        std::string point1_world_x;
        std::string point1_world_y;
        std::string point1_world_z;
        ros::param::get("world_x_1",point1_world_x);
        ros::param::get("world_y_1",point1_world_y);
        ros::param::get("world_z_1",point1_world_z);
        std::string point2_world_x;
        std::string point2_world_y;
        std::string point2_world_z;
        ros::param::get("world_x_2",point2_world_x);
        ros::param::get("world_y_2",point2_world_y);
        ros::param::get("world_z_2",point2_world_z);

        if (calib_id==CALIB_ID_XY_1) {
            outfile <<"cloud_x_1:" << avg_x << std::endl;
            outfile <<"cloud_y_1:" << avg_y << std::endl;
            outfile <<"world_x_1:" << point1_world_x << std::endl;
            outfile <<"world_y_1:" << point1_world_y << std::endl;
            calib_xy_1_done= true;
        }
        else if (calib_id==CALIB_ID_XY_2) {
            outfile <<"cloud_x_2:" << avg_x << std::endl;
            outfile <<"cloud_y_2:" << avg_y << std::endl;
            outfile <<"world_x_2:" << point2_world_x << std::endl;
            outfile <<"world_y_2:" << point2_world_y << std::endl;
            calib_xy_2_done= true;
        }
        else if (calib_id==CALIB_ID_Z_1) {
            outfile <<"cloud_z_1:" << avg_z << std::endl;
            outfile <<"world_z_1:" << point1_world_z << std::endl;
            calib_z_1_done= true;
        }
        else if (calib_id==CALIB_ID_Z_2) {
            outfile <<"cloud_z_2:" << avg_z << std::endl;
            outfile <<"world_z_2:" << point2_world_z << std::endl;
            calib_z_2_done= true;
        }
        outfile.close();
    }catch(...)
    {
        std::cout<<"write txt error"<<std::endl;
    }
}


bool add_vector(const std::string& calib_id,std::vector<pcl::PointXYZRGBA>* keep_points,int data_size){

    double _x=0;
    double _y=0;
    double _z=100;

    for (auto &point:*keep_points){
//        选择距离相机最近的点,就是z最小的点
          if (point.z<_z){
            _x=point.x;
            _y=point.y;
            _z=point.z;
          }
    }

    if (calib_id==CALIB_ID_XY_1 && cal_1_x.size()<(data_size+1)){
        cal_1_x.push_back(_x);
        cal_1_y.push_back(_y);
    }
    else if (calib_id==CALIB_ID_XY_2 && cal_2_x.size()<(data_size+1)){
        cal_2_x.push_back(_x);
        cal_2_y.push_back(_y);
    }
    else if (calib_id==CALIB_ID_Z_1 && cal_1_z.size()<(data_size+1)){
        cal_1_z.push_back(_z);
    }
    else if (calib_id==CALIB_ID_Z_2 && cal_2_z.size()<(data_size+1)){
        cal_2_z.push_back(_z);
    }





    if ((calib_id==CALIB_ID_XY_1 && cal_1_x.size()==data_size) ||
        (calib_id==CALIB_ID_XY_2 && cal_2_x.size()==data_size)||
        (calib_id==CALIB_ID_Z_1 && cal_1_z.size()==data_size)||
        (calib_id==CALIB_ID_Z_2 && cal_2_z.size()==data_size))

    {
        write_txt(calib_id,data_size);
    }



    return true;
}





void do_calib(std::vector<pcl::PointXYZRGBA>* keep_points,int data_size)
{
    std::string calib_id;

    if (calib_xy_1 && !calib_xy_2 && !calib_z_1 && !calib_z_2)
        calib_id=CALIB_ID_XY_1;
    else if(!calib_xy_1 && calib_xy_2 && !calib_z_1 && !calib_z_2)
        calib_id=CALIB_ID_XY_2;
    else if(!calib_xy_1 && !calib_xy_2 && calib_z_1 && !calib_z_2)
        calib_id=CALIB_ID_Z_1;
    else if(!calib_xy_1 && !calib_xy_2 && !calib_z_1 && calib_z_2)
        calib_id=CALIB_ID_Z_2;
    else
        calib_id=CALIB_ID_NONE;

    if ((calib_id == CALIB_ID_NONE) ||
    (calib_id == CALIB_ID_XY_1 && calib_xy_1_done) ||
    (calib_id == CALIB_ID_XY_2 && calib_xy_2_done) ||
    (calib_id == CALIB_ID_Z_1 && calib_z_1_done) ||
    (calib_id == CALIB_ID_Z_2 && calib_z_2_done) )
        { return; }
    else {add_vector(calib_id,keep_points,data_size);}






}






void dynamic_cb(depth_camera_calib::pclCalibConfig &config, uint32_t level) {
//    ROS_INFO("Reconfigure Request: x_max:%f x_min:%f y_max:%f y_min:%f  z_max:%f z_min:%f calib_xy_1:%s calib_xy_2:%s calib_z_1:%s calib_z_2:%s reset_calib:%s",
//             config.x_max,
//             config.x_min,
//             config.y_max,
//             config.y_min,
//             config.z_max,
//             config.z_min,
//
//             config.calib_xy_1?"True":"False",
//             config.calib_xy_2?"True":"False",
//             config.calib_z_1?"True":"False",
//             config.calib_z_2?"True":"False",
//             config.reset_calib?"True":"False"
//             );

    x_max=config.x_max;
    x_min=config.x_min;
    y_max=config.y_max;
    y_min=config.y_min;
    z_max=config.z_max;
    z_min=config.z_min;

    calib_xy_1=config.calib_xy_1;
    calib_xy_2=config.calib_xy_2;
    calib_z_1=config.calib_z_1;
    calib_z_2=config.calib_z_2;

//    load_calib_txt=config.load_calib_txt;
    prt_current_high_point_after_calib=config.prt_current_high_point_after_calib;

    if (config.reset_calib) {
        ROS_INFO("Reconfigure Request: reset all");

        calib_xy_1_done = false;
        calib_xy_2_done= false;
        calib_z_1_done= false;
        calib_z_2_done= false;
        cal_1_x.clear();
        cal_1_y.clear();
        cal_1_z.clear();
        cal_2_x.clear();
        cal_2_y.clear();
        cal_2_z.clear();


    }


}


//回调函数
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
//    std::string frame_id=cloud_msg->head->frame_id;
    auto* cloud = new pcl::PCLPointCloud2; //原始的点云的数据格式
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl_conversions::toPCL(*cloud_msg, *cloud);

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_points(new pcl::PointCloud<pcl::PointXYZRGBA>());

    pcl::fromPCLPointCloud2(*cloud, *cloud_points);

    auto *keep_points=new std::vector<pcl::PointXYZRGBA>();
    for (auto &point:*cloud_points){
        if (point.x <= x_max && point.x >= x_min && point.y <= y_max && point.y >= y_min && point.z <= z_max && point.z >= z_min) {
            keep_points->push_back(point);
        }
    }


    auto* filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>());

    filtered_cloud->width=keep_points->size();
    filtered_cloud->height=1;
    filtered_cloud->resize(filtered_cloud->width*filtered_cloud->height);

    for (size_t i = 0; i < keep_points->size(); ++i)   //循环填充点云数据
    {
        filtered_cloud->points[i]=(*keep_points)[i];

    }

    pcl::PCLPointCloud2 filtered_cloud2;
    pcl::toPCLPointCloud2 (*filtered_cloud, filtered_cloud2);

    sensor_msgs::PointCloud2 senser_msg_cloud2;//声明的输出的点云的格式

    pcl_conversions::moveFromPCL(filtered_cloud2, senser_msg_cloud2);//第一个参数是输入，后面的是输出

    senser_msg_cloud2.header.frame_id="camera_depth_optical_frame";

    pub.publish (senser_msg_cloud2);


    do_calib(keep_points,5);

//    if (load_calib_txt && !load_calib_txt_done){
//
//    }


    if(prt_current_high_point_after_calib){
        do_prt_current_high_point(keep_points);
    }


}



int main (int argc, char** argv){

    ros::init(argc, argv, "depth_camera_calib");
    ros::NodeHandle nh;



    dynamic_reconfigure::Server<depth_camera_calib::pclCalibConfig> server;
    dynamic_reconfigure::Server<depth_camera_calib::pclCalibConfig>::CallbackType f;
    f = boost::bind(&dynamic_cb, _1, _2);
    server.setCallback(f);

    std::string input_channel;
    std::string output_channel;
    ros::param::get("input_channel",input_channel);
    ros::param::get("output_channel",output_channel);

//    std::cout<<"input_channel:"<<input_channel<<std::endl;
//    std::cout<<"output_channel:"<<output_channel<<std::endl;

    //设定点云call back
    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> (input_channel, 1, cloud_cb);

    //设定点云处理后的 publisher
    pub = nh.advertise<sensor_msgs::PointCloud2> (output_channel, 1);

    ROS_INFO("Spinning node");
    ros::spin();
    return 0;



}