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();//防止运行过程中无响应，可能影响处理速度，暂时屏蔽
}//>
}
