void Map::voxelizeMap(void) { point pt; for (int y = 0; y < CUBE_SIZE; y++) { for (int x = 0; x < CUBE_SIZE; x++) { pt = interPoint(x, y); for (int l = 0; l < pt.z; l++) { this->_vox[l][y][x] = voxel(voxel::SOIL, l); this->_hMap[x + y * CUBE_SIZE] = pt.z; } } } }
void MFReconstruct::triangulation(cv::vector<double> *cam1Pixels, cv::vector<double> *cam2Pixels) { int width = cameraWidth; int height = cameraHeight; cv::Mat matCoordTrans(3,4,CV_32F);//定义变换矩阵将当前次扫描坐标系对齐至首次扫描坐标系 if (scanSN > 0){ ///加载刚体变换矩阵 QString loadPath = savePath_ + "/scan/transfer_mat" + QString::number(scanSN) + ".txt"; VirtualCamera *cam = new VirtualCamera(); cam->loadMatrix(matCoordTrans, 3, 4, loadPath.toStdString()); delete cam; } /* ///遍历图像纵坐标 for (int i = 0; i < height;i++){//< int kstart = 0;//表示每次遍历k(右图像横坐标)时的起点,在k循环找到匹配后更新为匹配k值 int lastMatchedJ = 0;//表示匹配成功的上一个J值 ///遍历左图像横坐标 for (int j = 0;j < width;j++){//<< cv::vector<double> cam1Pix = cam1Pixels[i * width + j];//获得左图像该点相位值,注意cam1Pix是一个向量 if (cam1Pix.size() == 0)//左图像该点未被赋相位值,略过 continue; int bestK; bool rightMatched = false;//表示该行最左侧点是否已经匹配,即bestK是否存在 double epixel = 0.1;//表示左右像素点相位值的初始匹配偏差,每处理一个j点刷新一次 ///遍历右图像横坐标 for (int k = kstart;k < width;k++){//<<< cv::vector<double> cam2Pix = cam2Pixels[i * width + k]; if (cam2Pix.size() == 0)//右图像该点未被赋相位值,略过 continue; double eCurrentJ = fabs(cam1Pix[0] - cam2Pix[0]); if (eCurrentJ <= epixel){ ///说明左相机(j,i)点与右相机(k,i)点匹配误差较原有值小,则(k,i)点更有可能是(j,i)点的匹配点,故将 /// k存入最佳匹配缓存 bestK = k; epixel = eCurrentJ; rightMatched = true; } else{ ///当相位误差有增大趋势时,由相位的单调变化规律可知,k点之后不可能再有匹配点,故break kstart = k; break; } }//>>> if (rightMatched){ ///对可能出现的左图像点误匹配右图像点情况进行处理 if (lastMatchedJ == 0){ if (bestK - kstart > 1){ kstart = 2*kstart - bestK;//将kstart恢复至误匹配以前的状态 rightMatched = false; break;//跳出j循环 } } else{ if (bestK - kstart > j - lastMatchedJ + 1){ kstart = 2*kstart - bestK;//将kstart恢复至误匹配以前的状态 rightMatched = false; break;//跳出j循环 } } lastMatchedJ = j; ///以左图像该点二维坐标、对应点视差构建该点二维齐次坐标 double point2D[] = {j, i, j - bestK, 1};//二维坐标 cv::Mat p2D = cv::Mat(4,1,CV_64F,point2D);//构建坐标矩阵 cv::Mat p3D; p3D = sr->Q * p2D;//此处调试以观察是否正确计算 double x = p3D.at<double>(0,0); double y = p3D.at<double>(1,0); double z = p3D.at<double>(2,0); double w = p3D.at<double>(3,0); double ax = x/w; double ay = y/w; double az = z/w; cv::Point3f interPoint(ax,ay,az); cv::Point3f refinedPoint; ///以下判断为多次重建得到的点云拼接做准备 if (scanSN > 0){ float point[] = {interPoint.x, interPoint.y, interPoint.z, 1}; cv::Mat pointMat(4, 1, CV_32F, point); cv::Mat refineMat(3, 1, CV_32F); refineMat = matCoordTrans * pointMat; refinedPoint.x = refineMat.at<float>(0, 0); refinedPoint.y = refineMat.at<float>(1, 0); refinedPoint.z = refineMat.at<float>(2, 0); } else refinedPoint = interPoint; points3DProjView->addPoint(i, j, refinedPoint); } }//>> // qApp->processEvents();//防止运行过程中无响应,可能影响处理速度,暂时屏蔽 }//> */ ///遍历图像纵坐标 for (int i = 0; i < height;i++){//< ///遍历左图像横坐标 for (int j = 0;j < width;j++){//<< cv::vector<double> cam1Pix = cam1Pixels[i * width + j];//获得左图像该点相位值,注意cam1Pix是一个向量 if (cam1Pix.size() == 0)//左图像该点未被赋相位值,略过 continue; int bestK; bool rightMatched = false;//表示该行最左侧点是否已经匹配,即bestK是否存在 double epixel = 1.0;//表示左右像素点相位值的初始匹配偏差,每处理一个j点刷新一次 ///遍历右图像横坐标 for (int k = 0;k < width;k++){//<<< cv::vector<double> cam2Pix = cam2Pixels[i * width + k]; if (cam2Pix.size() == 0)//右图像该点未被赋相位值,略过 continue; double eCurrentJ = fabs(cam1Pix[0] - cam2Pix[0]); if (eCurrentJ <= epixel){ ///说明左相机(j,i)点与右相机(k,i)点匹配误差较原有值小,则(k,i)点更有可能是(j,i)点的匹配点,故将 /// k存入最佳匹配缓存 bestK = k; epixel = eCurrentJ; rightMatched = true; } else{ ///当相位误差有增大趋势时,由相位的单调变化规律可知,k点之后不可能再有匹配点,故break if (rightMatched) break; } }//>>> if (rightMatched){ ///以左图像该点二维坐标、对应点视差构建该点二维齐次坐标 double point2D[] = {j, i, j - bestK, 1};//二维坐标 cv::Mat p2D = cv::Mat(4,1,CV_64F,point2D);//构建坐标矩阵 cv::Mat p3D; p3D = sr->Q * p2D;//此处调试以观察是否正确计算 double x = p3D.at<double>(0,0); double y = p3D.at<double>(1,0); double z = p3D.at<double>(2,0); double w = p3D.at<double>(3,0); double ax = x/w; double ay = y/w; double az = z/w; cv::Point3f interPoint(ax,ay,az); cv::Point3f refinedPoint; ///以下判断为多次重建得到的点云拼接做准备 if (scanSN > 0){ float point[] = {interPoint.x, interPoint.y, interPoint.z, 1}; cv::Mat pointMat(4, 1, CV_32F, point); cv::Mat refineMat(3, 1, CV_32F); refineMat = matCoordTrans * pointMat; refinedPoint.x = refineMat.at<float>(0, 0); refinedPoint.y = refineMat.at<float>(1, 0); refinedPoint.z = refineMat.at<float>(2, 0); } else refinedPoint = interPoint; if (haveColor){ int val = camImgs[0].at<uchar>(i,j); cv::Vec3i graycolor = cv::Vec3i(val,val,val); points3DProjView->addPoint(j, i, refinedPoint, graycolor); } else points3DProjView->addPoint(j, i, refinedPoint); // if (!addp) // QMessageBox::information(NULL,"fff","Add Point fail"); } }//>> // qApp->processEvents();//防止运行过程中无响应,可能影响处理速度,暂时屏蔽 }//> }