How to Align Pointcloud to update Octomap in Callback function

asked 2020-01-20 17:43:06 -0500

zhefan gravatar image

Hi, I have seen many questions in registering a series of pointcloud, BUT I am not sure how to align my pointcloud data obtained from callback function. Below is my code which is definitely incorrect since I did not do the pointcloud alignment (registration). The pointcloud data is NOT aligned yet. What I want to do is to continuously update my Octomap by calling InsertPointCloud() function. So how can I transform the pointcloud data in order to update the map correctly?

void callback(const OdometryConstPtr& odom, const PointCloud2ConstPtr& pc){
    **Get the position and orientation**
    float current_x = odom->pose.pose.position.x;
    float current_y = odom->pose.pose.position.y;
    float current_z = odom->pose.pose.position.z;
    float orientation_x = odom->pose.pose.orientation.x;
    float orientation_y = odom->pose.pose.orientation.y;
    float orientation_z = odom->pose.pose.orientation.z;
    float orientation_w = odom->pose.pose.orientation.w;
    point3d sensor_origin (0, 0, 0);
    point3d pos (current_x, current_y, current_z);
    octomath::Quaternion rot (orientation_w, orientation_x, orientation_y, orientation_z);
    pose6d frame_origin (pos, rot);
    octomap::pointCloud2ToOctomap(*pc, pc_octo);
    **Update Map**
    tree.insertPointCloud(pc_octo, sensor_origin, frame_origin);
    tree.writeBinary("check.bt");
}
edit retag flag offensive close merge delete