cv::Mat to PCL point cloud, to data ptr?
I have a cv::Mat
that is a depth image.
I am converting it to a PCL pointcloud like this:
PointCloud::Ptr PointcloudUtils::RGBDtoPCL(cv::Mat depth_image, Eigen::Matrix3f& _intrinsics)
{
PointCloud::Ptr pointcloud(new PointCloud);
float fx = _intrinsics(0, 0);
float fy = _intrinsics(1, 1);
float cx = _intrinsics(0, 2);
float cy = _intrinsics(1, 2);
float factor = 1;
depth_image.convertTo(depth_image, CV_32F); // convert the image data to float type
if (!depth_image.data) {
std::cerr << "No depth data!!!" << std::endl;
exit(EXIT_FAILURE);
}
pointcloud->width = depth_image.cols; //Dimensions must be initialized to use 2-D indexing
pointcloud->height = depth_image.rows;
pointcloud->resize(pointcloud->width*pointcloud->height);
#pragma omp parallel for
for (int v = 0; v < depth_image.rows; v ++)
{
for (int u = 0; u < depth_image.cols; u ++)
{
float Z = depth_image.at<float>(v, u) / factor;
PointT p;
p.z = Z;
p.x = (u - cx) * Z / fx;
p.y = (v - cy) * Z / fy;
p.z = p.z / 1000;
p.x = p.x / 1000;
p.y = p.y / 1000;
pointcloud->points.push_back(p);
}
}
return pointcloud;
}
I am also getting the data from the Mat like this:
unsigned short* dataMat1 = depth_image.ptr<unsigned short>();
What i need to do now is Convert the pointcloud into an unsigned short *
that matches the dataMat1
above.
What would this conversion look like?
Does the depth_image.ptr<unsigned short>
just read row 0: col1, col2, col3.... row 1: col1,col2
etc?
Thank you!
EDIT:
To simplify the problem, i am trying to get the pcl - to - Mat conversion working. I load my image, run the above code to get a pointcloud, then take that pointcloud and run:
cv::Mat PointcloudUtils::PCL2Mat(PointCloud::Ptr pointcloud, int original_width, int original_height)
{
cv::Mat depth_image(original_height, original_width, CV_32F);
// depth_image.create
int count = 0;
#pragma omp parallel for
for (int v = 0; v < depth_image.rows; ++v)
{
for (int u = 0; u < depth_image.cols; ++u)
{
depth_image.at<float>(v, u) = pointcloud->points.at(count++).z * 1000;
}
}
normalize(depth_image, depth_image, 0, 255, cv::NORM_MINMAX, CV_8U);
return depth_image;
}
I pass that function:
cv::Mat newDepth;
newDepth = pointcloudUtils_.PCL2Mat(newCloud, depth_image.cols, depth_image.rows);
cv::imshow("new", newDepth);
cv::waitKey(1);
(where depth_image
is the original loaded image). But When i view the newly created mat, it is solid black. Where am i going wrong?
thanks!
btw: if you use
pointcloud->points.push_back(p);
, you should not do:pointcloud->points.resize(pointcloud->height * pointcloud->width);
else you push the points after the already allocated , but all empty points.Hi! thanks! That part seems to be working pretty well. I will pull that line out to test though. As to the conversion I need, are you able to point me in the right direction?
tbh, i did not understand you there. do you really want to convert the pointcloud back to ushort ?
(i also vaguely remember, that PCL had some function to condense clouds to images)
I do yup. Basically I create the cloud from the image, then process the cloud. Then I need to run it through a function that takes a Mat.ptr. So either I can try to convert the cloud back to a Mat and get the data.ptr, or just skip the middle step. If possible.
"That part seems to be working pretty well." -- meaning, you see a nice cloud in the viz, i guess.
but if you take a look at cloud.points.size(), you'll see, it's 2x as large as it should be, and that the 1st half is all zeros
Ah i see! I will make that change. thank you. :)
@berak, If you have a minute, I have edited my question to add some conversion code. It looks correct to me, but doesn't work!