velodyne pointcloud transformation [closed]

asked 2017-11-07 11:40:01 -0500

i_robot_flight gravatar image


I am trying to combine GPS/IMU information with velodyne LiDAR on a DJI drone. I have velodyne pointcloud information being published on a world frame called "gps" using the velodyne,dji-sdk-ros, robot_localization and tf packages. I am trying to manipulate the output cloud to register it from an origin (see attached code). I am not sure about the output data and I am not able to visualize the new clouds. Can someone help me understand if this is the right way to create a map?

void CallBack1(const PointCloud::ConstPtr& msg){        
printf("Cloud Parameters/n");
BOOST_FOREACH(const pcl::PointXYZI& pt, msg->points){
    printf ("(%f, %f, %f, %f)\n", pt.x, pt.y, pt.z, pt.intensity); 
    if(msg->header.frame_id == "gps"){
         counter++ ;
        printf("Counter: %d",counter);
        if(counter == 1){        
                origin_pcl->points = msg->points;

        else if(counter>1){
                PointCloud_Map::Ptr pmap(new PointCloud_Map);
                PointCloud_Map_Total::Ptr pmapt(new PointCloud_Map_Total);

                BOOST_FOREACH(const pcl::PointXYZI& o_pt, origin_pcl->points){
                    pcl_pt.x = pt.x - o_pt.x;
                    pcl_pt.y = pt.y - o_pt.y;
                    pcl_pt.z = pt.z - o_pt.z;
                    pcl_pt.intensity = pt.intensity;


                BOOST_FOREACH(const pcl::PointXYZI& map_pt, pmap->points){
                    pcl_map.x = pcl_map.x + map_pt.x;
                    pcl_map.y = pcl_map.y + map_pt.y;
                    pcl_map.z = pcl_map.z + map_pt.z;
                    pcl_map.intensity = map_pt.intensity;

                pmapt->header.frame_id = "velodyne";    
                pmapt->header.stamp = msg->header.stamp;
                pmapt->height = 1;
                /*pmapt += *pmap;*/


int main(int argc, char** argv){
ros::init(argc, argv, "pcl_test");
ros::NodeHandle pcl_node;
ros::Publisher pubs = pcl_node.advertise<sensor_msgs::PointCloud2> ("transformed_velodyne_points", 10);
ros::Subscriber subs = pcl_node.subscribe("velodyne_points", 10, CallBack1);
ros::Rate loop_r(10);


Thanks for your help, Sneha

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by i_robot_flight
close date 2019-06-28 11:39:01.684203