PCL: merging two sets of points to one cloud and visualizeing with PCL cloudviewer - visualization

Question

I am trying to merge two sets of points from two different views to one single point cloud and visualize it with PCL cloud viewer.
mPtrPointCloud->points.clear();
mPtrPointCloud->points.resize(mFrameSize * 2);
auto it = mPtrPointCloud->points.begin();
received = PopReceived();
if(received != nullptr)
{
// p_data_cloud = (float*)received->mTransformedPC.data;
p_data_cloud = (float*)received->mCVPointCloud.data;
index = 0;
for (size_t i = 0; i < mFrameSize; ++i)
{
float X = p_data_cloud[index];
if (!isValidMeasure(X)) // Checking if it's a valid point
{
it->x = it->y = it->z = it->rgb = 0;
}
else
{
it->x = X;
it->y = p_data_cloud[index + 1];
it->z = p_data_cloud[index + 2];
it->rgb = convertColor(p_data_cloud[index + 3]); // Convert a 32bits float into a pcl .rgb format
}
index += 4;
++it;
}
}
frame = PopFrame();
if(frame != nullptr)
{
// p_data_cloud = frame->mSLPointCloud.getPtr<float>();
p_data_cloud = (float*)frame->mCVPointCloud.data;
index = 0;
for (size_t i = 0; i < mFrameSize; ++i)
{
float X = p_data_cloud[index];
if (!isValidMeasure(X)) // Checking if it's a valid point
{
it->x = it->y = it->z = it->rgb = 0;
}
else
{
it->x = X;
it->y = p_data_cloud[index + 1];
it->z = p_data_cloud[index + 2];
it->rgb = convertColor(p_data_cloud[index + 3]); // Convert a 32bits float into a pcl .rgb format
}
index += 4;
++it;
}
}
mPtrPCViewer->showCloud(mPtrPointCloud);
What I want to have is two sets of points are "fused" to one frame. However, it seems these two sets of points are still shown separately one after the other.
Could anyone help to explain how to really merge two sets of points into one cloud? Thanks

Answer 1

(1) Create a new empty pointcloud which will be the merged pointcloud at the end
pcl::PointCloud<pcl::PointXYZ> mPtrPointCloud;
(2) Transform point clouds to origin
pcl::PointCloud<pcl::PointXYZ> recieved_transformed;
Eigen::Transform<Scalar, 3, Eigen::Affine> recieved_transformation_mat(recieved.sensor_origin_ * recieved.sensor_orientation_);
pcl::transformPointCloud(recieved, recieved_transformed, recieved_transformation_mat);
pcl::PointCloud<pcl::PointXYZ> frame_transformed;
Eigen::Transform<Scalar, 3, Eigen::Affine> frame_transformation_mat(frame.sensor_origin_ * frame.sensor_orientation_);
pcl::transformPointCloud(frame, frame_transformed, frame_transformation_mat);
(3) Use the += operator
mPtrPointCloud += received_transformed;
mPtrPointCloud += frame_transformed;
(4) Visualize merged pointcloud
mPtrPCViewer->showCloud(mPtrPointCloud);
That's it. See also example http://pointclouds.org/documentation/tutorials/concatenate_clouds.php
http://pointclouds.org/documentation/tutorials/matrix_transform.php