How to Align Pointcloud to update Octomap in Callback function
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");
}