Point cloud data live visualization from ToF camera point cloud data

classic Classic list List threaded Threaded
1 message Options
Reply | Threaded
Open this post in threaded view
|

Point cloud data live visualization from ToF camera point cloud data

shaikh951
Hi,
I am working with Melaxis ToF camera, It consist of point cloud data. Using
PCL I am trying to capture live point cloud data and visualize it but after
capturing 1st frame next frames are writing on previous point cloud data.
What I am missing Unable to understand.
Here is my code.
-> This is the code for visualization.

boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new
pcl::visualization::PCLVisualizer());
viewer->setBackgroundColor(0,0,0);
void displayCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const
std::string& window_name)
{
    if (cloud->size() < 1)
    {
        std::cout << window_name << " display failure. Cloud contains no
points\n";
        return;
    }
    if(!viewer->updatePointCloud(cloud,"id"))
    {
        viewer->addPointCloud(cloud, "id");
       
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,1,
"id");
        viewer->registerKeyboardCallback(keyboardEventOccurred,
(void*)viewer.get());
        viewer->registerMouseCallback (mouseEventOccurred, (void*)viewer.get
());
    }

        viewer->spinOnce(1);
}

-> This is the code for ToF camera point cloud streaming

 while(1)
    {
        BTA_Frame *frame;
        printf("BTAgetFrame()\n");
        status = BTAgetFrame(btaHandle, &frame, 300);
        errorHandling(status);
        uint16_t *amplitudes;
        BTA_DataFormat dataFormat;
        BTA_Unit unit;
        uint16_t xRes, yRes;

        void *xCoordinates, *yCoordinates, *zCoordinates;
        status = BTAgetXYZcoordinates(frame, &xCoordinates, &yCoordinates,
&zCoordinates,
                                      &dataFormat, &unit, &xRes, &yRes);
        errorHandling(status);
        if (dataFormat == BTA_DataFormatSInt16) {
            // This dataformat tells us that it's ok to cast (void *) to
(int16_t *)
            if (unit == BTA_UnitMillimeter) {
                uint32_t radiusMin = 0xffffffff;        //4294967295 in
decimal
                int16_t xCoordinate = 0, yCoordinate = 0, zCoordinate = 0;
                for (int y = 0; y < yRes; y++) {
                    for (int x = 0; x < xRes; x++) {
                       
                        uint32_t radius = ((int16_t *)xCoordinates)[x +
y*xRes] * ((int16_t *)xCoordinates)[x + y*xRes];
                        radius += ((int16_t *)yCoordinates)[x + y*xRes] *
((int16_t *)yCoordinates)[x + y*xRes];
                        radius += ((int16_t *)zCoordinates)[x + y*xRes] *
((int16_t *)zCoordinates)[x + y*xRes];
                       
                        radiusMin = radius;
                        xCoordinate = ((int16_t *)xCoordinates)[x + y*xRes];
                        yCoordinate = ((int16_t *)yCoordinates)[x + y*xRes];
                        zCoordinate = ((int16_t *)zCoordinates)[x + y*xRes];
                        pcl::PointXYZ basic_point;
                        basic_point.x = xCoordinate;
                        basic_point.y = yCoordinate;
                        basic_point.z = zCoordinate;

                        basic_cloud_ptr->points.push_back (basic_point);
                    }
                }

            }
        }
displayCloud(basic_cloud_ptr,"viewer");
}


Thank you for your help!



--
Sent from: http://www.pcl-users.org/
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users