Conversion from cv::Mat to pcl::PointCloud

classic Classic list List threaded Threaded
25 messages Options
12
Reply | Threaded
Open this post in threaded view
|

Conversion from cv::Mat to pcl::PointCloud

blackibiza
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

koen buys-2
Do you already have the disparity from the stereo or do you still need to calculate this as well?




On 19 November 2012 16:55, blackibiza <[hidden email]> wrote:
Hello
in these days i am trying to convert video sequences acquired from a stereo
camera using OpenCV methods, which save the informations in cv::Mat
structures.
Is there a possibility to convert efficiently (~ realtime) the cv::Mat
structures in pcl::PointCloud (including eventually RGB colors?)

thanks in advance



--
View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936.html
Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users


_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

gestalone
I am interested in the same, please if you can do this, if is possible put here the code.

2012/11/19 Koen Buys <[hidden email]>
Do you already have the disparity from the stereo or do you still need to calculate this as well?





On 19 November 2012 16:55, blackibiza <[hidden email]> wrote:
Hello
in these days i am trying to convert video sequences acquired from a stereo
camera using OpenCV methods, which save the informations in cv::Mat
structures.
Is there a possibility to convert efficiently (~ realtime) the cv::Mat
structures in pcl::PointCloud (including eventually RGB colors?)

thanks in advance



--
View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936.html
Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users


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



_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

koen buys-2
During my internship at Willow I wrote a OpenCVPCL Bridge package which should still reside on kforge.ros.org.
However as this was unmaintained, I don't know what the current state is.
The problem was that it couldn't be included in PCL as this would create a dependency on OpenCV within PCL, which was unwanted.


On 19 November 2012 19:50, Manuel Gesto Díaz <[hidden email]> wrote:
I am interested in the same, please if you can do this, if is possible put here the code.


2012/11/19 Koen Buys <[hidden email]>
Do you already have the disparity from the stereo or do you still need to calculate this as well?





On 19 November 2012 16:55, blackibiza <[hidden email]> wrote:
Hello
in these days i am trying to convert video sequences acquired from a stereo
camera using OpenCV methods, which save the informations in cv::Mat
structures.
Is there a possibility to convert efficiently (~ realtime) the cv::Mat
structures in pcl::PointCloud (including eventually RGB colors?)

thanks in advance



--
View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936.html
Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users


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



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



_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

blackibiza
This post was updated on .
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

blackibiza
In reply to this post by koen buys-2
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Matteo Munaro
This post has NOT been accepted by the mailing list yet.
Hi blackibiza,

supposing that your disparity values are in a cv::Mat with elements of type CV_16U (unsigned short), you can access the value at row i and column j with the following:
unsigned short disparityValue = disparityMatrix.at<unsigned short>(i,j);

I hope this helps.

Cheers,
Matteo
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Matteo Munaro
In reply to this post by blackibiza

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: +390498277831
___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800

> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud
>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

koen buys-2
There is also a tool app in trunk that excepts both depth and rgb tiff image and creates a pointcloud pcd file for you.
However this is a brute force implementation, without really any optimizations (speed wise)


On 20 November 2012 14:41, Matteo Munaro <[hidden email]> wrote:

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: <a href="tel:%2B390498277831" value="+390498277831" target="_blank">+390498277831
___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800
> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

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



_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

blackibiza
In reply to this post by Matteo Munaro
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

gestalone
In reply to this post by koen buys-2
Thanks Mateo, this is perfect for me!!!

I only have one question, it´s about the:

 if (depthValue == depthValue); here i think depthValue != =0  , that means that the nan value, in my case is 0

but i am not really , sure,

by the way, Tanks a lot!

2012/11/20 Koen Buys <[hidden email]>
There is also a tool app in trunk that excepts both depth and rgb tiff image and creates a pointcloud pcd file for you.
However this is a brute force implementation, without really any optimizations (speed wise)



On 20 November 2012 14:41, Matteo Munaro <[hidden email]> wrote:

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: <a href="tel:%2B390498277831" value="+390498277831" target="_blank">+390498277831
___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800
> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

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



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



_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

blackibiza
In reply to this post by koen buys-2
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Matteo Munaro
In reply to this post by gestalone

Hi Manuel,

if the invalid data in your depth image are set to 0, you can use:
      if (depthValue > 0)

if the invalid data in your depth image are set to NaN, then you should use either 
      if (!isnan(depthValue)) 
or 
      if (depthValue == depthValue)
because NaN is not equal to itself.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: +390498277831
___________________________________________



Date: Tue, 20 Nov 2012 14:49:39 +0100
From: [hidden email]
To: [hidden email]
Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

Thanks Mateo, this is perfect for me!!!

I only have one question, it´s about the:

 if (depthValue == depthValue); here i think depthValue != =0  , that means that the nan value, in my case is 0

but i am not really , sure,

by the way, Tanks a lot!

2012/11/20 Koen Buys <[hidden email]>
There is also a tool app in trunk that excepts both depth and rgb tiff image and creates a pointcloud pcd file for you.
However this is a brute force implementation, without really any optimizations (speed wise)



On 20 November 2012 14:41, Matteo Munaro <[hidden email]> wrote:

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800
> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

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



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



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

_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

blackibiza
In reply to this post by Matteo Munaro
CONTENTS DELETED
The author has deleted this message.
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

gestalone
In reply to this post by Matteo Munaro
Hi Mateo, 

And other question, it´s possible to pass to pcl the matches?? because I want to pass the the inliers between 2 pictures taken by the kinect,  I find the key points and correspondences, with your algorith I can have bouth point clouds, now the problem it´s to pass the matches, the correct matches and look for a transformation. I know how i can  do the transformation in pcl, the problem it´s pass the matches in opencv to pcl.

Thanks in advanced.


2012/11/20 Matteo Munaro <[hidden email]>

Hi Manuel,

if the invalid data in your depth image are set to 0, you can use:
      if (depthValue > 0)

if the invalid data in your depth image are set to NaN, then you should use either 
      if (!isnan(depthValue)) 
or 
      if (depthValue == depthValue)
because NaN is not equal to itself.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: <a href="tel:%2B390498277831" value="+390498277831" target="_blank">+390498277831
___________________________________________



Date: Tue, 20 Nov 2012 14:49:39 +0100
From: [hidden email]

To: [hidden email]
Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

Thanks Mateo, this is perfect for me!!!

I only have one question, it´s about the:

 if (depthValue == depthValue); here i think depthValue != =0  , that means that the nan value, in my case is 0

but i am not really , sure,

by the way, Tanks a lot!

2012/11/20 Koen Buys <[hidden email]>
There is also a tool app in trunk that excepts both depth and rgb tiff image and creates a pointcloud pcd file for you.
However this is a brute force implementation, without really any optimizations (speed wise)



On 20 November 2012 14:41, Matteo Munaro <[hidden email]> wrote:

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800
> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

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



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



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

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



_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Matteo Munaro
In reply to this post by blackibiza

If you disparity is normalized, that is a zero disparity corresponds to infinite distance, then you should be able to estimate depth with:

depth = baseline * focalLength / disparity

where baseline is the distance between your cameras.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: +390498277831
___________________________________________


> Date: Tue, 20 Nov 2012 05:58:25 -0800

> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud
>
> Matteo Munaro wrote
> > If your disparity image is converted to a depth image and registered to
> > it, then you can obtain a XYZRGB pointcloud with this code:
>
> Actually i just have the disparity map which comes out from the method
> sgbm(cv::Mat left, cv::Mat right, cv::Mat output)
> http://opencv.willowgarage.com/documentation/cpp/camera_calibration_and_3d_reconstruction.html#stereosgbm
>
> I was missing the step of convertion between disparity map and depth map.
> Thank you for your precious support.
>
> Best regards
> Michele
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023968.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Matteo Munaro
In reply to this post by gestalone

Manuel,

the pointclouds you create with the code I posted are dense, thus you have one-to-one point correspondence with your rgb image.
If you want the pointcloud point corresponding to the image point at coordinates (x,y), you can simply find it this way: 
      pcl::PointXYZRGB point = ouputPointcloud->points[x + y*rgbImage.cols];

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

phone: +390498277831
___________________________________________



Date: Tue, 20 Nov 2012 15:06:29 +0100
From: [hidden email]
To: [hidden email]
Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

Hi Mateo, 

And other question, it´s possible to pass to pcl the matches?? because I want to pass the the inliers between 2 pictures taken by the kinect,  I find the key points and correspondences, with your algorith I can have bouth point clouds, now the problem it´s to pass the matches, the correct matches and look for a transformation. I know how i can  do the transformation in pcl, the problem it´s pass the matches in opencv to pcl.

Thanks in advanced.


2012/11/20 Matteo Munaro <[hidden email]>

Hi Manuel,

if the invalid data in your depth image are set to 0, you can use:
      if (depthValue > 0)

if the invalid data in your depth image are set to NaN, then you should use either 
      if (!isnan(depthValue)) 
or 
      if (depthValue == depthValue)
because NaN is not equal to itself.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

___________________________________________



Date: Tue, 20 Nov 2012 14:49:39 +0100
From: [hidden email]

To: [hidden email]
Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

Thanks Mateo, this is perfect for me!!!

I only have one question, it´s about the:

 if (depthValue == depthValue); here i think depthValue != =0  , that means that the nan value, in my case is 0

but i am not really , sure,

by the way, Tanks a lot!

2012/11/20 Koen Buys <[hidden email]>
There is also a tool app in trunk that excepts both depth and rgb tiff image and creates a pointcloud pcd file for you.
However this is a brute force implementation, without really any optimizations (speed wise)



On 20 November 2012 14:41, Matteo Munaro <[hidden email]> wrote:

If your disparity image is converted to a depth image and registered to it, then you can obtain a XYZRGB pointcloud with this code:

void createPointcloudFromRegisteredDepthImage(cv::Mat& depthImage, cv::Mat& rgbImage, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& outputPointcloud, Eigen::Matrix3f& rgbIntrinsicMatrix)
{
        float rgbFocalInvertedX = 1/rgbIntrinsicMatrix(0,0); // 1/fx
float rgbFocalInvertedY = 1/rgbIntrinsicMatrix(1,1); // 1/fy

pcl::PointXYZRGB newPoint;
for (int i=0;i<depthImage.rows;i++)
{
for (int j=0;j<depthImage.cols;j++)
{
float depthValue = depthImage.at<float>(i,j);

if (depthValue == depthValue)                // if depthValue is not NaN
{
// Find 3D position respect to rgb frame:
newPoint.z = depthValue;
newPoint.x = (j - rgbIntrinsicMatrix(0,2)) * newPoint.z * rgbFocalInvertedX;
newPoint.y = (i - rgbIntrinsicMatrix(1,2)) * newPoint.z * rgbFocalInvertedY;
newPoint.r = rgbImage.at<cv::Vec3b>(i,j)[2];
newPoint.g = rgbImage.at<cv::Vec3b>(i,j)[1];
newPoint.b = rgbImage.at<cv::Vec3b>(i,j)[0];
outputPointcloud->push_back(newPoint);
}
else
{
newPoint.z = std::numeric_limits<float>::quiet_NaN();
newPoint.x = std::numeric_limits<float>::quiet_NaN();
newPoint.y = std::numeric_limits<float>::quiet_NaN();
newPoint.r = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.g = std::numeric_limits<unsigned char>::quiet_NaN();
newPoint.b = std::numeric_limits<unsigned char>::quiet_NaN();
outputPointcloud->push_back(newPoint);
}
}
}
}

For doing depth->rgb registration, you can also check this page: http://www.ros.org/wiki/kinect_calibration/technical
It refers to Kinect, but I think it could help also for your case.

Cheers,
Matteo
___________________________________________

Matteo MUNARO

PhD Student
Intelligent and Autonomous Systems Lab (IAS-Lab)
Department of Information Engineering (DEI)
Faculty of Engineering, The University of Padua
Via Ognissanti 72, I-35131 Padova, Italy

___________________________________________


> Date: Tue, 20 Nov 2012 02:49:09 -0800
> From: [hidden email]
> To: [hidden email]
> Subject: Re: [PCL-users] Conversion from cv::Mat to pcl::PointCloud

>
> koen buys-2 wrote
> > During my internship at Willow I wrote a OpenCVPCL Bridge package which
> > should still reside on kforge.ros.org.
> > However as this was unmaintained, I don't know what the current state is.
> > The problem was that it couldn't be included in PCL as this would create a
> > dependency on OpenCV within PCL, which was unwanted.
>
> I checked the code, but you made the assumption of having already a
> pointcloud and a cv::Mat as input and produce the coloured pointcloud as
> output.
> My doubt is about converting the cv::Mat disparity map into a pointcloud and
> associate then the image colours.
>
>
>
>
> --
> View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023957.html
> Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
> _______________________________________________
> [hidden email] / http://pointclouds.org
> http://pointclouds.org/mailman/listinfo/pcl-users

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



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



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

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



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

_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

koen buys-2
In reply to this post by blackibiza



On 20 November 2012 14:51, blackibiza <[hidden email]> wrote:
koen buys-2 wrote
> There is also a tool app in trunk that excepts both depth and rgb tiff
> image and creates a pointcloud pcd file for you.
> However this is a brute force implementation, without really any
> optimizations (speed wise)

I already tried it but it is slow...i am trying to write down ~real-time
tool (until now the computation of disparity map produces around 8 fps), but
the tool of PCL takes seconds just for one disparity map.


Ow, I had far better performance ... (it's mainly a for loop over the struct)
 


--
View this message in context: http://www.pcl-users.org/Conversion-from-cv-Mat-to-pcl-PointCloud-tp4023936p4023967.html
Sent from the Point Cloud Library (PCL) Users mailing list mailing list archive at Nabble.com.
_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users


_______________________________________________
[hidden email] / http://pointclouds.org
http://pointclouds.org/mailman/listinfo/pcl-users
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

fedassa
Hi Black ibiza,

could you please specify which pcl stereo matching function you have been trying to use for your tests? Cos currently there are two different algorithms implemented on pcl_stereo, one being definitely more efficient than the other

Thanks
Fede
Reply | Threaded
Open this post in threaded view
|

Re: Conversion from cv::Mat to pcl::PointCloud

Shawarma
In reply to this post by blackibiza
Hello blackibiza,

By conversion between disparity map and depth map did you just use the cv::reprojectImageTo3D API call? Can you perhaps please elaborate on how you are doing the conversion to a PCL point cloud? I'm using a Stereo Camera and trying to do the same thing. My disparity image using OpenCV looks quite clean, but after using cv::reprojectImageTo3D and visualizing the point cloud, it looks nothing like the disparity image.

I know the obvious thing to investigate is the camera calibration parameters, since the Q matrix is largely used for the disparity to depth mapping. I've heavily investigated the camera calibration and I'm confident I'm doing things right, my rectified and undistorted images look quite good, so it makes me feel confident that the calibration was done right. More importantly, the epipolar lines when draw on the rectified image pairs, do line up correctly ...

I'd appreciate any insight you could please share ...
12