ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

You can use OpenCV to do this task. You can get your disparity map using StereoSGBM, then your depth map using reprojectImageTo3D. Once you have your depth map, you can reconstruct your 3D scene using something like this:

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

//Reconstruct PointCloud with the depthmap points
for (int i = 0; i < depth.rows; ++i)
{
    for (int j = 0; j < depth.cols; ++j)
    {
        pcl::PointXYZRGB p;

        //The coordinate of the point is taken from the depth map
        //Y and Z  taken negative to immediately visualize the cloud in the right way
        p.x = depth.at<Vec3f>(i,j)[0];
        p.y = -(depth.at<Vec3f>(i,j)[1]);
        p.z = -(depth.at<Vec3f>(i,j)[2]);

        //Coloring the point with the corrispondent point in the rectified image
        p.r = static_cast<uint8_t>(color.at<Vec3b>(i,j)[2]);
        p.g = static_cast<uint8_t>(color.at<Vec3b>(i,j)[1]);
        p.b = static_cast<uint8_t>(color.at<Vec3b>(i,j)[0]);

        //Insert point in the cloud, cutting the points that are too distant
        if(( abs( p.x ) < 500 )&&( abs( p.y ) < 200 )&&( abs( p.z ) < 500 ))
            cloud->points.push_back(p);
    }
}
cloud->width = (int) cloud->points.size();
cloud->height = 1;