通过深度图像和彩色图像构建三维点云

最近一直在研究图像处理方面，写了一些代码，望大佬给出改善建议：
下面需要opencv和openNI2的环境搭建

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 pointcloud=convtD2W(“dist.png”,”color.jpg”);writexyz(“result.txt”,pointcloud)；

实验结果：