利用该工程在获取点云的同时,可以获取深度图像,默认为dist.png。得到的彩色影像是和深度影像对应的,每个像素点代表的是一个三维点。
用下面的函数是可以将该深度图像和彩色图像转化为三维点云:
vector<double> convtD2W(char *a,char*b)
{
vector<double>data;
Vec3b color;double wx,wy,wz;
Vec3b color_;
Mat img=imread(a,CV_LOAD_IMAGE_COLOR);
Mat img_color=imread(b,CV_LOAD_IMAGE_COLOR);
for(int i=0;i<img.cols;i++)
{
for(int j=0;j<img.rows;j++)
{
color_=img_color.at<Vec3b>(j,i);
color=img.at<Vec3b>(j,i);
double l=color.val[0]+256*color.val[1];
double nx=(0.0+i)/img.cols-0.5;
double ny=0.5-(0.0+j)/img.rows;
/*double nx=(0.0+j)/img.rows-0.5;
double ny=0.5-(0.0+i)/img.cols;*/
//double l=color.val[0]/255.0*10;
//wx=sin(nx*3.141592653*2/3.0)*l;
//wy=sin(ny*3.141592653/2.0)*l;
//wz=sqrt(l*l-wx*wx+wy*wy);
wx=nx*l*1.2017212;
wy=ny*l*0.90303463;
/* wx=nx*l*0.84072256;
wy=ny*l*1.1200538;*/
wz=l;
if(wx<0.01&&wy<0.01&&wz<0.1)
{
}
else if(l<10000)
{
data.push_back(wx);
data.push_back(wy);
data.push_back(wz);
data.push_back(color_.val[0]+0.0);
data.push_back(color_.val[1]+0.0);
data.push_back(color_.val[2]+0.0);
}
}
}
return data;
}
由上面的函数得到的三维点云保存在数组当中,再利用以下的函数则可以保存为点云txt文件
void writexyz(char*a,vector<double>data)
{
FILE *fp=fopen(a,"w");
for(int i=0;i<data.size()/6;i++)
{
fprintf(fp,"%lf %lf %lf %d %d %d\n",data[6*i],data[6*i+1],data[6*i+2],int(data[6*i+3]),int(data[6*i+4]),int(data[6*i+5]));
}
fclose(fp);
}
用法实例:
vector<double> pointcloud=convtD2W("dist.png","color.jpg");writexyz("result.txt",pointcloud);
深度图像彩色图像
转换得到的点云截图
|