試驗了好久了,終于成功了!用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)是光軸與像素平面的交點坐标。
效果圖如下:
代碼如下:
<span style="font-size:18px;">#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
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<pcl::PointXYZRGB> cloud_a;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
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<uchar>(u,v)) / 255.0 * 10001.0;
Xw = (u-u0) * Zw / fx;
Yw = (v-v0) * Zw / fy;
cloud_a.points[num].b = color.at<cv::Vec3b>(u,v)[0];
cloud_a.points[num].g = color.at<cv::Vec3b>(u,v)[1];
cloud_a.points[num].r = color.at<cv::Vec3b>(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;
}</span>