diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..81c6476 --- /dev/null +++ b/.clang-format @@ -0,0 +1,7 @@ +--- +BasedOnStyle: LLVM +ColumnLimit: 100 +TabWidth: 2 +IndentPPDirectives: AfterHash +AlignConsecutiveAssignments: true +... diff --git a/clean.hpp b/clean.hpp index 8f50914..2490017 100644 --- a/clean.hpp +++ b/clean.hpp @@ -1,186 +1,193 @@ -#ifndef CLEANTOOL -#define CLEANTOOL -#include -#include -#include -#include -using namespace cv; -using namespace std; -float fcxcy[3];//SET THE CAMERA PARAMETERS F CX CY BEFORE USE calplanenormal. -int WINDOWSIZE=15;//SET SEARCH WINDOWSIZE(SUGGEST 15) BEFORE USE calplanenormal. -float Tthrehold;//SET THE threshold (SUGGEST 0.1-0.2)BEFORE USE calplanenormal. -// Ax+by+cz=D -void -CallFitPlane(const Mat& depth,int * points,int i,int j,float *plane12) { - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - vectorX_vector; - vectorY_vector; - vectorZ_vector; - for(int num_point=0; num_point(point_i, point_j ) * 1.0 / f; - float y = (point_i - cy) * depth.at(point_i, point_j )* 1.0 / f; - float z = depth.at(point_i,point_j); - X_vector.push_back(x); - Y_vector.push_back(y); - Z_vector.push_back(z); - } - CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 - if(X_vector.size()<3){ plane12[0]=-1;plane12[1]=-1;plane12[2]=-1;plane12[3]=-1;return;} - for (int ii=0;ii < X_vector.size(); ++ii){ - points_mat->data.fl[ii * 3 + 0] = X_vector[ii];//矩阵的值进行初始化 X的坐标值 - points_mat->data.fl[ii * 3 + 1] = Y_vector[ii];// Y的坐标值 - points_mat->data.fl[ii * 3 + 2] = Z_vector[ii];// - } - // float plane12[4] = { 0 };//定义用来储存平面参数的数组 - cvFitPlane(points_mat, plane12);//调用方程 - if( telldirection(plane12,i,j,depth.at(i,j)) ){ - plane12[0]=-plane12[0]; - plane12[1]=-plane12[1]; - plane12[2]=-plane12[2];} - X_vector.clear(); - Y_vector.clear(); - Z_vector.clear(); - cvReleaseMat(&points_mat); -} - -void -cvFitPlane(const CvMat* points, float* plane){ - // Estimate geometric centroid. - int nrows = points->rows; - int ncols = points->cols; - int type = points->type; - CvMat* centroid = cvCreateMat(1, ncols, type); - cvSet(centroid, cvScalar(0)); - for (int c = 0; cdata.fl[c] += points->data.fl[ncols*r + c]; - } - centroid->data.fl[c] /= nrows; - } - // Subtract geometric centroid from each point. - CvMat* points2 = cvCreateMat(nrows, ncols, type); - for (int r = 0; rdata.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c]; - // Evaluate SVD of covariance matrix. - CvMat* A = cvCreateMat(ncols, ncols, type); - CvMat* W = cvCreateMat(ncols, ncols, type); - CvMat* V = cvCreateMat(ncols, ncols, type); - cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T); - cvSVD(A, W, NULL, V, CV_SVD_V_T); - // Assign plane coefficients by singular vector corresponding to smallest singular value. - plane[ncols] = 0; - for (int c = 0; cdata.fl[ncols*(ncols - 1) + c]; - plane[ncols] += plane[c] * centroid->data.fl[c]; - } - // Release allocated resources. - cvReleaseMat(¢roid); - cvReleaseMat(&points2); - cvReleaseMat(&A); - cvReleaseMat(&W); - cvReleaseMat(&V); -} -void -search_plane_neighbor(Mat &img,int i,int j ,float threhold,int* result){ - int cols =img.cols; - int rows =img.rows; - for (int ii=0; ii(i,j); - for (int idx=0; idx= rows || ry>=cols )continue; - if( img.at(rx,ry)==0.0)continue; - if( abs(img.at(rx,ry)-center_depth)<=Tthrehold*center_depth ) - result[idx*WINDOWSIZE+idy]=1; - } -} -int -telldirection(float * abc,int i,int j,float d){ - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - float x = (j - cx) *d * 1.0 / f; - float y = (i - cy) *d * 1.0 / f; - float z = d; - // Vec3f camera_center=Vec3f(cx,cy,0); - Vec3f cor = Vec3f(0-x, 0-y, 0-z); - Vec3f abcline = Vec3f(abc[0],abc[1],abc[2]); - float corner = cor.dot(abcline); - // float corner =(cx-x)*abc[0]+(cy-y) *abc[1]+(0-z)*abc[2]; - if (corner>=0) - return 1; - else return 0; - -} -Mat -calplanenormal(Mat &src){ - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - Mat normals = Mat::zeros(src.size(),CV_32FC3); - src.convertTo(src,CV_32FC1); - src*=1.0; - int cols =src.cols; - int rows =src.rows; - // int plane_points[WINDOWSIZE*WINDOWSIZE]={0}; - int * plane_points = new int[WINDOWSIZE*WINDOWSIZE]; - float * plane12 = new float[4]; - for (int i=0;i< rows;i++) - for (int j=0;j< cols;j++){ - //for kitti and nyud test - if(src.at(i,j)==0.0)continue; - //for:nyud train - // if(src.at(i,j)<=4000.0)continue; - - search_plane_neighbor(src,i,j,15.0,plane_points); - CallFitPlane(src,plane_points,i,j,plane12); - Vec3f d = Vec3f(plane12[0],plane12[1],plane12[2]); - Vec3f n = normalize(d); - normals.at(i, j) = n; - } - Mat res = Mat::zeros(src.size(),CV_32FC3); - for (int i=0;i(i, j)[0] = -1.0 * normals.at(i, j)[0]; - res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; - res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; - } - - delete[] plane12; - delete[] plane_points; - normals.release(); - for (int i=0;i(i, j)[0]==0&&res.at(i, j)[1]==0&&res.at(i, j)[2]==0)){ - res.at(i, j)[0] += 1.0 ; - res.at(i, j)[2] += 1.0 ; - res.at(i, j)[1] += 1.0; - } - } - - res =res * 127.5; - res.convertTo(res, CV_8UC3); - cvtColor(res, res, COLOR_BGR2RGB); - return res; -} - -void -demo{ -//set parameters here:fcxcy[0]=0;fcxcy[1]=0;fcxcy[2]=0; -Mat src=imread(INPUT_FILE_NAME,CV_LOAD_IMAGE_ANYDEPTH); -Mat res=calplanenormal(src); -imwrite(OUTPUT_NAME,res); -} -#endif +#ifndef CLEANTOOL +#define CLEANTOOL +#include +#include +#include +#include +using namespace cv; +using namespace std; +float fcxcy[3]; // SET THE CAMERA PARAMETERS F CX CY BEFORE USE calplanenormal. +int WINDOWSIZE = 15; // SET SEARCH WINDOWSIZE(SUGGEST 15) BEFORE USE calplanenormal. +float Tthrehold; // SET THE threshold (SUGGEST 0.1-0.2)BEFORE USE + // calplanenormal. +// Ax+by+cz=D +void CallFitPlane(const Mat &depth, int *points, int i, int j, float *plane12) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + vector X_vector; + vector Y_vector; + vector Z_vector; + for (int num_point = 0; num_point < WINDOWSIZE * WINDOWSIZE; num_point++) + if (points[num_point] == 1) { // search 已经处理了边界,此处不需要再处理了 + int point_i, point_j; + point_i = floor(num_point / WINDOWSIZE); + point_j = num_point - (point_i * WINDOWSIZE); + point_i += i - int(WINDOWSIZE / 2); + point_j += j - int(WINDOWSIZE / 2); + float x = (point_j - cx) * depth.at(point_i, point_j) * 1.0 / f; + float y = (point_i - cy) * depth.at(point_i, point_j) * 1.0 / f; + float z = depth.at(point_i, point_j); + X_vector.push_back(x); + Y_vector.push_back(y); + Z_vector.push_back(z); + } + CvMat *points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1); //定义用来存储需要拟合点的矩阵 + if (X_vector.size() < 3) { + plane12[0] = -1; + plane12[1] = -1; + plane12[2] = -1; + plane12[3] = -1; + return; + } + for (int ii = 0; ii < X_vector.size(); ++ii) { + points_mat->data.fl[ii * 3 + 0] = X_vector[ii]; //矩阵的值进行初始化 X的坐标值 + points_mat->data.fl[ii * 3 + 1] = Y_vector[ii]; // Y的坐标值 + points_mat->data.fl[ii * 3 + 2] = Z_vector[ii]; // + } + // float plane12[4] = { 0 };//定义用来储存平面参数的数组 + cvFitPlane(points_mat, plane12); //调用方程 + if (telldirection(plane12, i, j, depth.at(i, j))) { + plane12[0] = -plane12[0]; + plane12[1] = -plane12[1]; + plane12[2] = -plane12[2]; + } + X_vector.clear(); + Y_vector.clear(); + Z_vector.clear(); + cvReleaseMat(&points_mat); +} + +void cvFitPlane(const CvMat *points, float *plane) { + // Estimate geometric centroid. + int nrows = points->rows; + int ncols = points->cols; + int type = points->type; + CvMat *centroid = cvCreateMat(1, ncols, type); + cvSet(centroid, cvScalar(0)); + for (int c = 0; c < ncols; c++) { + for (int r = 0; r < nrows; r++) { + centroid->data.fl[c] += points->data.fl[ncols * r + c]; + } + centroid->data.fl[c] /= nrows; + } + // Subtract geometric centroid from each point. + CvMat *points2 = cvCreateMat(nrows, ncols, type); + for (int r = 0; r < nrows; r++) + for (int c = 0; c < ncols; c++) + points2->data.fl[ncols * r + c] = points->data.fl[ncols * r + c] - centroid->data.fl[c]; + // Evaluate SVD of covariance matrix. + CvMat *A = cvCreateMat(ncols, ncols, type); + CvMat *W = cvCreateMat(ncols, ncols, type); + CvMat *V = cvCreateMat(ncols, ncols, type); + cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T); + cvSVD(A, W, NULL, V, CV_SVD_V_T); + // Assign plane coefficients by singular vector corresponding to smallest + // singular value. + plane[ncols] = 0; + for (int c = 0; c < ncols; c++) { + plane[c] = V->data.fl[ncols * (ncols - 1) + c]; + plane[ncols] += plane[c] * centroid->data.fl[c]; + } + // Release allocated resources. + cvReleaseMat(¢roid); + cvReleaseMat(&points2); + cvReleaseMat(&A); + cvReleaseMat(&W); + cvReleaseMat(&V); +} +void search_plane_neighbor(Mat &img, int i, int j, float threhold, int *result) { + int cols = img.cols; + int rows = img.rows; + for (int ii = 0; ii < WINDOWSIZE * WINDOWSIZE; ii++) + result[ii] = 0; + float center_depth = img.at(i, j); + for (int idx = 0; idx < WINDOWSIZE; idx++) + for (int idy = 0; idy < WINDOWSIZE; idy++) { + int rx = i - int(WINDOWSIZE / 2) + idx; + int ry = j - int(WINDOWSIZE / 2) + idy; + if (rx >= rows || ry >= cols) + continue; + if (img.at(rx, ry) == 0.0) + continue; + if (abs(img.at(rx, ry) - center_depth) <= Tthrehold * center_depth) + result[idx * WINDOWSIZE + idy] = 1; + } +} +int telldirection(float *abc, int i, int j, float d) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + float x = (j - cx) * d * 1.0 / f; + float y = (i - cy) * d * 1.0 / f; + float z = d; + // Vec3f camera_center=Vec3f(cx,cy,0); + Vec3f cor = Vec3f(0 - x, 0 - y, 0 - z); + Vec3f abcline = Vec3f(abc[0], abc[1], abc[2]); + float corner = cor.dot(abcline); + // float corner =(cx-x)*abc[0]+(cy-y) *abc[1]+(0-z)*abc[2]; + if (corner >= 0) + return 1; + else + return 0; +} +Mat calplanenormal(Mat &src) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + Mat normals = Mat::zeros(src.size(), CV_32FC3); + src.convertTo(src, CV_32FC1); + src *= 1.0; + int cols = src.cols; + int rows = src.rows; + // int plane_points[WINDOWSIZE*WINDOWSIZE]={0}; + int *plane_points = new int[WINDOWSIZE * WINDOWSIZE]; + float *plane12 = new float[4]; + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + // for kitti and nyud test + if (src.at(i, j) == 0.0) + continue; + // for:nyud train + // if(src.at(i,j)<=4000.0)continue; + + search_plane_neighbor(src, i, j, 15.0, plane_points); + CallFitPlane(src, plane_points, i, j, plane12); + Vec3f d = Vec3f(plane12[0], plane12[1], plane12[2]); + Vec3f n = normalize(d); + normals.at(i, j) = n; + } + Mat res = Mat::zeros(src.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] = -1.0 * normals.at(i, j)[0]; + res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; + res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; + } + + delete[] plane12; + delete[] plane_points; + normals.release(); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (!(res.at(i, j)[0] == 0 && res.at(i, j)[1] == 0 && + res.at(i, j)[2] == 0)) { + res.at(i, j)[0] += 1.0; + res.at(i, j)[2] += 1.0; + res.at(i, j)[1] += 1.0; + } + } + + res = res * 127.5; + res.convertTo(res, CV_8UC3); + cvtColor(res, res, COLOR_BGR2RGB); + return res; +} + +void demo { + // set parameters here:fcxcy[0]=0;fcxcy[1]=0;fcxcy[2]=0; + Mat src = imread(INPUT_FILE_NAME, CV_LOAD_IMAGE_ANYDEPTH); + Mat res = calplanenormal(src); + imwrite(OUTPUT_NAME, res); +} +#endif diff --git a/tool.cpp b/tool.cpp index 727662f..e8f9ad7 100644 --- a/tool.cpp +++ b/tool.cpp @@ -1,949 +1,942 @@ #include "tool.h" -float fcxcy[3]={0,0,0}; -//15 0.1 for kitti lidar, 7 0.1 for nyu -int WINDOWSIZE =15; +float fcxcy[3] = {0, 0, 0}; +// 15 0.1 for kitti lidar, 7 0.1 for nyu +int WINDOWSIZE = 15; float Tthrehold = 0.1; -void -Mat2CvMat(Mat* Input,CvMat * out){*out = *Input;} -void -CvMat2Mat(CvMat* Input,Mat * out){*out = Mat(Input, true);} -void -MkoneDir(char * dirname){ - if(access(dirname, F_OK)==-1){ - bool success_flag=mkdir(dirname, S_IRWXU|S_IRWXG|S_IRWXO); - if(success_flag!=0){ - string Dir_p(dirname); - std::cout<<"E: Error raise in make dir: "<caps; - while(flag){ - string firstname=stdirname.substr(0,stdirname.find_first_of("/")+1); - stdirname=stdirname.substr(stdirname.find_first_of("/")+1,stdirname.size()-1); - caps.push_back(firstname); - if(stdirname.size()<=1) - flag=false; - } - int cengji=caps.size(); - for (int i= 0;i caps; + while (flag) { + string firstname = stdirname.substr(0, stdirname.find_first_of("/") + 1); + stdirname = stdirname.substr(stdirname.find_first_of("/") + 1, stdirname.size() - 1); + caps.push_back(firstname); + if (stdirname.size() <= 1) + flag = false; + } + int cengji = caps.size(); + for (int i = 0; i < cengji; i++) { + string dir_name; + for (int j = 0; j <= i; j++) + dir_name.append(caps[j]); + MkoneDir(dir_name); + // cout<caps; - while(flag){ - string firstname=stdirname.substr(0,stdirname.find_first_of("/")+1); - stdirname=stdirname.substr(stdirname.find_first_of("/")+1,stdirname.size()-1); - caps.push_back(firstname); - if(stdirname.size()<=1) - flag=false; - } - int cengji=caps.size(); - for (int i= 0;i caps; + while (flag) { + string firstname = stdirname.substr(0, stdirname.find_first_of("/") + 1); + stdirname = stdirname.substr(stdirname.find_first_of("/") + 1, stdirname.size() - 1); + caps.push_back(firstname); + if (stdirname.size() <= 1) + flag = false; + } + int cengji = caps.size(); + for (int i = 0; i < cengji; i++) { + string dir_name; + for (int j = 0; j <= i; j++) + dir_name.append(caps[j]); + MkoneDir(dir_name); + // cout< -get_specific_files(std::string path, std::string suffix){ - cout< fileList; - dir = opendir(path.c_str()); - while((ptr = readdir(dir)) != NULL) - { - if(strcmp(ptr->d_name,".") != 0 && strcmp(ptr->d_name,"..") != 0) - { - - string name=path+ptr->d_name; - // cout<<"filename:"< get_specific_files(std::string path, std::string suffix) { + cout << path << endl; + DIR *dir; + struct dirent *ptr; + vector fileList; + dir = opendir(path.c_str()); + while ((ptr = readdir(dir)) != NULL) { + if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) { + + string name = path + ptr->d_name; + // cout<<"filename:"< -ReadDir(string path){ - DIR* dir; - struct dirent* ptr; - vector fileList; - dir = opendir(path.c_str()); - while((ptr = readdir(dir)) != NULL) - { - if(strcmp(ptr->d_name,".") != 0 && strcmp(ptr->d_name,"..") != 0) - { - - string name=path+ptr->d_name; - //cout<<"filename:"< ReadDir(string path) { + DIR *dir; + struct dirent *ptr; + vector fileList; + dir = opendir(path.c_str()); + while ((ptr = readdir(dir)) != NULL) { + if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) { + + string name = path + ptr->d_name; + // cout<<"filename:"< -ReadDir(char* fpath){ - string path(fpath); - DIR* dir; - struct dirent* ptr; - vector fileList; - dir = opendir(path.c_str()); - while((ptr = readdir(dir)) != NULL) - { - if(strcmp(ptr->d_name,".") != 0 && strcmp(ptr->d_name,"..") != 0) - { - - string name=path+ptr->d_name; - //cout<<"filename:"< ReadDir(char *fpath) { + string path(fpath); + DIR *dir; + struct dirent *ptr; + vector fileList; + dir = opendir(path.c_str()); + while ((ptr = readdir(dir)) != NULL) { + if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0) { + + string name = path + ptr->d_name; + // cout<<"filename:"<0)&&(ph0)&&(pw(ph,pw)=input.at(ph,pw); - }return res; - } - - - else{ - input.convertTo(input, CV_8UC3); - Mat res = Mat::zeros(input.size(),CV_8UC3); - for(int i =0;i<500;i++){ - int ph=rand()%h;int pw=rand()%w; - // for (int idx=0; idx<1;idx++) - // for (int idy=0; idy<1;idy++) - { - if(( (ph>0)&&(ph0)&&(pw(ph,pw)=input.at(ph,pw); - } - } - return res; - } - - - + input.convertTo(input, CV_16UC1); + Mat res = Mat::zeros(input.size(), CV_16UC1); + for (int i = 0; i < 500; i++) { + int ph = rand() % h; + int pw = rand() % w; + // for (int idx=0; idx<1;idx++) + // for (int idy=0; idy<1;idy++){ + if (((ph > 0) && (ph < h) && (pw > 0) && (pw < w))) + res.at(ph, pw) = input.at(ph, pw); + } + return res; + } + + else { + input.convertTo(input, CV_8UC3); + Mat res = Mat::zeros(input.size(), CV_8UC3); + for (int i = 0; i < 500; i++) { + int ph = rand() % h; + int pw = rand() % w; + // for (int idx=0; idx<1;idx++) + // for (int idy=0; idy<1;idy++) + { + if (((ph > 0) && (ph < h) && (pw > 0) && (pw < w))) + res.at(ph, pw) = input.at(ph, pw); + } + } + return res; + } } -void -search_plane_neighbor(Mat &img,int i,int j ,float threhold,int* result){ - int cols =img.cols; - int rows =img.rows; - for (int ii=0; ii(i,j); - for (int idx=0; idx= rows || ry>=cols )continue; - if( img.at(rx,ry)==0.0)continue; - if( abs(img.at(rx,ry)-center_depth)<=Tthrehold*center_depth ) - result[idx*WINDOWSIZE+idy]=1; - } +void search_plane_neighbor(Mat &img, int i, int j, float threhold, int *result) { + int cols = img.cols; + int rows = img.rows; + for (int ii = 0; ii < WINDOWSIZE * WINDOWSIZE; ii++) + result[ii] = 0; + float center_depth = img.at(i, j); + for (int idx = 0; idx < WINDOWSIZE; idx++) + for (int idy = 0; idy < WINDOWSIZE; idy++) { + int rx = i - int(WINDOWSIZE / 2) + idx; + int ry = j - int(WINDOWSIZE / 2) + idy; + if (rx >= rows || ry >= cols) + continue; + if (img.at(rx, ry) == 0.0) + continue; + if (abs(img.at(rx, ry) - center_depth) <= Tthrehold * center_depth) + result[idx * WINDOWSIZE + idy] = 1; + } } // Ax+by+cz=D -void -CallFitPlane(const Mat& depth,int * points,int i,int j,float *plane12) { - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - vectorX_vector; - vectorY_vector; - vectorZ_vector; - for(int num_point=0; num_point(point_i, point_j ) * 1.0 / f; - float y = (point_i - cy) * depth.at(point_i, point_j )* 1.0 / f; - float z = depth.at(point_i,point_j); - X_vector.push_back(x); - Y_vector.push_back(y); - Z_vector.push_back(z); - } - CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 - if(X_vector.size()<3){ plane12[0]=-1;plane12[1]=-1;plane12[2]=-1;plane12[3]=-1;return;} - for (int ii=0;ii < X_vector.size(); ++ii){ - points_mat->data.fl[ii * 3 + 0] = X_vector[ii];//矩阵的值进行初始化 X的坐标值 - points_mat->data.fl[ii * 3 + 1] = Y_vector[ii];// Y的坐标值 - points_mat->data.fl[ii * 3 + 2] = Z_vector[ii];// - } - // float plane12[4] = { 0 };//定义用来储存平面参数的数组 - cvFitPlane(points_mat, plane12);//调用方程 - if( telldirection(plane12,i,j,depth.at(i,j)) ){ - plane12[0]=-plane12[0]; - plane12[1]=-plane12[1]; - plane12[2]=-plane12[2];} - X_vector.clear(); - Y_vector.clear(); - Z_vector.clear(); - cvReleaseMat(&points_mat); +void CallFitPlane(const Mat &depth, int *points, int i, int j, float *plane12) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + vector X_vector; + vector Y_vector; + vector Z_vector; + for (int num_point = 0; num_point < WINDOWSIZE * WINDOWSIZE; num_point++) + if (points[num_point] == 1) { // search 已经处理了边界,此处不需要再处理了 + int point_i, point_j; + point_i = floor(num_point / WINDOWSIZE); + point_j = num_point - (point_i * WINDOWSIZE); + point_i += i - int(WINDOWSIZE / 2); + point_j += j - int(WINDOWSIZE / 2); + float x = (point_j - cx) * depth.at(point_i, point_j) * 1.0 / f; + float y = (point_i - cy) * depth.at(point_i, point_j) * 1.0 / f; + float z = depth.at(point_i, point_j); + X_vector.push_back(x); + Y_vector.push_back(y); + Z_vector.push_back(z); + } + CvMat *points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1); //定义用来存储需要拟合点的矩阵 + if (X_vector.size() < 3) { + plane12[0] = -1; + plane12[1] = -1; + plane12[2] = -1; + plane12[3] = -1; + return; + } + for (int ii = 0; ii < X_vector.size(); ++ii) { + points_mat->data.fl[ii * 3 + 0] = X_vector[ii]; //矩阵的值进行初始化 X的坐标值 + points_mat->data.fl[ii * 3 + 1] = Y_vector[ii]; // Y的坐标值 + points_mat->data.fl[ii * 3 + 2] = Z_vector[ii]; // + } + // float plane12[4] = { 0 };//定义用来储存平面参数的数组 + cvFitPlane(points_mat, plane12); //调用方程 + if (telldirection(plane12, i, j, depth.at(i, j))) { + plane12[0] = -plane12[0]; + plane12[1] = -plane12[1]; + plane12[2] = -plane12[2]; + } + X_vector.clear(); + Y_vector.clear(); + Z_vector.clear(); + cvReleaseMat(&points_mat); } -void -cvFitPlane(const CvMat* points, float* plane){ - // Estimate geometric centroid. - int nrows = points->rows; - int ncols = points->cols; - int type = points->type; - CvMat* centroid = cvCreateMat(1, ncols, type); - cvSet(centroid, cvScalar(0)); - for (int c = 0; cdata.fl[c] += points->data.fl[ncols*r + c]; - } - centroid->data.fl[c] /= nrows; - } - // Subtract geometric centroid from each point. - CvMat* points2 = cvCreateMat(nrows, ncols, type); - for (int r = 0; rdata.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c]; - // Evaluate SVD of covariance matrix. - CvMat* A = cvCreateMat(ncols, ncols, type); - CvMat* W = cvCreateMat(ncols, ncols, type); - CvMat* V = cvCreateMat(ncols, ncols, type); - cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T); - cvSVD(A, W, NULL, V, CV_SVD_V_T); - // Assign plane coefficients by singular vector corresponding to smallest singular value. - plane[ncols] = 0; - for (int c = 0; cdata.fl[ncols*(ncols - 1) + c]; - plane[ncols] += plane[c] * centroid->data.fl[c]; - } - // Release allocated resources. - cvReleaseMat(¢roid); - cvReleaseMat(&points2); - cvReleaseMat(&A); - cvReleaseMat(&W); - cvReleaseMat(&V); +void cvFitPlane(const CvMat *points, float *plane) { + // Estimate geometric centroid. + int nrows = points->rows; + int ncols = points->cols; + int type = points->type; + CvMat *centroid = cvCreateMat(1, ncols, type); + cvSet(centroid, cvScalar(0)); + for (int c = 0; c < ncols; c++) { + for (int r = 0; r < nrows; r++) { + centroid->data.fl[c] += points->data.fl[ncols * r + c]; + } + centroid->data.fl[c] /= nrows; + } + // Subtract geometric centroid from each point. + CvMat *points2 = cvCreateMat(nrows, ncols, type); + for (int r = 0; r < nrows; r++) + for (int c = 0; c < ncols; c++) + points2->data.fl[ncols * r + c] = points->data.fl[ncols * r + c] - centroid->data.fl[c]; + // Evaluate SVD of covariance matrix. + CvMat *A = cvCreateMat(ncols, ncols, type); + CvMat *W = cvCreateMat(ncols, ncols, type); + CvMat *V = cvCreateMat(ncols, ncols, type); + cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T); + cvSVD(A, W, NULL, V, CV_SVD_V_T); + // Assign plane coefficients by singular vector corresponding to smallest + // singular value. + plane[ncols] = 0; + for (int c = 0; c < ncols; c++) { + plane[c] = V->data.fl[ncols * (ncols - 1) + c]; + plane[ncols] += plane[c] * centroid->data.fl[c]; + } + // Release allocated resources. + cvReleaseMat(¢roid); + cvReleaseMat(&points2); + cvReleaseMat(&A); + cvReleaseMat(&W); + cvReleaseMat(&V); } -int -telldirection(float * abc,int i,int j,float d){ - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - float x = (j - cx) *d * 1.0 / f; - float y = (i - cy) *d * 1.0 / f; - float z = d; - // Vec3f camera_center=Vec3f(cx,cy,0); - Vec3f cor = Vec3f(0-x, 0-y, 0-z); - Vec3f abcline = Vec3f(abc[0],abc[1],abc[2]); - float corner = cor.dot(abcline); - // float corner =(cx-x)*abc[0]+(cy-y) *abc[1]+(0-z)*abc[2]; - if (corner>=0) - return 1; - else return 0; - +int telldirection(float *abc, int i, int j, float d) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + float x = (j - cx) * d * 1.0 / f; + float y = (i - cy) * d * 1.0 / f; + float z = d; + // Vec3f camera_center=Vec3f(cx,cy,0); + Vec3f cor = Vec3f(0 - x, 0 - y, 0 - z); + Vec3f abcline = Vec3f(abc[0], abc[1], abc[2]); + float corner = cor.dot(abcline); + // float corner =(cx-x)*abc[0]+(cy-y) *abc[1]+(0-z)*abc[2]; + if (corner >= 0) + return 1; + else + return 0; } -void -readTxt(string file,float* fcxcy){ - ifstream infile; - cout.setf(ios::fixed); - infile.open(file.data()); //将文件流对象与文件连接起来 - assert(infile.is_open()); //若失败,则输出错误消息,并终止程序运行 - float f = 0.0 ,cx= 0.0,cy = 0.0,null=0.0; - - infile>>f; - infile>>null; - infile>>cx; - infile>>null; - infile>>null; - infile>>cy; - - // float fcxcy[3]; - fcxcy[0] =f; - fcxcy[1] =cx; - fcxcy[2] =cy; - // return fcxcy; - // cout<> f; + infile >> null; + infile >> cx; + infile >> null; + infile >> null; + infile >> cy; + + // float fcxcy[3]; + fcxcy[0] = f; + fcxcy[1] = cx; + fcxcy[2] = cy; + // return fcxcy; + // cout< -search_working_dir(string inputdir){ - string prefix ="_sync"; - vector input_filepathList= ReadDir(inputdir); - vector workingdirlist ; - int filenum=input_filepathList.size(); - for(int m=0;m search_working_dir(string inputdir) { + string prefix = "_sync"; + vector input_filepathList = ReadDir(inputdir); + vector workingdirlist; + int filenum = input_filepathList.size(); + for (int m = 0; m < filenum; m++) { + string filename = input_filepathList[m]; + string filename_prefix = filename.substr(filename.size() - 5, filename.size() - 1); + if (filename_prefix == prefix) + workingdirlist.push_back(filename); + } + return workingdirlist; } -void -get_dir_para(string inputdir,float * fcxcy){ - string data = inputdir.substr(inputdir.find("2011_"), 10); - if(data=="2011_09_26"){fcxcy[0]=721.5377;fcxcy[1]=596.5593;fcxcy[2]=149.854;} - else if(data=="2011_09_28"){fcxcy[0]=707.0493;fcxcy[1]=604.0814;fcxcy[2]=162.5066;} - else if(data=="2011_09_29"){fcxcy[0]=718.3351 ;fcxcy[1]=600.3891;fcxcy[2]=159.5122;} - else if(data=="2011_09_30"){fcxcy[0]=707.0912;fcxcy[1]=601.8873;fcxcy[2]=165.1104;} - else if(data=="2011_10_03"){fcxcy[0]=718.856 ;fcxcy[1]= 607.1928;fcxcy[2]= 161.2157;} - else{cout<<"Dir data:"<(i,j)==0.0)continue; - //for:nyud train - // if(src.at(i,j)<=4000.0)continue; - - search_plane_neighbor(src,i,j,15.0,plane_points); - CallFitPlane(src,plane_points,i,j,plane12); - Vec3f d = Vec3f(plane12[0],plane12[1],plane12[2]); - Vec3f n = normalize(d); - normals.at(i, j) = n; - } - Mat res = Mat::zeros(src.size(),CV_32FC3); - for (int i=0;i(i, j)[0] = -1.0 * normals.at(i, j)[0]; - res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; - res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; - } +Mat calplanenormal(Mat &src) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + Mat normals = Mat::zeros(src.size(), CV_32FC3); + src.convertTo(src, CV_32FC1); + src *= 1.0; + int cols = src.cols; + int rows = src.rows; + // int plane_points[WINDOWSIZE*WINDOWSIZE]={0}; + int *plane_points = new int[WINDOWSIZE * WINDOWSIZE]; + float *plane12 = new float[4]; + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + // for kitti and nyud test + if (src.at(i, j) == 0.0) + continue; + // for:nyud train + // if(src.at(i,j)<=4000.0)continue; + + search_plane_neighbor(src, i, j, 15.0, plane_points); + CallFitPlane(src, plane_points, i, j, plane12); + Vec3f d = Vec3f(plane12[0], plane12[1], plane12[2]); + Vec3f n = normalize(d); + normals.at(i, j) = n; + } + Mat res = Mat::zeros(src.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] = -1.0 * normals.at(i, j)[0]; + res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; + res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; + } - delete[] plane12; - delete[] plane_points; - normals.release(); - for (int i=0;i(i, j)[0]==0&&res.at(i, j)[1]==0&&res.at(i, j)[2]==0)){ - res.at(i, j)[0] += 1.0 ; - res.at(i, j)[2] += 1.0 ; - res.at(i, j)[1] += 1.0; - } + delete[] plane12; + delete[] plane_points; + normals.release(); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (!(res.at(i, j)[0] == 0 && res.at(i, j)[1] == 0 && + res.at(i, j)[2] == 0)) { + res.at(i, j)[0] += 1.0; + res.at(i, j)[2] += 1.0; + res.at(i, j)[1] += 1.0; } - - res =res * 127.5; - res.convertTo(res, CV_8UC3); - cvtColor(res, res, COLOR_BGR2RGB); - return res; -} + } + res = res * 127.5; + res.convertTo(res, CV_8UC3); + cvtColor(res, res, COLOR_BGR2RGB); + return res; +} -Mat -caldensenormal(Mat & depth){ - float f =fcxcy[0]; - float cx=fcxcy[1]; - float cy=fcxcy[2]; - int rows=depth.rows; - int cols=depth.cols; - Mat normals = Mat::zeros(depth.size(),CV_32FC3); - for (int i=0;i(i, j ) * 1.0 / f; - y = (i - cy) * depth.at(i, j )* 1.0 / f; - z = depth.at(i, j); - if (j == 0 && i == 0){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == 0 && i == rows- 1){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - else if(i == 0 && j ==cols - 1) { - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = -1.0 * Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == cols - 1 && i == rows - 1){ - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = -1.0 * Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - else if (j == 0 || i==0){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == cols- 1){ - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = -1.0 * Vec3d(x1-x, y1-y, z1-z); - y3 = Vec3d(x2-x, y2-y, z2-z); - } - else if(i == rows- 1){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - Vec3f d = x3.cross(y3); - Vec3f n = normalize(d); - normals.at(i, j) = n; - } - else{ - x = (j - cx) * depth.at(i, j ) * 1.0 / f; - y = (i - cy) * depth.at(i, j )* 1.0 / f; - z = depth.at(i, j); - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - Vec3d d = x3.cross(y3); - Vec3d n = normalize(d); - normals.at(i, j) = n; - } +Mat caldensenormal(Mat &depth) { + float f = fcxcy[0]; + float cx = fcxcy[1]; + float cy = fcxcy[2]; + int rows = depth.rows; + int cols = depth.cols; + Mat normals = Mat::zeros(depth.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + float x, y, z, x1, y1, z1, x2, y2, z2; + Vec3f x3, y3; + if (i == 0 || i == rows - 1 || j == 0 || j == cols - 1) { + x = (j - cx) * depth.at(i, j) * 1.0 / f; + y = (i - cy) * depth.at(i, j) * 1.0 / f; + z = depth.at(i, j); + if (j == 0 && i == 0) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == 0 && i == rows - 1) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } else if (i == 0 && j == cols - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = -1.0 * Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == cols - 1 && i == rows - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = -1.0 * Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == 0 || i == 0) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == cols - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = -1.0 * Vec3d(x1 - x, y1 - y, z1 - z); + y3 = Vec3d(x2 - x, y2 - y, z2 - z); + } else if (i == rows - 1) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } + Vec3f d = x3.cross(y3); + Vec3f n = normalize(d); + normals.at(i, j) = n; + } else { + x = (j - cx) * depth.at(i, j) * 1.0 / f; + y = (i - cy) * depth.at(i, j) * 1.0 / f; + z = depth.at(i, j); + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + Vec3d d = x3.cross(y3); + Vec3d n = normalize(d); + normals.at(i, j) = n; + } } - Mat res = Mat::zeros(depth.size(),CV_32FC3); - for (int i=0;i(i, j)[0] = -1.0 * normals.at(i, j)[0]; - res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; - res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; + Mat res = Mat::zeros(depth.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] = -1.0 * normals.at(i, j)[0]; + res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; + res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; } - //--------------------------------------- - for (int i=0;i(i,j)!=0) - { - res.at(i, j)[0] += 1.0 ; - res.at(i, j)[2] += 1.0 ; + //--------------------------------------- + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) + if (depth.at(i, j) != 0) { + res.at(i, j)[0] += 1.0; + res.at(i, j)[2] += 1.0; res.at(i, j)[1] += 1.0; - } - res =res * 127.5; - res.convertTo(res, CV_8UC3); - cvtColor(res, res, COLOR_BGR2RGB); - return res; - - - + } + res = res * 127.5; + res.convertTo(res, CV_8UC3); + cvtColor(res, res, COLOR_BGR2RGB); + return res; } -Mat -rawdepth2normal(Mat & rawdepth,float* paras){ - float f=paras[0]; - float cx=paras[1]; - float cy=paras[2]; - Mat depth; - rawdepth.convertTo(rawdepth,CV_32FC3); - std::vector channels(3); - split(rawdepth, channels); - depth=65536.0*channels[0]+ 256.0*channels[1]+1.0*channels[2]; - Mat normals = Mat::zeros(depth.size(),CV_32FC3); - int rows=depth.rows; - int cols=depth.cols; - for (int i=0;i(i, j ) * 1.0 / f; - y = (i - cy) * depth.at(i, j )* 1.0 / f; - z = depth.at(i, j); - if (j == 0 && i == 0){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == 0 && i == rows- 1){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - else if(i == 0 && j ==cols - 1) { - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = -1.0 * Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == cols - 1 && i == rows - 1){ - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = -1.0 * Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - else if (j == 0 || i==0){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - } - else if (j == cols- 1){ - x1 = (j - 1 - cx) *depth.at(i, j-1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j-1 ) * 1.0 / f; - z1 = depth.at(i, j-1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = -1.0 * Vec3d(x1-x, y1-y, z1-z); - y3 = Vec3d(x2-x, y2-y, z2-z); - } - else if(i == rows- 1){ - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i-1, j ) * 1.0 / f; - y2 = (i - 1 - cy) * depth.at(i-1, j ) * 1.0 / f; - z2 = depth.at(i-1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = -1.0 * Vec3f(x2-x, y2-y, z2-z); - } - Vec3f d = x3.cross(y3); - Vec3f n = normalize(d); - normals.at(i, j) = n; - } - else{ - x = (j - cx) * depth.at(i, j ) * 1.0 / f; - y = (i - cy) * depth.at(i, j )* 1.0 / f; - z = depth.at(i, j); - x1 = (j + 1 - cx) *depth.at(i, j+1 ) * 1.0 / f; - y1 = (i - cy) * depth.at(i, j+1 ) * 1.0 / f; - z1 = depth.at(i, j+1 ); - x2 = (j - cx) * depth.at(i+1, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(i+1, j ) * 1.0 / f; - z2 = depth.at(i+1, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - Vec3d d = x3.cross(y3); - Vec3d n = normalize(d); - normals.at(i, j) = n; - } +Mat rawdepth2normal(Mat &rawdepth, float *paras) { + float f = paras[0]; + float cx = paras[1]; + float cy = paras[2]; + Mat depth; + rawdepth.convertTo(rawdepth, CV_32FC3); + std::vector channels(3); + split(rawdepth, channels); + depth = 65536.0 * channels[0] + 256.0 * channels[1] + 1.0 * channels[2]; + Mat normals = Mat::zeros(depth.size(), CV_32FC3); + int rows = depth.rows; + int cols = depth.cols; + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + float x, y, z, x1, y1, z1, x2, y2, z2; + Vec3f x3, y3; + if (i == 0 || i == rows - 1 || j == 0 || j == cols - 1) { + x = (j - cx) * depth.at(i, j) * 1.0 / f; + y = (i - cy) * depth.at(i, j) * 1.0 / f; + z = depth.at(i, j); + if (j == 0 && i == 0) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == 0 && i == rows - 1) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } else if (i == 0 && j == cols - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = -1.0 * Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == cols - 1 && i == rows - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = -1.0 * Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == 0 || i == 0) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + } else if (j == cols - 1) { + x1 = (j - 1 - cx) * depth.at(i, j - 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j - 1) * 1.0 / f; + z1 = depth.at(i, j - 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = -1.0 * Vec3d(x1 - x, y1 - y, z1 - z); + y3 = Vec3d(x2 - x, y2 - y, z2 - z); + } else if (i == rows - 1) { + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i - 1, j) * 1.0 / f; + y2 = (i - 1 - cy) * depth.at(i - 1, j) * 1.0 / f; + z2 = depth.at(i - 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = -1.0 * Vec3f(x2 - x, y2 - y, z2 - z); + } + Vec3f d = x3.cross(y3); + Vec3f n = normalize(d); + normals.at(i, j) = n; + } else { + x = (j - cx) * depth.at(i, j) * 1.0 / f; + y = (i - cy) * depth.at(i, j) * 1.0 / f; + z = depth.at(i, j); + x1 = (j + 1 - cx) * depth.at(i, j + 1) * 1.0 / f; + y1 = (i - cy) * depth.at(i, j + 1) * 1.0 / f; + z1 = depth.at(i, j + 1); + x2 = (j - cx) * depth.at(i + 1, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(i + 1, j) * 1.0 / f; + z2 = depth.at(i + 1, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + Vec3d d = x3.cross(y3); + Vec3d n = normalize(d); + normals.at(i, j) = n; + } } - Mat res = Mat::zeros(depth.size(),CV_32FC3); - for (int i=0;i(i, j)[0] = -1.0 * normals.at(i, j)[0]; - res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; - res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; + Mat res = Mat::zeros(depth.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] = -1.0 * normals.at(i, j)[0]; + res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; + res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; } - //-----------------test:天空变成朝下(屋顶),便于测试学习效果--------------------- - for (int i=0;i(i, j)==16777215.0){ - res.at(i, j)[0] = 0.0 ; - res.at(i, j)[1] = 0.0 ; + //-----------------test:天空变成朝下(屋顶),便于测试学习效果--------------------- + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (depth.at(i, j) == 16777215.0) { + res.at(i, j)[0] = 0.0; + res.at(i, j)[1] = 0.0; res.at(i, j)[2] = 1.0; - } + } } - //--------------------------------------- - for (int i=0;i(i, j)[0] += 1.0 ; - res.at(i, j)[2] += 1.0 ; - res.at(i, j)[1] += 1.0; + //--------------------------------------- + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] += 1.0; + res.at(i, j)[2] += 1.0; + res.at(i, j)[1] += 1.0; } - res =res * 127.5; - res.convertTo(res, CV_8UC3); - cvtColor(res, res, COLOR_BGR2RGB); - return res; + res = res * 127.5; + res.convertTo(res, CV_8UC3); + cvtColor(res, res, COLOR_BGR2RGB); + return res; } -Mat -GetaSphere(){ - int x=512; - int y=512; - int r=250; - int centerx=x/2; - int centery=y/2; - Mat res= Mat::zeros(x,y,CV_32FC1); - for(int i=centerx-r;i=r*r-5) - res.at(i,j)=0; - else - { float depth= sqrt(r*r-distance2); - res.at(i,j)=depth;} - - } - return res; +Mat GetaSphere() { + int x = 512; + int y = 512; + int r = 250; + int centerx = x / 2; + int centery = y / 2; + Mat res = Mat::zeros(x, y, CV_32FC1); + for (int i = centerx - r; i < centerx + r; i++) + for (int j = centery - r; j < centery + r; j++) { + + float distance2 = (i - centerx) * (i - centerx) + (j - centery) * (j - centery); + if (distance2 >= r * r - 5) + res.at(i, j) = 0; + else { + float depth = sqrt(r * r - distance2); + res.at(i, j) = depth; + } + } + return res; } -void -nearneigbor(Mat& src,int windowsize, Mat * ress){ - int cols =src.cols; - int rows =src.rows; - src.convertTo(src,CV_16UC1); - Mat res1 = Mat::zeros(src.size(),CV_16UC1); - Mat res2 = Mat::zeros(src.size(),CV_8UC1); - for(int i=0;i(i,j)!=0) {res1.at(i,j)=src.at(i,j); continue;} - int rdx=0,rdy=0;float depth=65535256; - for(int m=0;m=rows ||idy>=cols||idx<0||idy<0)continue; - if(src.at(idx,idy)==0)continue; - float rd=sqrt(pow((m-windowsize/2),2)+pow((n-windowsize/2),2)); - if( rd(i,j)=src.at(rdx,rdy); - res2.at(i,j)=(uchar)depth; - } - - } - ress[0]=res1; ress[1]=res2; +void nearneigbor(Mat &src, int windowsize, Mat *ress) { + int cols = src.cols; + int rows = src.rows; + src.convertTo(src, CV_16UC1); + Mat res1 = Mat::zeros(src.size(), CV_16UC1); + Mat res2 = Mat::zeros(src.size(), CV_8UC1); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (src.at(i, j) != 0) { + res1.at(i, j) = src.at(i, j); + continue; + } + int rdx = 0, rdy = 0; + float depth = 65535256; + for (int m = 0; m < windowsize; m++) + for (int n = 0; n < windowsize; n++) { + int idx = i - windowsize / 2 + m, idy = j - windowsize / 2 + n; + if (idx >= rows || idy >= cols || idx < 0 || idy < 0) + continue; + if (src.at(idx, idy) == 0) + continue; + float rd = sqrt(pow((m - windowsize / 2), 2) + pow((n - windowsize / 2), 2)); + if (rd < depth) { + rdx = idx; + rdy = idy; + depth = rd; + } + } + if (rdx * rdy != 0) { + res1.at(i, j) = src.at(rdx, rdy); + res2.at(i, j) = (uchar)depth; + } + } + ress[0] = res1; + ress[1] = res2; } -void -os(Mat& src,int windowsize, Mat * ress){ - int cols =src.cols; - int rows =src.rows; - src.convertTo(src,CV_16UC1); - Mat res1 = Mat::zeros(src.size(),CV_8UC1); - - for(int i=0;i(i,j)==0) { continue;} - int rdx=0,rdy=0;float depth=65535256; - for(int m=0;m=rows ||idy>=cols||idx<0||idy<0||(idx==i&&idy==j))continue; - if(src.at(idx,idy)==0)continue; - float rd=sqrt(pow((m-windowsize/2),2)+pow((n-windowsize/2),2)); - if( rd(i,j)=(uchar)depth; - } - - } - ress[0]=res1; +void os(Mat &src, int windowsize, Mat *ress) { + int cols = src.cols; + int rows = src.rows; + src.convertTo(src, CV_16UC1); + Mat res1 = Mat::zeros(src.size(), CV_8UC1); + + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (src.at(i, j) == 0) { + continue; + } + int rdx = 0, rdy = 0; + float depth = 65535256; + for (int m = 0; m < windowsize; m++) + for (int n = 0; n < windowsize; n++) { + int idx = i - windowsize / 2 + m, idy = j - windowsize / 2 + n; + if (idx >= rows || idy >= cols || idx < 0 || idy < 0 || (idx == i && idy == j)) + continue; + if (src.at(idx, idy) == 0) + continue; + float rd = sqrt(pow((m - windowsize / 2), 2) + pow((n - windowsize / 2), 2)); + if (rd < depth) { + rdx = idx; + rdy = idy; + depth = rd; + } + } + if (rdx * rdy != 0) { + res1.at(i, j) = (uchar)depth; + } + } + ress[0] = res1; } //############################################################### //以前比较菜的求normal的方法，再此处备份，应该不会再用了。 -int -search_neighbor_x(Mat &img,int i,int j,int circle_size ){ - int cols =img.cols; - int rows =img.rows; - int neighbor_x =-1; - int idx = 0; - for (int idx1=0; idx1= rows)continue; - else{ - if(img.at(i+idx , j )!= 0.0){ - neighbor_x = i+ idx; - break; - } - else continue; - } - } - return neighbor_x; +int search_neighbor_x(Mat &img, int i, int j, int circle_size) { + int cols = img.cols; + int rows = img.rows; + int neighbor_x = -1; + int idx = 0; + for (int idx1 = 0; idx1 < circle_size; idx1++) { + idx = idx1 + 1; + if (i + idx >= rows) + continue; + else { + if (img.at(i + idx, j) != 0.0) { + neighbor_x = i + idx; + break; + } else + continue; + } + } + return neighbor_x; } -int -search_neighbor_y(Mat &img,int i,int j,int circle_size ){ - int cols =img.cols; - int rows =img.rows; - int neighbor_y = -1; - int idy = 0; - for (int idy1=0; idy1= cols)continue; - else{ - if(img.at(i , j + idy)!= 0.0){ - neighbor_y = j + idy; - break; - } - else continue; - } - } - return neighbor_y; +int search_neighbor_y(Mat &img, int i, int j, int circle_size) { + int cols = img.cols; + int rows = img.rows; + int neighbor_y = -1; + int idy = 0; + for (int idy1 = 0; idy1 < circle_size; idy1++) { + idy = idy1 + 1; + if (j + idy >= cols) + continue; + else { + if (img.at(i, j + idy) != 0.0) { + neighbor_y = j + idy; + break; + } else + continue; + } + } + return neighbor_y; } -Mat -sparce_depth2normal( Mat &input,float * paras,int circle_size){ - float f=paras[0]; - float cx=paras[1]; - float cy=paras[2]; - Mat normals = Mat::zeros(input.size(),CV_32FC3); - Mat depth=input.clone(); - depth.convertTo(depth,CV_32FC1); - - int cols =depth.cols; - int rows =depth.rows; - - for (int i=0;i< rows;i++) - for (int j=0;j< cols;j++){ - if (depth.at(i,j)!=0.0){ - float x,y,z,x1,y1,z1,x2,y2,z2; - Vec3f x3,y3; - int neighborx = search_neighbor_x(depth,i,j,circle_size); - int neighbory = search_neighbor_y(depth,i,j,circle_size); - if(neighborx == -1||neighbory == -1)continue; - else{ - // cout<(i, j ) * 1.0 / f; - y = (i - cy) * depth.at(i, j )* 1.0 / f; - z = depth.at(i, j); - x1 = (neighbory - cx) *depth.at(i, neighbory) * 1.0 / f; - y1 = (i - cy) * depth.at(i,neighbory ) * 1.0 / f; - z1 = depth.at(i, neighbory); - x2 = (j - cx) * depth.at(neighborx, j ) * 1.0 / f; - y2 = (i + 1 - cy) * depth.at(neighborx, j ) * 1.0 / f; - z2 = depth.at(neighborx, j); - x3 = Vec3f(x1-x, y1-y, z1-z); - y3 = Vec3f(x2-x, y2-y, z2-z); - Vec3d d = x3.cross(y3); - Vec3d n = normalize(d); - normals.at(i, j) = n; - } - } - } - - Mat res = Mat::zeros(depth.size(),CV_32FC3); - for (int i=0;i(i, j)[0] = -1.0 * normals.at(i, j)[0]; - res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; - res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; +Mat sparce_depth2normal(Mat &input, float *paras, int circle_size) { + float f = paras[0]; + float cx = paras[1]; + float cy = paras[2]; + Mat normals = Mat::zeros(input.size(), CV_32FC3); + Mat depth = input.clone(); + depth.convertTo(depth, CV_32FC1); + + int cols = depth.cols; + int rows = depth.rows; + + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (depth.at(i, j) != 0.0) { + float x, y, z, x1, y1, z1, x2, y2, z2; + Vec3f x3, y3; + int neighborx = search_neighbor_x(depth, i, j, circle_size); + int neighbory = search_neighbor_y(depth, i, j, circle_size); + if (neighborx == -1 || neighbory == -1) + continue; + else { + // cout<(i, j) * 1.0 / f; + y = (i - cy) * depth.at(i, j) * 1.0 / f; + z = depth.at(i, j); + x1 = (neighbory - cx) * depth.at(i, neighbory) * 1.0 / f; + y1 = (i - cy) * depth.at(i, neighbory) * 1.0 / f; + z1 = depth.at(i, neighbory); + x2 = (j - cx) * depth.at(neighborx, j) * 1.0 / f; + y2 = (i + 1 - cy) * depth.at(neighborx, j) * 1.0 / f; + z2 = depth.at(neighborx, j); + x3 = Vec3f(x1 - x, y1 - y, z1 - z); + y3 = Vec3f(x2 - x, y2 - y, z2 - z); + Vec3d d = x3.cross(y3); + Vec3d n = normalize(d); + normals.at(i, j) = n; + } + } } - for (int i=0;i(i, j)[0]==0&&res.at(i, j)[1]==0&&res.at(i, j)[2]==0)){ - res.at(i, j)[0] += 1.0 ; - res.at(i, j)[2] += 1.0 ; - res.at(i, j)[1] += 1.0; - } + Mat res = Mat::zeros(depth.size(), CV_32FC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + res.at(i, j)[0] = -1.0 * normals.at(i, j)[0]; + res.at(i, j)[2] = -1.0 * normals.at(i, j)[1]; + res.at(i, j)[1] = -1.0 * normals.at(i, j)[2]; } - res =res * 127.5; - res.convertTo(res, CV_8UC3); - cvtColor(res, res, COLOR_BGR2RGB); - return res; - + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (!(res.at(i, j)[0] == 0 && res.at(i, j)[1] == 0 && + res.at(i, j)[2] == 0)) { + res.at(i, j)[0] += 1.0; + res.at(i, j)[2] += 1.0; + res.at(i, j)[1] += 1.0; + } + } + res = res * 127.5; + res.convertTo(res, CV_8UC3); + cvtColor(res, res, COLOR_BGR2RGB); + return res; } -Vec3b -Fill( int i, int j,Mat &src,int size){ - int w =src.cols; - int h =src.rows; - vector idxs,idys,dsts; - for (int ii = 0;ii= 0 &&idy >= 0) { - if(src.at(idx,idy)[0]!=0 || src.at(idx,idy)[1]!=0 ||src.at(idx,idy)[2]!=0 ){ - int dst = abs(ii- int(size / 2))+abs(jj - int(size / 2)); - idxs.push_back(idx); - idys.push_back(idy); - dsts.push_back(dst); - } - } - else continue; - } - int num =idxs.size(); - if( num ==0 ) - return Vec3b(0,0,0); - else{ - int resdix=0; - int min =size * 100; - for(int g=0;g< num;g++){ - if (dsts[g](idxs[resdix],idys[resdix] ); - - } - - - - - - +Vec3b Fill(int i, int j, Mat &src, int size) { + int w = src.cols; + int h = src.rows; + vector idxs, idys, dsts; + for (int ii = 0; ii < size; ii++) + for (int jj = 0; jj < size; jj++) { + int idx = ii + i - int(size / 2); + int idy = jj + j - int(size / 2); + if (idx < h && idy < w && idx >= 0 && idy >= 0) { + if (src.at(idx, idy)[0] != 0 || src.at(idx, idy)[1] != 0 || + src.at(idx, idy)[2] != 0) { + int dst = abs(ii - int(size / 2)) + abs(jj - int(size / 2)); + idxs.push_back(idx); + idys.push_back(idy); + dsts.push_back(dst); + } + } else + continue; + } + int num = idxs.size(); + if (num == 0) + return Vec3b(0, 0, 0); + else { + int resdix = 0; + int min = size * 100; + for (int g = 0; g < num; g++) { + if (dsts[g] < min) { + min = dsts[g]; + resdix = g; + } + } + return src.at(idxs[resdix], idys[resdix]); + } } -Mat -Interpoletion(Mat & src,int size =8){ - int cols =src.cols; - int rows =src.rows; - Mat res = Mat::zeros(src.size(),CV_8UC3); - for (int i=0;i< rows;i++) - for (int j=0;j< cols;j++){ - if(src.at(i,j)[0]!=0 +src.at(i,j)[1]!=0 +src.at(i,j)[2]!=0)res.at(i,j)=src.at(i,j); - else res.at(i,j)=Fill(i,j,src,size); - } - return res; +Mat Interpoletion(Mat &src, int size = 8) { + int cols = src.cols; + int rows = src.rows; + Mat res = Mat::zeros(src.size(), CV_8UC3); + for (int i = 0; i < rows; i++) + for (int j = 0; j < cols; j++) { + if (src.at(i, j)[0] != 0 + src.at(i, j)[1] != 0 + src.at(i, j)[2] != 0) + res.at(i, j) = src.at(i, j); + else + res.at(i, j) = Fill(i, j, src, size); + } + return res; } -void -get_pic_para(string name,float * fcxcy){ - string data = name.substr(name.find("2011_"), 10); - if(data=="2011_09_26"){fcxcy[0]=721.5377;fcxcy[1]=596.5593;fcxcy[2]=149.854;} - else if(data=="2011_09_28"){fcxcy[0]=707.0493;fcxcy[1]=604.0814;fcxcy[2]=162.5066;} - else if(data=="2011_09_29"){fcxcy[0]=718.3351 ;fcxcy[1]=600.3891;fcxcy[2]=159.5122;} - else if(data=="2011_09_30"){fcxcy[0]=707.0912;fcxcy[1]=601.8873;fcxcy[2]=165.1104;} - else if(data=="2011_10_03"){fcxcy[0]=718.856 ;fcxcy[1]= 607.1928;fcxcy[2]= 161.2157;} - else{cout<<"Dir data:"< 0) { @@ -953,13 +946,13 @@ get_pic_para(string name,float * fcxcy){ // else { // fprintf(stderr, "E: Invalid program argument: %s", *argv); // return 0; -// } +// } // } // else { // fprintf(stderr, "E: Invalid program argument: %s", *argv); // return 0; // } -// argv++; argc--; +// argv++; argc--; // } // return 1; // } \ No newline at end of file diff --git a/tool.h b/tool.h index 672ccb7..20184f4 100644 --- a/tool.h +++ b/tool.h @@ -1,30 +1,28 @@ #ifndef TOOL #define TOOL +#include +#include +#include +#include +#include #include -#include #include -#include -#include +#include #include +#include +#include #include -#include #include #include -#include -#include -#include -#include -#include using namespace cv; using namespace std; -template - Type stringToNum(const std::string& str){ - std::istringstream iss(str); - Type num; - iss >> num; - return num; - } +template Type stringToNum(const std::string &str) { + std::istringstream iss(str); + Type num; + iss >> num; + return num; +} // string Int_to_String(int n) // { @@ -38,43 +36,43 @@ template //} //-- basic toolbox------ //---------------------- -void MkoneDir(char * dirname); +void MkoneDir(char *dirname); void MkoneDir(string stdirname); -void Mat2CvMat(Mat* Input,CvMat * out); -void CvMat2Mat(CvMat* Input,Mat * out); +void Mat2CvMat(Mat *Input, CvMat *out); +void CvMat2Mat(CvMat *Input, Mat *out); vector ReadDir(string path); -vector ReadDir(char* fpath); +vector ReadDir(char *fpath); void MkDir(string stdirname); -void MkDir(char * char_stdirname); +void MkDir(char *char_stdirname); //根据特定后缀获得文件 std::vector get_specific_files(std::string path, std::string suffix); -//temp test function -Mat closecheck( Mat &raw); -Mat rawdepth2normal(Mat & rawdepth,float * paras); +// temp test function +Mat closecheck(Mat &raw); +Mat rawdepth2normal(Mat &rawdepth, float *paras); //-- sparcenormal toolbox------ //----------------------------- extern float fcxcy[3]; -extern int WINDOWSIZE; +extern int WINDOWSIZE; extern float Tthrehold; //生成一个球形物体的深度图用来求normal标称方向 Mat GetaSphere(); Mat sample(Mat input); -void readTxt(string file,float* fcxcy); -void cvFitPlane(const CvMat* points, float* plane); -void CallFitPlane(const Mat& depth,int * points,int i,int j,float *plane12); -void search_plane_neighbor(Mat &img,int i,int j ,float threhold,int* result); -int telldirection(float * abc,int i,int j,float d); +void readTxt(string file, float *fcxcy); +void cvFitPlane(const CvMat *points, float *plane); +void CallFitPlane(const Mat &depth, int *points, int i, int j, float *plane12); +void search_plane_neighbor(Mat &img, int i, int j, float threhold, int *result); +int telldirection(float *abc, int i, int j, float d); vector search_working_dir(string inputdir); -void get_dir_para(string inputdir,float * fcxcy); +void get_dir_para(string inputdir, float *fcxcy); Mat calplanenormal(Mat &src); -Mat caldensenormal(Mat & rawdept); +Mat caldensenormal(Mat &rawdept); //------------------lidar-combine------------------------ //最近邻插值同时生成一张显示最近邻的欧式距离的可信度图 -void nearneigbor(Mat& src,int windowsize, Mat * ress); -int search_neighbor_x(Mat &img,int i,int j,int circle_size ); -int search_neighbor_y(Mat &img,int i,int j,int circle_size ); -Mat sparce_depth2normal( Mat &input,float * paras,int circle_size); +void nearneigbor(Mat &src, int windowsize, Mat *ress); +int search_neighbor_x(Mat &img, int i, int j, int circle_size); +int search_neighbor_y(Mat &img, int i, int j, int circle_size); +Mat sparce_depth2normal(Mat &input, float *paras, int circle_size); //误差欧式距离权重法 -void os(Mat& src,int windowsize, Mat * ress); +void os(Mat &src, int windowsize, Mat *ress); #endif