velodyne pointcloud transformation [closed]

edit

Hello,

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;
                } 

                pmap->points.push_back(pcl_pt);    

                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->points.push_back(pcl_map);            
                /*pmapt += *pmap;*/
                toROSMsg(*pmapt,transformed_cloud);
        }                    
    }
}

}

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);
while(pcl_node.ok()){
    pubs.publish(transformed_cloud);    
    ros::spin();
    loop_r.sleep();
}

}

Thanks for your help, Sneha