void HumanBodyLaser::saveFinalDispInfos(const int lvl) { string savefn; string lvlstr = type2string(lvl); float disscl = stereoPyr[lvl]->GetDisScale(); Mat dispim; savefn = outdir + "/disp_" + type2string(lvl) + ".jpg"; (stereoPyr[lvl]->GetDisp()).convertTo(dispim, CV_8UC1, disscl); imwrite(savefn, dispim); savefn = outdir + "/disp_" + type2string(lvl) + ".yml"; saveDispYML(savefn, stereoPyr[lvl]->GetDisp()); savefn = outdir + "/matches_" + type2string(lvl) + ".list"; saveAllMatches(savefn, stereoPyr[lvl]->GetDisp(), stereoPyr[lvl]->GetBestMcost(), stereoPyr[lvl]->GetBestPrior()); savefn = outdir + "/mcost_" + type2string(lvl) + ".yml"; saveBestCostYML(savefn, stereoPyr[lvl]->GetBestMcost()); savefn = outdir + "/display_disp_" + type2string(lvl) + ".jpg"; Mat msk = stereoPyr[lvl]->GetMskL(); Mat erode_msk = erodeMask(msk, stereoPyr[lvl]->GetWsize() * 2 + 5); Mat erode_dispim; dispim.copyTo(erode_dispim, erode_msk); imwrite(savefn, erode_dispim); }
void HumanBodyLaser::compute3DPoints() { cout << "\nCompute 3D points from the disparity result." << endl; Mat mask = erodeMask(mskL, (stereoPyr[0]->GetWsize())*2); Mat disp = stereoPyr[0]->GetDisp(); vector<Point3f> points(0); vector<Vec3b> colors(0); vector<Vec3f> normals(0); float offsetD = stPointL.x - stPointR.x; int height = imSize.height; int width = imSize.width; int validnum = 0; for(int y = 0; y < height; ++y) { float * pdsp = (float *)disp.ptr<float>(y); uchar * pmsk = (uchar *)mask.ptr<uchar>(y); for(int x = 0; x < width; ++x) { if (pmsk[x] == 0) { pdsp[x] = 0; continue; } if (pdsp[x] != 0) { pdsp[x] += offsetD; validnum ++; } } } Mat colorImL; imL.convertTo(colorImL, CV_8U, 255); //RGB Disp2Point(disp, stPointL, colorImL, mask, QMatrix, points, colors, normals); //refine3DPoints(points, colors, validnum); //string savefn = outdir + "/pointcloud_" + camL + camR + "_" + frame + ".ply"; string savefn = "d_pointcloud_" + camL + camR + "_" + frame + ".ply"; writePLY(savefn, width, height, validnum, PT_HAS_COLOR|PT_HAS_NORMAL, points, colors, normals); cout << "\nsave " << savefn << " done. " << validnum << " Points." << endl; }
void HumanBodyLaser::BestDEBUG() { /*stereoPyr[tlvl]->GetDispL().convertTo(testim, CV_8UC1, stereoPyr[tlvl]->GetDisScale()); imshow("testim", testim); waitKey(0); destroyWindow("testim");*/ cout << "\n\t * DEBUG * " << endl; /*int tlvl = 0; int tlvlsz = 2 * 5 + 1;*/ int tlvl = 0; int tlvlsz = 2 * (5 - tlvl) + 1; stereoPyr[tlvl]->setWsize(tlvlsz); stereoPyr[tlvl]->setMeanLR(); stereoPyr[tlvl]->setPatchParams(tlvlsz + 2); stereoPyr[tlvl]->setVotingParams(tlvlsz); float disscl = stereoPyr[tlvl]->GetDisScale(); string tlvlstr = type2string(tlvl), fn; Mat tim; Mat timK; Mat testim; double maxk, mink; fn = outdir + "/d_rematch_dispL_0.jpg"; testim = imread(fn, CV_LOAD_IMAGE_GRAYSCALE); tim = erodeMask(stereoPyr[tlvl]->GetMskL(), stereoPyr[tlvl]->GetWsize() * 8); Mat saveim; testim.copyTo(saveim, tim); fn = outdir + "/d_erode_dispL_0.jpg"; imwrite(fn, saveim); exit(1); fn = outdir + "/init_disp_0.yml"; readInDispYML(fn, tim); Mat timR = Mat::zeros(tim.size(), CV_32FC1); Mat tmskR = stereoPyr[tlvl]->GetMskR(); Mat tmskL = stereoPyr[tlvl]->GetMskL(); int H = timR.size().height; int W = timR.size().width; for(int y = 0; y < H; ++y) { uchar * pmL = (uchar *)tmskL.ptr<uchar>(y); uchar * pmR = (uchar *)tmskR.ptr<uchar>(y); float * pdisL = (float *)tim.ptr<float>(y); float * pdisR = (float *)timR.ptr<float>(y); for(int x = 0; x < W; ++x) { if(pmL[x] == 0) continue; if(pdisL[x] == 0.f) continue; float d = pdisL[x]; int rx = x - (int)d; if(rx < 0 || rx > W) continue; if(pmR[rx] == 0) continue; pdisR[rx] = d; } } tim.convertTo(testim, CV_8UC1, disscl ); imwrite("d_dispL_0.jpg", testim); timR.convertTo(testim, CV_8UC1, disscl); imwrite("d_dispR_0.jpg", testim); stereoPyr[tlvl]->setDispL(tim); stereoPyr[tlvl]->setDispR(timR); Matfloat2uint(tim, timK); stereoPyr[tlvl]->setBestKL(timK); Matfloat2uint(timR, timK); stereoPyr[tlvl]->setBestKR(timK); stereoPyr[tlvl]->rematch(0); stereoPyr[tlvl]->rematch(1); fn = "d_rematch_dispL_" + tlvlstr + ".jpg"; (stereoPyr[tlvl]->GetDispL()).convertTo(testim, CV_8UC1, disscl); imwrite(fn, testim); fn = "d_rematch_dispL_" + tlvlstr + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDispL()); fn = "d_rematch_dispR_" + tlvlstr + ".jpg"; (stereoPyr[tlvl]->GetDispR()).convertTo(testim, CV_8UC1, disscl); imwrite(fn, testim); fn = "d_rematch_dispR_" + tlvlstr + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDispR()); exit(1); /*--------------------------------------------------------------------------------------------------------------------*/ /* disparity refinement : region voting */ /*-------------------------------------------------------------------------------------------------------------------*/ cout << "\n\tdisparity general refinement ." << endl; /*stereoPyr[tlvl]->detectOutliers(minDisp-DISP_OCCLUSION, minDisp-DISP_MISMATCH, 0); fn = "d_rematch_dispL_" + tlvlstr + ".jpg"; testim = imread(fn, CV_LOAD_IMAGE_COLOR); fn = "d_outliers_rematch_dispL_" + tlvlstr + ".jpg"; saveOutliers(fn, testim, stereoPyr[tlvl]->GetOutliersL()); fn = "d_rematch_dispR_" + tlvlstr + ".jpg"; testim = imread(fn, CV_LOAD_IMAGE_COLOR); fn = "d_outliers_rematch_dispR_" + tlvlstr + ".jpg"; saveOutliers(fn, testim ,stereoPyr[tlvl]->GetOutliersR()); fn = "d_outlierL_rematch_" + tlvlstr + ".yml"; saveOutliersYML(fn, stereoPyr[tlvl]->GetOutliersL()); fn = "d_outlierR_rematch_" + tlvlstr + ".yml"; saveOutliersYML(fn, stereoPyr[tlvl]->GetOutliersR()); */ fn = "d_dispL_" + tlvlstr + ".jpg"; testim = imread(fn, CV_LOAD_IMAGE_COLOR); tim = Mat::zeros(testim.size(), CV_8UC1); Mat msk = stereoPyr[tlvl]->GetMskL(); Mat disp = stereoPyr[tlvl]->GetDispL(); for(int y = 0; y < H; ++y) { uchar * pm = (uchar *)msk.ptr<uchar>(y); uchar * pt = (uchar *)tim.ptr<uchar>(y); float * pd = (float *)disp.ptr<float>(y); for(int x = 0; x < W; ++x) { if(pm[x] == 0) continue; if(pd[x] == 0) pt[x] = 1; } } stereoPyr[tlvl]->setOutlierL(tim); fn = "d_outliers_dispL_" + tlvlstr + ".jpg"; saveOutliers(fn, testim, stereoPyr[tlvl]->GetOutliersL()); fn = "d_dispR_" + tlvlstr + ".jpg"; testim = imread(fn, CV_LOAD_IMAGE_COLOR); tim = Mat::zeros(testim.size(), CV_8UC1); msk = stereoPyr[tlvl]->GetMskR(); disp = stereoPyr[tlvl]->GetDispR(); for(int y = 0; y < H; ++y) { uchar * pm = (uchar *)msk.ptr<uchar>(y); uchar * pt = (uchar *)tim.ptr<uchar>(y); float * pd = (float *)disp.ptr<float>(y); for(int x = 0; x < W; ++x) { if(pm[x] == 0) continue; if(pd[x] == 0) pt[x] = 1; } } stereoPyr[tlvl]->setOutlierR(tim); fn = "d_outliers_dispR_" + tlvlstr + ".jpg"; saveOutliers(fn, testim ,stereoPyr[tlvl]->GetOutliersR()); stereoPyr[tlvl]->calPatchBorders(); stereoPyr[tlvl]->regionVoting(); fn = "d_rv_dispL_" + type2string(tlvl) + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDispL()); fn = "d_rv_dispL_" + type2string(tlvl) + ".jpg"; (stereoPyr[tlvl]->GetDispL()).convertTo(testim, CV_8UC1, disscl); imwrite(fn, testim); testim = imread(fn, CV_LOAD_IMAGE_COLOR); fn = "d_outliers_rv_dispL_" + type2string(tlvl) + ".jpg"; saveOutliers(fn, testim, stereoPyr[tlvl]->GetOutliersL()); fn = "d_rv_dispR_" + type2string(tlvl) + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDispR()); fn = "d_rv_dispR_" + type2string(tlvl) + ".jpg"; (stereoPyr[tlvl]->GetDispR()).convertTo(testim, CV_8UC1, disscl); imwrite(fn, testim); testim = imread(fn, CV_LOAD_IMAGE_COLOR); fn = "d_outliers_rv_dispR_" + type2string(tlvl) + ".jpg"; saveOutliers(fn, testim ,stereoPyr[tlvl]->GetOutliersR()); /*--------------------------------------------------------------------------------------------------------------------*/ /* save results */ /*-------------------------------------------------------------------------------------------------------------------*/ stereoPyr[tlvl]->saveDisparity(); fn = "d_disp_" + type2string(tlvl) + ".jpg"; (stereoPyr[tlvl]->GetDisp()).convertTo(testim, CV_8UC1, disscl); imwrite(fn, testim); fn = "d_disp_" + type2string(tlvl) + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDisp()); stereoPyr[tlvl]->beelerRefine(); fn = "d_refined_subpixel_disp_" + type2string(tlvl) + ".yml"; saveDispYML(fn, stereoPyr[tlvl]->GetDisp()); // fn = outdir + "/rv_dispL_" + type2string(tlvl) + ".yml"; // readInDispYML(fn, tim); // stereoPyr[tlvl]->setDispL(tim); // fn = outdir + "/rv_dispR_" + type2string(tlvl) + ".yml"; // readInDispYML(fn, tim); // stereoPyr[tlvl]->setDispR(tim); // // stereoPyr[tlvl]->beelerRefine(); // // printf( "\n\t--------------------------------------------------------\n" ); // printf( "\trefined by Beeler's formula. \n"); // printf( "\t--------------------------------------------------------\n" ); //#ifdef _DEBUG // fn = "refined_subpixel_disp_" + type2string(tlvl) + ".yml"; // saveDispYML(fn, stereoPyr[tlvl]->GetDisp()); //#endif /*fn = outdir + "/refined_subpixel_disp_" + type2string(tlvl) + ".yml"; readInDispYML(fn, tim); stereoPyr[tlvl]->setDisp(tim);*/ return; //re-matching //fn = outdir + "/propagate_dispL_" + tlvlstr + ".yml"; //readInDispYML(fn, tim); //stereoPyr[tlvl]->setDispL(tim); //Matfloat2uint(tim, timK); //stereoPyr[tlvl]->setBestKL(timK); //fn = outdir + "/propagate_dispR_" + tlvlstr + ".yml"; //readInDispYML(fn, tim); //stereoPyr[tlvl]->setDispR(tim); //Matfloat2uint(tim, timK); //stereoPyr[tlvl]->setBestKR(timK); ////rematch //stereoPyr[tlvl]->rematch(0); //stereoPyr[tlvl]->rematch(1); // //Mat dispim; //fn = "rematch_dispL_" + tlvlstr + ".jpg"; //(stereoPyr[tlvl]->GetDispL()).convertTo(dispim, CV_8UC1, disscl); //imwrite(fn, dispim); // //fn = "rematch_dispL_" + tlvlstr + ".yml"; //saveDispYML(fn, stereoPyr[tlvl]->GetDispL()); //fn = "rematch_dispR_" + tlvlstr + ".jpg"; //(stereoPyr[tlvl]->GetDispR()).convertTo(dispim, CV_8UC1, disscl); //imwrite(fn, dispim); //fn = "rematch_dispR_" + tlvlstr + ".yml"; //saveDispYML(fn, stereoPyr[tlvl]->GetDispR()); ////bestk的访问方式 //exit(1); //scanline optimization fn = outdir + "/rematch_dispL_" + tlvlstr + ".yml"; readInDispYML(fn, tim); stereoPyr[tlvl]->setDispL(tim); fn = outdir + "/rematch_dispR_" + tlvlstr + ".yml"; readInDispYML(fn ,tim); stereoPyr[tlvl]->setDispR(tim); fn = outdir + "/rematch_KL_" + tlvlstr + ".yml"; readInBestkYML(fn, tim); stereoPyr[tlvl]->setBestKL(tim); fn = outdir + "/rematch_KR_" + tlvlstr + ".yml"; readInBestkYML(fn, tim); stereoPyr[tlvl]->setBestKR(tim); fn = outdir + "/rematch_mcostL_" + tlvlstr + ".yml"; readInBestCostYML(fn, tim); stereoPyr[tlvl]->setBestCostL(tim); fn = outdir + "/rematch_mcostR_" + tlvlstr + ".yml"; readInBestCostYML(fn, tim); stereoPyr[tlvl]->setBestCostR(tim); fn = outdir + "/rematch_priorL_" + tlvlstr + ".yml"; readInBestPriorYML(fn, tim); fn = outdir + "/rematch_priorR_" + tlvlstr + ".yml"; readInBestPriorYML(fn, tim); stereoPyr[tlvl]->setBestPriorR(tim); int disRanges = stereoPyr[tlvl]->GetDisRanges(); Mat * tcostvolL = new Mat[disRanges+1]; Mat * tcostvolR = new Mat[disRanges+1]; for(int k = 0; k<=disRanges; ++k) { fn = outdir + "/mcost_" + tlvlstr + "_0" + type2string(k) + ".yml"; readInBestCostYML(fn, tim); tcostvolL[k] = tim.clone(); fn = outdir + "/mcost_" + tlvlstr + "_1" + type2string(k) + ".yml"; readInBestCostYML(fn, tim); tcostvolR[k] = tim.clone(); } cout << "read in dispL/R, bestCostL/R, bestKL/R, bestPriorL/R, costVolL/R done." << endl; stereoPyr[tlvl]->setCostVol(0, tcostvolL); stereoPyr[tlvl]->setCostVol(1, tcostvolR); stereoPyr[tlvl]->scanlineOptimization(0); stereoPyr[tlvl]->scanlineOptimization(1); Mat dispim; string savefn; savefn = "so_dispL_" + tlvlstr + ".jpg"; stereoPyr[tlvl]->GetDispL().convertTo(dispim, CV_8UC1, disscl); imwrite(savefn, dispim); savefn = "so_dispR_" + tlvlstr + ".jpg"; stereoPyr[tlvl]->GetDispR().convertTo(dispim, CV_8UC1, disscl); imwrite(savefn, dispim); exit(1); }
//static void GradientsMergeMosaic::mergeMosaicProcessImage(imageType_t const & rImage_p, realType_t dBlackPoint_p, pcl_enum eType_p, int32 shrinkCount_p, int32 featherRadius_p, imageType_t &rSumImageDx_p, imageType_t &rSumImageDy_p, sumMaskImageType_t &rSumMaskImage_p, weightImageType_t &rCountImageDx_p, weightImageType_t &rCountImageDy_p) { #ifdef DEBUG int const nCols=rImage_p.Width(); int const nRows=rImage_p.Height(); int const nChannels=rImage_p.NumberOfChannels(); #endif Assert(nCols==rSumImageDx_p.Width()+1); Assert(nRows==rSumImageDx_p.Height()); Assert(nChannels==rSumImageDx_p.NumberOfChannels()); Assert(nCols==rSumImageDy_p.Width()); Assert(nRows==rSumImageDy_p.Height()+1); Assert(nChannels==rSumImageDy_p.NumberOfChannels()); Assert(nCols==rSumMaskImage_p.Width()); Assert(nRows==rSumMaskImage_p.Height()); Assert(1==rSumMaskImage_p.NumberOfChannels()); Assert(eType_p!=GradientsMergeMosaicType::Average || nCols==rCountImageDx_p.Width()+1); Assert(eType_p!=GradientsMergeMosaicType::Average || nRows==rCountImageDx_p.Height()); Assert(eType_p!=GradientsMergeMosaicType::Average || 1==rCountImageDx_p.NumberOfChannels()); Assert(eType_p!=GradientsMergeMosaicType::Average || nCols==rCountImageDy_p.Width()); Assert(eType_p!=GradientsMergeMosaicType::Average || nRows==rCountImageDy_p.Height()+1); Assert(eType_p!=GradientsMergeMosaicType::Average || 1==rCountImageDy_p.NumberOfChannels()); Assert(shrinkCount_p>=0); Assert(featherRadius_p>=0); Assert(dBlackPoint_p>=0.0); weightImageType_t maskImage; TimeMessage startAddImage("Adding image to data"); TimeMessage startBinarize("Binarize Image"); binarizeImage(rImage_p,dBlackPoint_p,maskImage); // save this for border computation later weightImageType_t fullMask(maskImage); startBinarize.Stop(); // we are doing this because image after StarAlign usually contain aliased pixels. // These must not to be used during merge. TimeMessage startShrink("Shrinking mask"); erodeMask(maskImage,shrinkCount_p); startShrink.Stop(); TimeMessage startFeather("Feathering mask"); featherMask(maskImage,featherRadius_p); startFeather.Stop(); TimeMessage startBorder("Computing border"); addBorder(fullMask,maskImage); fullMask.AllocateData(0,0); // save memory startBorder.Stop(); TimeMessage startSumMask("Creating combined mask"); addToMask(maskImage,eType_p,rSumMaskImage_p); startSumMask.Stop(); TimeMessage startAddGradients("Adding gradients data"); addToImage(rImage_p,eType_p,maskImage,rSumImageDx_p,rSumImageDy_p,rCountImageDx_p, rCountImageDy_p); startAddGradients.Stop(); }
//static void GradientsHdrComposition::hdrCompositionProcessImage( imageType_t const & rImage_p, int imageNum_p, realType_t dBias_p, realType_t dBlackPoint_p, int32 shrinkCount_p, imageType_t &rSumImageDx_p, imageType_t &rSumImageDy_p, numImageType_t &rDxImage_p, numImageType_t &rDyImage_p) { #ifdef DEBUG int const nCols=rImage_p.Width(); int const nRows=rImage_p.Height(); int const nChannels=rImage_p.NumberOfChannels(); #endif Assert(nCols==rSumImageDx_p.Width()+1); Assert(nRows==rSumImageDx_p.Height()); Assert(nChannels==rSumImageDx_p.NumberOfChannels()); Assert(nCols==rSumImageDy_p.Width()); Assert(nRows==rSumImageDy_p.Height()+1); Assert(nChannels==rSumImageDy_p.NumberOfChannels()); Assert(nCols==rDxImage_p.Width()); Assert(nRows==rDxImage_p.Height()); Assert(nChannels==rDxImage_p.NumberOfChannels()); Assert(nCols==rDyImage_p.Width()); Assert(nRows==rDyImage_p.Height()); Assert(nChannels==rDyImage_p.NumberOfChannels()); Assert(shrinkCount_p>=0); Assert(dBlackPoint_p>=0.0); TimeMessage startAddImage("Adding image to data"); TimeMessage startBiasImage("Applying bias()"); imageType_t loggedImage=rImage_p; loggedImage.ResetSelections(); loggedImage-=dBias_p; startBiasImage.Stop(); TimeMessage startLogImage("Applying log()"); logImage(loggedImage,loggedImage); startLogImage.Stop(); // binarize image to find where background is. // actual image is then eroded by shrinkCount to make sure // that no aliased pixels are part of the image weightImageType_t maskImage; TimeMessage startBinarize("Binarize Image"); binarizeImage(rImage_p,dBlackPoint_p,maskImage); TimeMessage startShrink("Shrinking mask"); erodeMask(maskImage,shrinkCount_p); startShrink.Stop(); TimeMessage startAddGradients("Adding gradients data"); addToImage(loggedImage,imageNum_p,maskImage,rSumImageDx_p,rSumImageDy_p,rDxImage_p,rDyImage_p); startAddGradients.Stop(); }