结合彩色图和深度图创建点云(OpenCV+OpenNI+PCL)

试验了好久了,终于成功了!用OpenNI获取彩色和深度数据流,转化成OpenCV的Mat图像格式。

对相机进行标定,获取相机的内部参数:

Calibration results after optimization (with uncertainties):   //优化后的参数值

Focal Length:          fc = [ 510.99171   513.71815 ] ? [ 1.99569   2.13975 ]    //焦距
Principal point:       cc = [ 324.12889   236.29232 ] ? [ 3.66214   3.41361 ]    //关键点
Skew:             alpha_c = [ 0.00000 ] ? [ 0.00000  ]   => angle of pixel axes = 90.00000 ? 0.00000 degrees
Distortion:            kc = [ 0.06196   -0.25035   -0.00173   0.00098  0.00000 ] ? [ 0.02152   0.08623   0.00242   0.00263  0.00000 ]
Pixel error:          err = [ 0.33426   0.40108 ]


设(u, v)是像素坐标系的坐标,(Xw, Yw,Zw)是世界坐标系的坐标。

则标定后可以通过(u, v)求出(Xw, Yw),公式如下:

Xw = (u - u0) * Zw / fx;

Yw = (v - v0) *Zw / fy;

其中(u0,v0)是光轴与像素平面的交点坐标。


效果图如下:

结合彩色图和深度图创建点云(OpenCV+OpenNI+PCL)_第1张图片

结合彩色图和深度图创建点云(OpenCV+OpenNI+PCL)_第2张图片

结合彩色图和深度图创建点云(OpenCV+OpenNI+PCL)_第3张图片

代码如下:

#include 
#include 
#include 
#include 
#include 

int user_data;
const double u0 = 319.52883;
const double v0 = 271.61749;
const double fx = 528.57523;
const double fy = 527.57387;

void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor (0.0, 0.0, 0.0);
}
    
int main ()
{
	pcl::PointCloud cloud_a;
	pcl::PointCloud::Ptr cloud (new pcl::PointCloud);


	cv::Mat color = cv::imread("Image1.jpg");
	cv::Mat depth = cv::imread("cc.jpg");

	int rowNumber = color.rows;
	int colNumber = color.cols;

	cloud_a.height  = rowNumber;
	cloud_a.width = colNumber;
	cloud_a.points.resize(cloud_a.width * cloud_a.height);

	for (unsigned int u = 0; u < rowNumber; ++u)
	{
		for (unsigned int v = 0; v < colNumber; ++v)
		{
			unsigned int num = u*colNumber + v;
			double Xw = 0, Yw = 0, Zw = 0;

			Zw = ((double)depth.at(u,v)) / 255.0 * 10001.0;
			Xw = (u-u0) * Zw / fx;
			Yw = (v-v0) * Zw / fy;

			cloud_a.points[num].b = color.at(u,v)[0];
			cloud_a.points[num].g = color.at(u,v)[1];
			cloud_a.points[num].r = color.at(u,v)[2];

			cloud_a.points[num].x = Xw;
			cloud_a.points[num].y = Yw;
			cloud_a.points[num].z = Zw;
		}
	}

	*cloud = cloud_a;

    pcl::visualization::CloudViewer viewer("Cloud Viewer");    

    viewer.showCloud(cloud);

    viewer.runOnVisualizationThreadOnce (viewerOneOff);

    while (!viewer.wasStopped ())
    {
		user_data = 9;
    }

    return 0;
}


你可能感兴趣的:(结合彩色图和深度图创建点云(OpenCV+OpenNI+PCL))