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();
}
