Get textured pointcloud with Block-Matching-Algorithm

Question

I want to texture a generated pointcloud with the original image color from two images. For this I calculated the disparity-map with Block-Matching and did the reconstruction. Also writing an export function for .ply-files wasn't a big deal. My problem is: How do I get the color from block-matching-algorithm? It does look for similar pixels on rectified images, but there is no variable which saves the position of a found matching, referred to API. Afterward it is not possible to recover the color.

StereoBM sbm;
sbm(left_rectfied_image, right_rectified_image, disparity, CV_32F);

(I am working with OpenCV 2.4.8)

Answer 1

Yes! The disparity map you are computing is for the Rectified Left Image! You can simply use the Left Image Pixel XY Co-Ordinate values for all the points in 3D. for e.g.

reprojectImageTo3D(disp_32, xyz, Q, true);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>); 
const double max_z = 1.0e4;

for (int Row = 0; Row < xyz.rows; Row++)
{
    for (int Col = 0; Col < xyz.cols; Col++)
    {
        pcl::PointXYZRGB point;
        vec3b Pix;
        //Just taking the Z Axis alone
        Vec3f Depth= xyz.at<Vec3f>(Row,Col);
        point.x = Depth[0];
        point.y = Depth[1];
        point.z = Depth[2];

        if(fabs(Depth[2] - max_z) < FLT_EPSILON || fabs(Depth[2]) > max_z|| Depth[2] > 0) 
            continue;

        Pix= mCamFrame_Left.at<vec3b>(Row,Col);

        uint32_t rgb = (static_cast<uint32_t>(Pix.val[0]) << 16 |static_cast<uint32_t>(Pix.val[1]) << 8 | static_cast<uint32_t>(Pix.val[2]));
        point.rgb = *reinterpret_cast<float*>(&rgb);
        point_cloud_ptr->points.push_back (point);
    }
}
point_cloud_ptr->width = (int) point_cloud_ptr->points.size();
point_cloud_ptr->height = 1;