hesuicong
/
openMVS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
24 Commits
1 Branch
0 Tags
13 MiB
 
 
 
 
 
 
Tree: 4927b0ca02
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '4927b0ca02'
${ noResults }
openMVS/libs/MVS/SemiGlobalMatcher.cpp

2365 lines
83 KiB
Raw Blame History

/*
* SemiGlobalMatcher.cpp
*
* Copyright (c) 2014-2015 SEACAVE
*
* Author(s):
*
* cDc <cdc.seacave@gmail.com>
*
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU Affero General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Affero General Public License for more details.
*
* You should have received a copy of the GNU Affero General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
*
* Additional Terms:
*
* You are required to preserve legal notices and author attributions in
* that material or in the Appropriate Legal Notices displayed by works
* containing it.
*/
#include "Common.h"
#include "SemiGlobalMatcher.h"
#include "Scene.h"
using namespace MVS;
using namespace STEREO;
// D E F I N E S ///////////////////////////////////////////////////
// uncomment to enable OpenCV filter demo
//#define _USE_FILTER_DEMO
// S T R U C T S ///////////////////////////////////////////////////
#ifdef _USE_FILTER_DEMO
#include "opencv2/ximgproc/disparity_filter.hpp"
const String keys =
"{help h usage ? | | print this message }"
"{GT |None | optional ground-truth disparity (MPI-Sintel or Middlebury format) }"
"{dst_path |None | optional path to save the resulting filtered disparity map }"
"{dst_raw_path |None | optional path to save raw disparity map before filtering }"
"{algorithm |bm | stereo matching method (bm or sgbm) }"
"{filter |wls_conf| used post-filtering (wls_conf or wls_no_conf) }"
"{no-display | | don't display results }"
"{no-downscale | | force stereo matching on full-sized views to improve quality }"
"{dst_conf_path |None | optional path to save the confidence map used in filtering }"
"{vis_mult |1.0 | coefficient used to scale disparity map visualizations }"
"{max_disparity |160 | parameter of stereo matching }"
"{window_size |-1 | parameter of stereo matching }"
"{wls_lambda |8000.0 | parameter of post-filtering }"
"{wls_sigma |1.5 | parameter of post-filtering }"
;
cv::Rect computeROI(cv::Size2i src_sz, cv::Ptr<cv::StereoMatcher> matcher_instance)
{
int min_disparity = matcher_instance->getMinDisparity();
int num_disparities = matcher_instance->getNumDisparities();
int block_size = matcher_instance->getBlockSize();
int bs2 = block_size/2;
int minD = min_disparity, maxD = min_disparity + num_disparities - 1;
int xmin = maxD + bs2;
int xmax = src_sz.width + minD - bs2;
int ymin = bs2;
int ymax = src_sz.height - bs2;
return cv::Rect(xmin, ymin, xmax - xmin, ymax - ymin);
}
int disparityFiltering(cv::Mat left, cv::Mat right, int argc, const LPCSTR* argv)
{
cv::CommandLineParser parser(argc,argv,keys);
parser.about("Disparity Filtering Demo");
if(parser.has("help"))
{
parser.printMessage();
return 0;
}
String GT_path = parser.get<String>("GT");
String dst_path = parser.get<String>("dst_path");
String dst_raw_path = parser.get<String>("dst_raw_path");
String dst_conf_path = parser.get<String>("dst_conf_path");
String algo = parser.get<String>("algorithm");
String filter = parser.get<String>("filter");
bool no_display = parser.has("no-display");
bool no_downscale = parser.has("no-downscale");
int max_disp = parser.get<int>("max_disparity");
double lambda = parser.get<double>("wls_lambda");
double sigma = parser.get<double>("wls_sigma");
double vis_mult = parser.get<double>("vis_mult");
int wsize;
if(parser.get<int>("window_size")>=0) //user provided window_size value
wsize = parser.get<int>("window_size");
else
{
if(algo=="sgbm")
wsize = 3; //default window size for SGBM
else if(!no_downscale && algo=="bm" && filter=="wls_conf")
wsize = 7; //default window size for BM on downscaled views (downscaling is performed only for wls_conf)
else
wsize = 15; //default window size for BM on full-sized views
}
if(!parser.check())
{
parser.printErrors();
return -1;
}
cv::Mat GT_disp;
bool noGT(cv::ximgproc::readGT(GT_path,GT_disp)!=0);
cv::Mat left_for_matcher, right_for_matcher;
cv::Mat left_disp,right_disp;
cv::Mat filtered_disp;
cv::Mat conf_map(left.rows,left.cols,CV_8U);
conf_map = cv::Scalar(255);
cv::Rect ROI;
cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter;
double matching_time, filtering_time;
if(max_disp<=0 || max_disp%16!=0)
{
std::cout<<"Incorrect max_disparity value: it should be positive and divisible by 16";
return -1;
}
if(wsize<=0 || wsize%2!=1)
{
std::cout<<"Incorrect window_size value: it should be positive and odd";
return -1;
}
if(filter=="wls_conf") // filtering with confidence (significantly better quality than wls_no_conf)
{
if(!no_downscale)
{
// downscale the views to speed-up the matching stage, as we will need to compute both left
// and right disparity maps for confidence map computation
//! [downscale]
max_disp/=2;
if(max_disp%16!=0)
max_disp += 16-(max_disp%16);
resize(left ,left_for_matcher ,cv::Size(),0.5,0.5, cv::INTER_LINEAR_EXACT);
resize(right,right_for_matcher,cv::Size(),0.5,0.5, cv::INTER_LINEAR_EXACT);
//! [downscale]
}
else
{
left_for_matcher = left.clone();
right_for_matcher = right.clone();
}
if(algo=="bm")
{
//! [matching]
cv::Ptr<cv::StereoBM> left_matcher = cv::StereoBM::create(max_disp,wsize);
wls_filter = cv::ximgproc::createDisparityWLSFilter(left_matcher);
cv::Ptr<cv::StereoMatcher> right_matcher = cv::ximgproc::createRightMatcher(left_matcher);
cvtColor(left_for_matcher, left_for_matcher, cv::COLOR_BGR2GRAY);
cvtColor(right_for_matcher, right_for_matcher, cv::COLOR_BGR2GRAY);
matching_time = (double)cv::getTickCount();
left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp);
right_matcher->compute(right_for_matcher,left_for_matcher, right_disp);
matching_time = ((double)cv::getTickCount() - matching_time)/cv::getTickFrequency();
//! [matching]
}
else if(algo=="sgbm")
{
cv::Ptr<cv::StereoSGBM> left_matcher = cv::StereoSGBM::create(-max_disp/2+1,max_disp,wsize);
left_matcher->setP1(24*wsize*wsize);
left_matcher->setP2(96*wsize*wsize);
left_matcher->setPreFilterCap(63);
left_matcher->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
wls_filter = cv::ximgproc::createDisparityWLSFilter(left_matcher);
cv::Ptr<cv::StereoMatcher> right_matcher = cv::ximgproc::createRightMatcher(left_matcher);
matching_time = (double)cv::getTickCount();
left_matcher-> compute(left_for_matcher, right_for_matcher,left_disp);
right_matcher->compute(right_for_matcher,left_for_matcher, right_disp);
matching_time = ((double)cv::getTickCount() - matching_time)/cv::getTickFrequency();
}
else
{
std::cout<<"Unsupported algorithm";
return -1;
}
//! [filtering]
wls_filter->setLambda(lambda);
wls_filter->setSigmaColor(sigma);
filtering_time = (double)cv::getTickCount();
wls_filter->filter(left_disp,left,filtered_disp,right_disp);
filtering_time = ((double)cv::getTickCount() - filtering_time)/cv::getTickFrequency();
//! [filtering]
conf_map = wls_filter->getConfidenceMap();
// Get the ROI that was used in the last filter call:
ROI = wls_filter->getROI();
if(!no_downscale)
{
// upscale raw disparity and ROI back for a proper comparison:
resize(left_disp,left_disp,cv::Size(),2.0,2.0,cv::INTER_LINEAR_EXACT);
left_disp = left_disp*2.0;
ROI = cv::Rect(ROI.x*2,ROI.y*2,ROI.width*2,ROI.height*2);
}
}
else if(filter=="wls_no_conf")
{
/* There is no convenience function for the case of filtering with no confidence, so we
will need to set the ROI and matcher parameters manually */
left_for_matcher = left.clone();
right_for_matcher = right.clone();
if(algo=="bm")
{
cv::Ptr<cv::StereoBM> matcher = cv::StereoBM::create(max_disp,wsize);
matcher->setTextureThreshold(0);
matcher->setUniquenessRatio(0);
cvtColor(left_for_matcher, left_for_matcher, cv::COLOR_BGR2GRAY);
cvtColor(right_for_matcher, right_for_matcher, cv::COLOR_BGR2GRAY);
ROI = computeROI(left_for_matcher.size(),matcher);
wls_filter = cv::ximgproc::createDisparityWLSFilterGeneric(false);
wls_filter->setDepthDiscontinuityRadius((int)ceil(0.33*wsize));
matching_time = (double)cv::getTickCount();
matcher->compute(left_for_matcher,right_for_matcher,left_disp);
matching_time = ((double)cv::getTickCount() - matching_time)/cv::getTickFrequency();
}
else if(algo=="sgbm")
{
cv::Ptr<cv::StereoSGBM> matcher = cv::StereoSGBM::create(0,max_disp,wsize);
matcher->setUniquenessRatio(0);
matcher->setDisp12MaxDiff(1000000);
matcher->setSpeckleWindowSize(0);
matcher->setP1(24*wsize*wsize);
matcher->setP2(96*wsize*wsize);
matcher->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);
ROI = computeROI(left_for_matcher.size(),matcher);
wls_filter = cv::ximgproc::createDisparityWLSFilterGeneric(false);
wls_filter->setDepthDiscontinuityRadius((int)ceil(0.5*wsize));
matching_time = (double)cv::getTickCount();
matcher->compute(left_for_matcher,right_for_matcher,left_disp);
matching_time = ((double)cv::getTickCount() - matching_time)/cv::getTickFrequency();
}
else
{
std::cout<<"Unsupported algorithm";
return -1;
}
wls_filter->setLambda(lambda);
wls_filter->setSigmaColor(sigma);
filtering_time = (double)cv::getTickCount();
wls_filter->filter(left_disp,left,filtered_disp,cv::Mat(),ROI);
filtering_time = ((double)cv::getTickCount() - filtering_time)/cv::getTickFrequency();
}
else
{
std::cout<<"Unsupported filter";
return -1;
}
//collect and print all the stats:
std::cout.precision(2);
std::cout<<"Matching time: "<<matching_time<<"s"<<std::endl;
std::cout<<"Filtering time: "<<filtering_time<<"s"<<std::endl;
std::cout<<std::endl;
double MSE_before,percent_bad_before,MSE_after,percent_bad_after;
if(!noGT)
{
MSE_before = cv::ximgproc::computeMSE(GT_disp,left_disp,ROI);
percent_bad_before = cv::ximgproc::computeBadPixelPercent(GT_disp,left_disp,ROI);
MSE_after = cv::ximgproc::computeMSE(GT_disp,filtered_disp,ROI);
percent_bad_after = cv::ximgproc::computeBadPixelPercent(GT_disp,filtered_disp,ROI);
std::cout.precision(5);
std::cout<<"MSE before filtering: "<<MSE_before<<std::endl;
std::cout<<"MSE after filtering: "<<MSE_after<<std::endl;
std::cout<<std::endl;
std::cout.precision(3);
std::cout<<"Percent of bad pixels before filtering: "<<percent_bad_before<<std::endl;
std::cout<<"Percent of bad pixels after filtering: "<<percent_bad_after<<std::endl;
}
if(dst_path!="None")
{
cv::Mat filtered_disp_vis;
cv::ximgproc::getDisparityVis(filtered_disp,filtered_disp_vis,vis_mult);
cv::imwrite(dst_path,filtered_disp_vis);
}
if(dst_raw_path!="None")
{
cv::Mat raw_disp_vis;
cv::ximgproc::getDisparityVis(left_disp,raw_disp_vis,vis_mult);
cv::imwrite(dst_raw_path,raw_disp_vis);
}
if(dst_conf_path!="None")
{
cv::imwrite(dst_conf_path,conf_map);
}
if(!no_display)
{
cv::namedWindow("left", cv::WINDOW_AUTOSIZE);
cv::imshow("left", left);
cv::namedWindow("right", cv::WINDOW_AUTOSIZE);
cv::imshow("right", right);
if(!noGT)
{
cv::Mat GT_disp_vis;
cv::ximgproc::getDisparityVis(GT_disp,GT_disp_vis,vis_mult);
cv::namedWindow("ground-truth disparity", cv::WINDOW_AUTOSIZE);
cv::imshow("ground-truth disparity", GT_disp_vis);
}
//! [visualization]
cv::Mat raw_disp_vis;
cv::ximgproc::getDisparityVis(left_disp,raw_disp_vis,vis_mult);
cv::namedWindow("raw disparity", cv::WINDOW_AUTOSIZE);
cv::imshow("raw disparity", raw_disp_vis);
cv::Mat filtered_disp_vis;
cv::ximgproc::getDisparityVis(filtered_disp,filtered_disp_vis,vis_mult);
cv::namedWindow("filtered disparity", cv::WINDOW_AUTOSIZE);
cv::imshow("filtered disparity", filtered_disp_vis);
cv::waitKey();
//! [visualization]
}
return 0;
}
#endif
/*----------------------------------------------------------------*/
// apply a gradient like filter
void PrefilterXSobel(const cv::Mat& src, cv::Mat& dst, int ftzero=31)
{
const int OFS = 256 * 4, TABSZ = OFS * 2 + 256;
uint8_t tab[TABSZ];
for (int x = 0; x < TABSZ; x++)
tab[x] = (uint8_t)(x - OFS < -ftzero ? 0 : x - OFS > ftzero ? ftzero * 2 : x - OFS + ftzero);
uint8_t val0 = tab[0 + OFS];
#ifdef _USE_SSE
volatile bool useSIMD = cv::checkHardwareSupport(CV_CPU_SSE2);
#endif
ASSERT(src.type() == CV_8U);
const cv::Size size(src.size());
dst.create(size, src.type());
int y;
for (y = 0; y < size.height - 1; y += 2) {
const uint8_t* srow1 = src.ptr<uint8_t>(y);
const uint8_t* srow0 = y > 0 ? srow1 - src.step : size.height > 1 ? srow1 + src.step : srow1;
const uint8_t* srow2 = y < size.height - 1 ? srow1 + src.step : size.height > 1 ? srow1 - src.step : srow1;
const uint8_t* srow3 = y < size.height - 2 ? srow1 + src.step * 2 : srow1;
uint8_t* dptr0 = dst.ptr<uint8_t>(y);
uint8_t* dptr1 = dptr0 + dst.step;
dptr0[0] = dptr0[size.width - 1] = dptr1[0] = dptr1[size.width - 1] = val0;
int x = 1;
#ifdef _USE_SSE
if (useSIMD) {
__m128i z = _mm_setzero_si128(), ftz = _mm_set1_epi16((short)ftzero);
__m128i ftz2 = _mm_set1_epi8(cv::saturate_cast<uint8_t>(ftzero * 2));
for (; x <= size.width - 9; x += 8) {
__m128i c0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x - 1)), z);
__m128i c1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x - 1)), z);
__m128i d0 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow0 + x + 1)), z);
__m128i d1 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow1 + x + 1)), z);
d0 = _mm_sub_epi16(d0, c0);
d1 = _mm_sub_epi16(d1, c1);
__m128i c2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x - 1)), z);
__m128i c3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x - 1)), z);
__m128i d2 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow2 + x + 1)), z);
__m128i d3 = _mm_unpacklo_epi8(_mm_loadl_epi64((__m128i*)(srow3 + x + 1)), z);
d2 = _mm_sub_epi16(d2, c2);
d3 = _mm_sub_epi16(d3, c3);
__m128i v0 = _mm_add_epi16(d0, _mm_add_epi16(d2, _mm_add_epi16(d1, d1)));
__m128i v1 = _mm_add_epi16(d1, _mm_add_epi16(d3, _mm_add_epi16(d2, d2)));
v0 = _mm_packus_epi16(_mm_add_epi16(v0, ftz), _mm_add_epi16(v1, ftz));
v0 = _mm_min_epu8(v0, ftz2);
_mm_storel_epi64((__m128i*)(dptr0 + x), v0);
_mm_storel_epi64((__m128i*)(dptr1 + x), _mm_unpackhi_epi64(v0, v0));
}
}
#endif
for (; x < size.width - 1; x++) {
int d0 = srow0[x + 1] - srow0[x - 1], d1 = srow1[x + 1] - srow1[x - 1],
d2 = srow2[x + 1] - srow2[x - 1], d3 = srow3[x + 1] - srow3[x - 1];
int v0 = tab[d0 + d1 * 2 + d2 + OFS];
int v1 = tab[d1 + d2 * 2 + d3 + OFS];
dptr0[x] = (uint8_t)v0;
dptr1[x] = (uint8_t)v1;
}
}
for (; y < size.height; y++) {
uint8_t* dptr = dst.ptr<uint8_t>(y);
for (int x = 0; x < size.width; x++)
dptr[x] = val0;
}
}
/*----------------------------------------------------------------*/
// S T R U C T S ///////////////////////////////////////////////////
enum EVENT_TYPE {
EVT_JOB = 0,
EVT_CLOSE,
};
class EVTClose : public Event
{
public:
EVTClose() : Event(EVT_CLOSE) {}
};
class EVTPixelProcess : public Event
{
public:
typedef std::function<void (int,int,int)> FncPixel;
const cv::Size size;
volatile Thread::safe_t& idxPixel;
const FncPixel fncPixel;
bool Run(void*) override {
const int numPixels(size.area());
int idx;
while ((idx=(int)Thread::safeInc(idxPixel)) < numPixels)
fncPixel(idx, idx/size.width, idx%size.width);
return true;
}
EVTPixelProcess(cv::Size s, volatile Thread::safe_t& idx, FncPixel f) : Event(EVT_JOB), size(s), idxPixel(idx), fncPixel(f) {}
};
class EVTPixelAccumInc : public Event
{
public:
typedef std::function<void (int)> FncPixel;
const int numPixels;
volatile Thread::safe_t& idxPixel;
const FncPixel fncPixel;
bool Run(void*) override {
int idx;
while ((idx=(int)Thread::safeInc(idxPixel)) < numPixels)
fncPixel(idx);
return true;
}
EVTPixelAccumInc(int s, volatile Thread::safe_t& idx, FncPixel f) : Event(EVT_JOB), numPixels(s), idxPixel(idx), fncPixel(f) {}
};
class EVTPixelAccumDec : public Event
{
public:
typedef std::function<void (int)> FncPixel;
volatile Thread::safe_t& idxPixel;
const FncPixel fncPixel;
bool Run(void*) override {
int idx;
while ((idx=(int)Thread::safeDec(idxPixel)) >= 0)
fncPixel(idx);
return true;
}
EVTPixelAccumDec(volatile Thread::safe_t& idx, FncPixel f) : Event(EVT_JOB), idxPixel(idx), fncPixel(f) {}
};
/*----------------------------------------------------------------*/
// S T R U C T S ///////////////////////////////////////////////////
// - P1 and P2s are algorithm constants very similar to those from the original SGM algorithm;
// they are set to defaults according to the patch size
// - alpha & beta form the final P2 as P2*(1+alpha*e^(-DI^2/(2*beta^2)))
// where DI is the difference in image intensity I(x)-I(x_prev) in [0,255] range
// - subpixelSteps represents how much sub-pixel accuracy is searched/stored;
// if 1 no sub-pixel precision, if for example 4 a 0.25 sub-pixel accuracy is stored;
// the stored value is quantized and represented as integer: val=(float)valStored/subpixelSteps
SemiGlobalMatcher::SemiGlobalMatcher(SgmSubpixelMode _subpixelMode, Disparity _subpixelSteps, AccumCost _P1, AccumCost P2, float P2alpha, float P2beta)
:
subpixelMode(_subpixelMode),
subpixelSteps(_subpixelSteps),
P1(_P1), P2s(GenerateP2s(P2, P2alpha, P2beta))
{
}
SemiGlobalMatcher::~SemiGlobalMatcher()
{
}
CLISTDEF0IDX(SemiGlobalMatcher::AccumCost,int) SemiGlobalMatcher::GenerateP2s(AccumCost P2, float P2alpha, float P2beta)
{
CLISTDEF0IDX(AccumCost,int) P2s(256);
FOREACH(i, P2s)
P2s[i] = (AccumCost)ROUND2INT(P2*(1.f+P2alpha*EXP(-SQUARE((float)i)/(2.f*SQUARE(P2beta)))));
return P2s;
}
// Compute SGM stereo for this image and each of the neighbor views:
// - minResolution is the resolution of the top of the pyramid for tSGM;
// can be 0 to force the standard SGM algorithm
void SemiGlobalMatcher::Match(const Scene& scene, IIndex idxImage, IIndex numNeighbors, unsigned minResolution)
{
const Image& leftImage = scene.images[idxImage];
const float fMinScore(MAXF(leftImage.neighbors.front().score*OPTDENSE::fViewMinScoreRatio, OPTDENSE::fViewMinScore));
FOREACH(idxNeighbor, leftImage.neighbors) {
const ViewScore& neighbor = leftImage.neighbors[idxNeighbor];
// exclude neighbors that over the limit or too small score
ASSERT(scene.images[neighbor.ID].IsValid());
if ((numNeighbors && idxNeighbor >= numNeighbors) ||
(neighbor.score < fMinScore))
break;
// check if the disparity-map was already estimated for the same image pairs
const Image& rightImage = scene.images[neighbor.ID];
const String pairName(MAKE_PATH(String::FormatString("%04u_%04u", leftImage.ID, rightImage.ID)));
if (File::isPresent((pairName+".dimap").c_str()) || File::isPresent(MAKE_PATH(String::FormatString("%04u_%04u.dimap", rightImage.ID, leftImage.ID))))
continue;
TD_TIMER_STARTD();
IndexArr points;
Matrix3x3 H; Matrix4x4 Q;
ViewData leftData, rightData;
MaskMap leftMaskMap, rightMaskMap; {
// fetch pairs of corresponding image points
//TODO: use precomputed points from SelectViews()
Point3fArr leftPoints, rightPoints;
FOREACH(idxPoint, scene.pointcloud.points) {
const PointCloud::ViewArr& views = scene.pointcloud.pointViews[idxPoint];
if (views.FindFirst(idxImage) != PointCloud::ViewArr::NO_INDEX) {
points.push_back((uint32_t)idxPoint);
if (views.FindFirst(neighbor.ID) != PointCloud::ViewArr::NO_INDEX) {
const Point3 X(scene.pointcloud.points[idxPoint]);
leftPoints.emplace_back(leftImage.camera.TransformPointW2I3(X));
rightPoints.emplace_back(rightImage.camera.TransformPointW2I3(X));
}
}
}
// stereo-rectify image pair
if (!Image::StereoRectifyImages(leftImage, rightImage, leftPoints, rightPoints, leftData.imageColor, rightData.imageColor, leftMaskMap, rightMaskMap, H, Q))
continue;
ASSERT(leftData.imageColor.size() == rightData.imageColor.size());
}
#ifdef _USE_FILTER_DEMO
// run openCV implementation
const LPCSTR argv[] = {"disparityFiltering", "-algorithm=sgbm", "-max_disparity=160", "-no-downscale"};
disparityFiltering(leftData.imageColor, rightData.imageColor, (int)SizeOfArray(argv), argv);
#endif
// color to gray conversion
#if SGM_SIMILARITY == SGM_SIMILARITY_CENSUS
leftData.imageColor.toGray(leftData.imageGray, cv::COLOR_BGR2GRAY, false, true);
rightData.imageColor.toGray(rightData.imageGray, cv::COLOR_BGR2GRAY, false, true);
#else
leftData.imageColor.toGray(leftData.imageGray, cv::COLOR_BGR2GRAY, true, true);
rightData.imageColor.toGray(rightData.imageGray, cv::COLOR_BGR2GRAY, true, true);
#endif
// compute scale used for the disparity estimation
REAL scale(1);
if (minResolution) {
unsigned resolutionLevel(8);
Image8U::computeMaxResolution(leftData.imageGray.width(), leftData.imageGray.height(), resolutionLevel, minResolution);
scale = REAL(1)/MAXF(2,POWI(2,resolutionLevel));
}
const bool tSGM(!ISEQUAL(scale, REAL(1)));
DisparityMap leftDisparityMap, rightDisparityMap; AccumCostMap costMap;
do {
#if 0
// export the intermediate disparity-maps
if (!leftDisparityMap.empty()) {
const REAL _scale(scale*0.5);
Matrix3x3 _H(H); Matrix4x4 _Q(Q);
Image::ScaleStereoRectification(_H, _Q, _scale);
ExportDisparityDataRawFull(String::FormatString("%s_%d.dimap", pairName.c_str(), LOG2I(ROUND2INT<unsigned>(REAL(1)/_scale))), leftDisparityMap, costMap, Image8U::computeResize(leftImage.GetSize(), _scale), H, _Q, 1);
}
#endif
// initialize
const ViewData leftDataLevel(leftData.GetImage(scale));
const ViewData rightDataLevel(rightData.GetImage(scale));
const cv::Size size(leftDataLevel.imageGray.size());
const cv::Size sizeValid(size.width-2*halfWindowSizeX, size.height-2*halfWindowSizeY);
const bool bFirstLevel(leftDisparityMap.empty());
if (bFirstLevel) {
// initialize the disparity-map with a rough estimate based on the sparse point-cloud
//TODO: remove DepthData::ViewData dependency
Image leftImageLevel(leftImage.GetImage(scene.platforms, scale*0.5, false));
DepthData::ViewData image;
image.pImageData = &leftImageLevel; // used only for avgDepth
image.image.create(leftImageLevel.GetSize());
image.camera = leftImageLevel.camera;
DepthMap depthMap;
Depth dMin, dMax;
TriangulatePoints2DepthMap(image, scene.pointcloud, points, depthMap, dMin, dMax, true);
points.Release();
Matrix3x3 H2(H); Matrix4x4 Q2(Q);
Image::ScaleStereoRectification(H2, Q2, scale*0.5);
const cv::Size sizeHalf(Image8U::computeResize(size, 0.5));
const cv::Size sizeValidHalf(sizeHalf.width-2*halfWindowSizeX, sizeHalf.height-2*halfWindowSizeY);
leftDisparityMap.create(sizeValidHalf);
Depth2DisparityMap(depthMap, H2.inv(), Q2.inv(), 1, leftDisparityMap);
// resize masks
cv::resize(leftMaskMap, leftMaskMap, size, 0, 0, cv::INTER_NEAREST);
cv::resize(rightMaskMap, rightMaskMap, size, 0, 0, cv::INTER_NEAREST);
const cv::Rect ROI(halfWindowSizeX,halfWindowSizeY, sizeValid.width,sizeValid.height);
leftMaskMap(ROI).copyTo(leftMaskMap);
rightMaskMap(ROI).copyTo(rightMaskMap);
} else {
// upscale masks
UpscaleMask(leftMaskMap, sizeValid);
UpscaleMask(rightMaskMap, sizeValid);
}
// estimate right-left disparity-map
Index numCosts;
if (tSGM) {
// upscale the disparity-map from the previous level
FlipDirection(leftDisparityMap, rightDisparityMap);
numCosts = Disparity2RangeMap(rightDisparityMap, rightMaskMap, bFirstLevel?11:5, bFirstLevel?33:7);
} else {
// extract global min and max disparities
Range range{std::numeric_limits<Disparity>::max(), std::numeric_limits<Disparity>::min()};
ASSERT(leftDisparityMap.isContinuous());
const Disparity* pd = leftDisparityMap.ptr<const Disparity>();
const Disparity* const pde = pd+leftDisparityMap.area();
do {
const Disparity d(*pd);
if (range.minDisp > d)
range.minDisp = d;
if (range.maxDisp < d)
range.maxDisp = d;
} while (++pd < pde);
// set disparity search range to the global min/max range
const Disparity numDisp(range.numDisp()+16);
const Disparity disp(range.minDisp+range.maxDisp);
range.minDisp = disp-numDisp;
range.maxDisp = disp+numDisp;
maxNumDisp = range.numDisp();
numCosts = 0;
imagePixels.resize(sizeValid.area());
for (PixelData& pixel: imagePixels) {
pixel.range = range;
pixel.idx = numCosts;
numCosts += maxNumDisp;
}
}
imageCosts.resize(numCosts);
imageAccumCosts.resize(numCosts);
Match(rightDataLevel, leftDataLevel, rightDisparityMap, costMap);
// estimate left-right disparity-map
if (tSGM) {
numCosts = Disparity2RangeMap(leftDisparityMap, leftMaskMap, bFirstLevel?11:5, bFirstLevel?33:7);
imageCosts.resize(numCosts);
imageAccumCosts.resize(numCosts);
} else {
for (PixelData& pixel: imagePixels) {
const Disparity maxDisp(-pixel.range.minDisp);
pixel.range.minDisp = -pixel.range.maxDisp;
pixel.range.maxDisp = maxDisp;
}
}
Match(leftDataLevel, rightDataLevel, leftDisparityMap, costMap);
// check disparity-map cross-consistency
#if 0
if (ISEQUAL(scale, REAL(1))) {
cv::Ptr<cv::ximgproc::DisparityWLSFilter> filter = cv::ximgproc::createDisparityWLSFilterGeneric(true);
const cv::Rect rcValid(halfWindowSizeX,halfWindowSizeY, sizeValid.width,sizeValid.height);
Image32F leftDisparityMap32F, rightDisparityMap32F, filtered32F;
leftDisparityMap.convertTo(leftDisparityMap32F, CV_32F, 16);
rightDisparityMap.convertTo(rightDisparityMap32F, CV_32F, 16);
filter->filter(leftDisparityMap32F, leftData.imageColor(rcValid), filtered32F, rightDisparityMap32F);
filtered32F.convertTo(leftDisparityMap, CV_16S, 1.0/16, 0.5);
} else
#endif
if (bFirstLevel) {
// perform a rigorous filtering of the estimated disparity maps in order to
// estimate the common region or interest and set the validity masks
ConsistencyCrossCheck(leftDisparityMap, rightDisparityMap);
ConsistencyCrossCheck(rightDisparityMap, leftDisparityMap);
cv::filterSpeckles(leftDisparityMap, NO_DISP, OPTDENSE::nSpeckleSize, 5);
cv::filterSpeckles(rightDisparityMap, NO_DISP, OPTDENSE::nSpeckleSize, 5);
ExtractMask(leftDisparityMap, leftMaskMap);
ExtractMask(rightDisparityMap, rightMaskMap);
} else {
// simply run a left-right consistency check
ConsistencyCrossCheck(leftDisparityMap, rightDisparityMap);
}
} while ((scale*=2) < REAL(1)+ZEROTOLERANCE<REAL>());
#if 0
// remove speckles
if (OPTDENSE::nSpeckleSize > 0)
cv::filterSpeckles(leftDisparityMap, NO_DISP, OPTDENSE::nSpeckleSize, 5);
#endif
// sub-pixel disparity-map estimation
RefineDisparityMap(leftDisparityMap);
#if 1
// export disparity-map for the left image
DEBUG_EXTRA("Disparity-map for images %3u and %3u: %dx%d (%s)", leftImage.ID, rightImage.ID,
leftImage.width, leftImage.height, TD_TIMER_GET_FMT().c_str());
ExportPointCloud(pairName+".ply", leftImage, leftDisparityMap, Q, subpixelSteps);
ExportDisparityMap(pairName+".png", leftDisparityMap);
ExportDisparityDataRawFull(pairName+".dimap", leftDisparityMap, costMap, leftImage.GetSize(), H, Q, subpixelSteps);
#else
// convert disparity-map to final depth-map for the left image
DepthMap depthMap(leftImage.image.size()); ConfidenceMap confMap;
Disparity2DepthMap(leftDisparityMap, costMap, H, Q, subpixelSteps, depthMap, confMap);
DEBUG_EXTRA("Depth-map for images %3u and %3u: %dx%d (%s)", leftImage.ID, rightImage.ID,
depthMap.width(), depthMap.height(), TD_TIMER_GET_FMT().c_str());
ExportDepthMap(pairName+".png", depthMap);
MVS::ExportPointCloud(pairName+".ply", leftImage, depthMap, NormalMap());
ExportDepthDataRaw(pairName+".dmap", leftImage.name, IIndexArr{leftImage.ID,rightImage.ID}, leftImage.GetSize(), leftImage.camera.K, leftImage.camera.R, leftImage.camera.C, 0, FLT_MAX, depthMap, NormalMap(), confMap);
#endif
}
}
void SemiGlobalMatcher::Fuse(const Scene& scene, IIndex idxImage, IIndex numNeighbors, unsigned minViews, DepthMap& depthMap, ConfidenceMap& confMap)
{
TD_TIMER_STARTD();
const Image& leftImage = scene.images[idxImage];
const float fMinScore(MAXF(leftImage.neighbors.front().score*OPTDENSE::fViewMinScoreRatio, OPTDENSE::fViewMinScore));
struct PairData {
DepthMap depthMap;
DepthRangeMap depthRangeMap;
ConfidenceMap confMap;
PairData(const cv::Size& size) : depthMap(size) {}
};
// load and put in same space all disparity-maps containing this view
CLISTDEFIDX(PairData,IIndex) pairs;
pairs.reserve(numNeighbors);
FOREACH(idxNeighbor, leftImage.neighbors) {
const ViewScore& neighbor = leftImage.neighbors[idxNeighbor];
// exclude neighbors that over the limit or too small score
ASSERT(scene.images[neighbor.ID].IsValid());
if ((numNeighbors && idxNeighbor >= numNeighbors) ||
(neighbor.score < fMinScore))
break;
// check if the disparity-map was estimated for this images pair
const Image& rightImage = scene.images[neighbor.ID];
Disparity subpixelSteps;
cv::Size imageSize; Matrix3x3 H; Matrix4x4 Q;
DisparityMap disparityMap; AccumCostMap costMap;
if (!ImportDisparityDataRawFull(MAKE_PATH(String::FormatString("%04u_%04u.dimap", leftImage.ID, rightImage.ID)), disparityMap, costMap, imageSize, H, Q, subpixelSteps)) {
if (!ImportDisparityDataRawFull(MAKE_PATH(String::FormatString("%04u_%04u.dimap", rightImage.ID, leftImage.ID)), disparityMap, costMap, imageSize, H, Q, subpixelSteps)) {
DEBUG("warning: no disparity-data file found for image pair (%u,%u)", leftImage.ID, rightImage.ID);
continue;
}
// adapt Q to project the 3D point from right into the left-image
RMatrix poseR;
CMatrix poseC;
ComputeRelativePose(rightImage.camera.R, rightImage.camera.C, leftImage.camera.R, leftImage.camera.C, poseR, poseC);
Matrix4x4 P(Matrix4x4::IDENTITY);
AssembleProjectionMatrix(leftImage.camera.K, poseR, poseC, reinterpret_cast<Matrix3x4&>(P));
Matrix4x4 invK(Matrix4x4::IDENTITY);
cv::Mat(rightImage.camera.GetInvK()).copyTo(cv::Mat(4,4,cv::DataType<Matrix4x4::Type>::type,invK.val)(cv::Rect(0,0,3,3)));
Q = P*invK*Q;
}
// convert disparity-map to final depth-map for the left image
PairData& pair = pairs.emplace_back(leftImage.image.size());
if (!ProjectDisparity2DepthMap(disparityMap, costMap, Q, subpixelSteps, pair.depthMap, pair.depthRangeMap, pair.confMap)) {
pairs.RemoveLast();
continue;
}
#if TD_VERBOSE != TD_VERBOSE_OFF
// save pair depth-map
if (VERBOSITY_LEVEL > 3) {
const String pairName(MAKE_PATH(String::FormatString("depth%04u_%04u", leftImage.ID, rightImage.ID)));
ExportDepthMap(pairName+".png", pair.depthMap);
MVS::ExportPointCloud(pairName+".ply", leftImage, pair.depthMap, NormalMap());
}
#endif
}
// fuse available depth-maps such that for each pixel set its depth as the average of the largest cluster of agreeing depths;
// pixel depths values agree if their trust regions overlap
depthMap.create(leftImage.image.size()); confMap.create(depthMap.size());
for (int r=0; r<depthMap.rows; ++r) {
for (int c=0; c<depthMap.cols; ++c) {
struct Cluster {
IIndexArr views;
DepthRange range;
};
CLISTDEFIDX(Cluster,IIndex) clusters(0, pairs.size());
FOREACH(p, pairs) {
const PairData& pair = pairs[p];
const Depth depth(pair.depthMap(r,c));
if (depth <= 0)
continue;
const DepthRange& range(pair.depthRangeMap(r,c));
unsigned numClusters(0);
for (Cluster& cluster: clusters) {
if (!ISINSIDE(depth, cluster.range.x, cluster.range.y))
continue;
cluster.views.push_back(p);
if (cluster.range.x < range.x)
cluster.range.x = range.x;
if (cluster.range.y > range.y)
cluster.range.y = range.y;
++numClusters;
}
if (numClusters == 0)
clusters.emplace_back(Cluster{IIndexArr{p}, range});
}
if (clusters.empty()) {
depthMap(r,c) = Depth(0);
confMap(r,c) = float(0);
continue;
}
const Cluster& cluster = clusters.GetMax([](const Cluster& i, const Cluster& j) { return i.views.size() < j.views.size(); });
if (cluster.views.size() < minViews) {
depthMap(r,c) = Depth(0);
confMap(r,c) = float(0);
continue;
}
Depth& depth = depthMap(r,c); depth = 0;
float& conf = confMap(r,c); conf = 0;
unsigned numDepths(0);
for (IIndex p: cluster.views) {
const PairData& pair = pairs[p];
depth += pair.depthMap(r,c);
conf += pair.confMap(r,c);
++numDepths;
}
depth /= numDepths;
conf /= numDepths;
}
}
DEBUG_EXTRA("Depth-map for image %3u fused: %dx%d (%s)", leftImage.ID,
depthMap.width(), depthMap.height(), TD_TIMER_GET_FMT().c_str());
#if TD_VERBOSE != TD_VERBOSE_OFF
// save depth-map
if (VERBOSITY_LEVEL > 2) {
const String fileName(MAKE_PATH(String::FormatString("depth%04u", leftImage.ID)));
ExportDepthMap(fileName+".png", depthMap);
MVS::ExportPointCloud(fileName+".ply", leftImage, depthMap, NormalMap());
}
#endif
}
// Compute SGM stereo on the images
void SemiGlobalMatcher::Match(const ViewData& leftImage, const ViewData& rightImage, DisparityMap& disparityMap, AccumCostMap& costMap)
{
const cv::Size size(leftImage.imageGray.size());
const cv::Size sizeValid(size.width-2*halfWindowSizeX, size.height-2*halfWindowSizeY);
ASSERT(leftImage.imageColor.size() == size);
#if 0 && !defined(_RELEASE)
// display search info (average disparity and range)
DisplayState(sizeValid);
#endif
// compute costs
{
ASSERT(!imageCosts.empty());
const float eps(1e-3f); // used suppress the effect of noise in untextured regions
auto pixel = [&](int idx, int r, int c) {
// ignore pixel if not valid
const PixelData& pixel = imagePixels[idx];
if (!pixel.range.isValid())
return;
#if SGM_SIMILARITY == SGM_SIMILARITY_CENSUS
struct Compute {
static inline int HammingDistance(uint64_t c1, uint64_t c2) {
return PopCnt(c1 ^ c2);
}
};
// compute pixel cost
Cost* costs = imageCosts.data()+pixel.idx;
const Census lc(leftImage.imageCensus(r,c));
for (int d=pixel.range.minDisp; d<pixel.range.maxDisp; ++d) {
const ImageRef x(c+d,r);
if (!rightImage.imageCensus.isInside(x)) {
*costs++ = 255;
continue;
}
const Census rc(rightImage.imageCensus(x));
*costs++ = Compute::HammingDistance(lc, rc)*4;
}
#else
struct Compute {
static float NormL1Sq(const Pixel8U& a, const Pixel8U& b) {
// |(a-b)| L1 norm of (a-b) when types are byte
return float(
SQUARE(unsigned(a[0]<b[0] ? b[0]-a[0] : a[0]-b[0])) +
SQUARE(unsigned(a[1]<b[1] ? b[1]-a[1] : a[1]-b[1])) +
SQUARE(unsigned(a[2]<b[2] ? b[2]-a[2] : a[2]-b[2])));
}
static float WeightColor(const Image8U3& image, const Pixel8U& center, const ImageRef& x) {
// color weight [0..1]
static const float sigmaColor(-1.f/(2.f*SQUARE(0.3f*255)));
return Compute::NormL1Sq(image(x), center) * sigmaColor;
}
static float WeightSpatial(int x, int y) {
// spatial weight [0..1]
static const float sigmaSpatial(-1.f/(2.f*SQUARE(0.4f*MAXF<int>(windowSizeX,windowSizeY))));
return float(SQUARE(x) + SQUARE(y)) * sigmaSpatial;
}
};
const ImageRef u(c+halfWindowSizeX,r+halfWindowSizeY);
// initialize pixel patch weights
WeightedPatch w;
w.normSq0 = 0;
w.sumWeights = 0;
int n = 0;
const Pixel8U& colCenter = leftImage.imageColor(u);
for (int i=-halfWindowSizeY; i<=halfWindowSizeY; ++i) {
for (int j=-halfWindowSizeX; j<=halfWindowSizeX; ++j) {
const ImageRef x(u.x+j,u.y+i);
WeightedPatch::Pixel& pw = w.weights[n++];
w.normSq0 +=
(pw.tempWeight = leftImage.imageGray(x)) *
(pw.weight = EXP(Compute::WeightColor(leftImage.imageColor, colCenter, x)+Compute::WeightSpatial(j,i)));
w.sumWeights += pw.weight;
}
}
ASSERT(n == numTexels);
const float tm(w.normSq0/w.sumWeights);
w.normSq0 = 0;
n = 0;
do {
WeightedPatch::Pixel& pw = w.weights[n];
const float t(pw.tempWeight - tm);
w.normSq0 += (pw.tempWeight = pw.weight * t) * t;
} while (++n < numTexels);
// compute pixel cost
Cost* costs = imageCosts.data()+pixel.idx;
for (int d=pixel.range.minDisp; d<pixel.range.maxDisp; ++d) {
float sum(0), sumSq(0), nom(0);
for (int i=-halfWindowSizeY, n=0; i<=halfWindowSizeY; ++i) {
for (int j=-halfWindowSizeX; j<=halfWindowSizeX; ++j) {
const ImageRef x(u.x+j+d,u.y+i);
if (!rightImage.imageGray.isInside(x)) {
*costs++ = 255;
goto NEXT_COST;
}
const float f(rightImage.imageGray(x));
const WeightedPatch::Pixel& pw = w.weights[n++];
const float fw(f*pw.weight);
sum += fw;
sumSq += f*fw;
nom += f*pw.tempWeight;
}
}
{
const float normSq1(sumSq-SQUARE(sum)/w.sumWeights);
const float ncc(nom/SQRT(w.normSq0*normSq1+eps));
*costs++ = (ncc <= 0 ? Cost(255) : (Cost)ROUND2INT((1.f-MINF(ncc,1.f))*255.f));
}
NEXT_COST:;
}
#endif
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(sizeValid, idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<sizeValid.height; ++r)
for (int c=0; c<sizeValid.width; ++c)
pixel(r*sizeValid.width+c, r, c);
}
// accumulate costs
{
ASSERT(!imageAccumCosts.empty());
imageAccumCosts.Memset(0);
#if SGM_SIMILARITY == SGM_SIMILARITY_CENSUS
const ImageGray::Type Igray(127);
#else
const ImageGray::Type Igray(0.5f);
#endif
struct LineData {
AccumCost* L;
Range R;
~LineData() { delete[] L; }
AccumCost operator[] (int i) const { return L[i]; }
AccumCost& operator[] (int i) { return L[i]; }
};
auto pixelAccum = [&](const Cost* costs, const LineData& Lp, LineData& Ls, AccumCost* accums, ImageGray::Type DI) {
struct Compute {
static inline void MINS(AccumCost& m, AccumCost v) { if (m > v) m = v; }
};
ASSERT(Ls.R.isValid());
#if SGM_SIMILARITY == SGM_SIMILARITY_CENSUS
const AccumCost P2(P2s[DI]);
#else
const AccumCost P2(P2s[ABS(ROUND2INT(255.f*DI))]);
#endif
const Disparity minDisp(MAXF(Lp.R.minDisp, Ls.R.minDisp));
const Disparity maxDisp(MINF(Lp.R.maxDisp, Ls.R.maxDisp));
if (minDisp >= maxDisp) {
// the disparity ranges for the two pixels do not intersect;
// fill all accumulated costs with L(d)=C(d)+P2
const Disparity numDisp(Ls.R.numDisp());
for (int idxDisp=0; idxDisp<numDisp; ++idxDisp)
accums[idxDisp] += (Ls[idxDisp] = costs[idxDisp]+P2);
} else {
// accumulate cost as L(d)=C(d)+min(Lp(d)+V(d,dp))-min(Lp)
// where V(d,dp) is:
// 0 if d=dp
// P1 if |d-dp|=1
// P2 if |d-dp|>1
AccumCost minLp(std::numeric_limits<AccumCost>::max());
for (const AccumCost *L=Lp.L+(minDisp-Lp.R.minDisp), *endL=L+(maxDisp-minDisp); L<endL; ++L)
Compute::MINS(minLp, *L);
for (Disparity d=Ls.R.minDisp; d<Ls.R.maxDisp; ++d) {
const int idxDisp(d-Ls.R.minDisp);
AccumCost& L = Ls[idxDisp];
L = std::numeric_limits<AccumCost>::max();
for (Disparity dp=minDisp; dp<maxDisp; ++dp) {
const int idxDispp(dp-Lp.R.minDisp);
if (dp == d)
Compute::MINS(L, Lp[idxDispp]);
else if (dp == d-1 || dp == d+1)
Compute::MINS(L, Lp[idxDispp]+P1);
else
Compute::MINS(L, Lp[idxDispp]+P2);
}
accums[idxDisp] += (L = costs[idxDisp]+L-minLp);
}
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
struct AccumLines {
LineData linesBuffer[2];
LineData* lines[2];
AccumLines(Disparity maxNumDisp) {
for (LineData& line: linesBuffer) {
line.L = new AccumCost[maxNumDisp];
memset(line.L, 0, sizeof(AccumCost)*maxNumDisp);
line.R.minDisp = line.R.maxDisp = 0;
}
lines[0] = linesBuffer+0;
lines[1] = linesBuffer+1;
}
void NextLine() { std::swap(lines[0], lines[1]); }
const LineData& operator() (int i) const { return *lines[i]; }
LineData& operator() (int i) { return *lines[i]; }
};
#define ACCUM_PIXELS(cond) \
AccumLines lines(maxNumDisp); \
ImageGray::Type Ip(Igray); \
do { \
const int idx(u.y*sizeValid.width+u.x); \
const PixelData& pixel = imagePixels[idx]; \
if (!pixel.range.isValid()) \
continue; \
const Cost* costs = imageCosts.cdata()+pixel.idx; \
AccumCost* accums = imageAccumCosts.data()+pixel.idx; \
const LineData& Lp = lines(0); \
LineData& Ls = lines(1); \
Ls.R = pixel.range; \
const ImageGray::Type I(leftImage.imageGray(u)); \
pixelAccum(costs, Lp, Ls, accums, I-Ip); \
Ip = I; \
lines.NextLine(); \
} while (cond)
{ // width-down
auto pixels = [&](int x) {
ImageRef u(x,0);
ACCUM_PIXELS(++u.y < sizeValid.height);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.width, idxPixel, pixels));
WaitThreadWorkers(threads.size());
}
{ // height-right
auto pixels = [&](int y) {
ImageRef u(0,y);
ACCUM_PIXELS(++u.x < sizeValid.width);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.height, idxPixel, pixels));
WaitThreadWorkers(threads.size());
}
{ // width-up
auto pixels = [&](int x) {
ImageRef u(x,sizeValid.height-1);
ACCUM_PIXELS(--u.y >= 0);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.width, idxPixel, pixels));
WaitThreadWorkers(threads.size());
}
{ // height-left
auto pixels = [&](int y) {
ImageRef u(sizeValid.width-1,y);
ACCUM_PIXELS(--u.x >= 0);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.height, idxPixel, pixels));
WaitThreadWorkers(threads.size());
}
if (numDirs == 4) {
{ // width-right-down
auto pixels = [&](int x) {
ImageRef u(x,0);
ACCUM_PIXELS(++u.x < sizeValid.width && ++u.y < sizeValid.height);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.width, idxPixel, pixels));
}
{ // height-right-down
auto pixels = [&](int y) {
ImageRef u(0,y);
ACCUM_PIXELS(++u.x < sizeValid.width && ++u.y < sizeValid.height);
};
volatile Thread::safe_t idxPixel(0);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.height, idxPixel, pixels));
}
WaitThreadWorkers(threads.size()*2);
{ // width-left-down
auto pixels = [&](int x) {
ImageRef u(x,0);
ACCUM_PIXELS(--u.x >= 0 && ++u.y < sizeValid.height);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.width-1, idxPixel, pixels));
}
{ // height-left-down
auto pixels = [&](int y) {
ImageRef u(sizeValid.width-1,y);
ACCUM_PIXELS(--u.x >= 0 && ++u.y < sizeValid.height);
};
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.height, idxPixel, pixels));
}
WaitThreadWorkers(threads.size()*2);
{ // width-right-up
auto pixels = [&](int x) {
ImageRef u(x,sizeValid.height-1);
ACCUM_PIXELS(++u.x < sizeValid.width && --u.y >= 0);
};
volatile Thread::safe_t idxPixel(0);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.width, idxPixel, pixels));
}
{ // height-right-up
auto pixels = [&](int y) {
ImageRef u(0,y);
ACCUM_PIXELS(++u.x < sizeValid.width && --u.y >= 0);
};
volatile Thread::safe_t idxPixel(sizeValid.height);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumDec(idxPixel, pixels));
}
WaitThreadWorkers(threads.size()*2);
{ // width-left-up
auto pixels = [&](int x) {
ImageRef u(x,sizeValid.height-1);
ACCUM_PIXELS(--u.x >= 0 && --u.y >= 0);
};
volatile Thread::safe_t idxPixel(sizeValid.width);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumDec(idxPixel, pixels));
}
{ // height-left-up
auto pixels = [&](int y) {
ImageRef u(sizeValid.width-1,y);
ACCUM_PIXELS(--u.x >= 0 && --u.y >= 0);
};
volatile Thread::safe_t idxPixel(sizeValid.height-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumDec(idxPixel, pixels));
}
WaitThreadWorkers(threads.size()*2);
}
#undef ACCUM_PIXELS
} else {
const ImageRef dirs[] = {{-1,0}, {0,-1}, {-1,-1}, {1,-1}};
struct AccumLines {
const Disparity maxNumDisp;
LineData* linesBuffer;
LineData* lines[numDirs][2];
AccumLines(Disparity _maxNumDisp) : maxNumDisp(_maxNumDisp), linesBuffer(NULL) {}
~AccumLines() { delete[] linesBuffer; }
void Init(int w) {
const int linewidth(w+2);
const int buffersize(2*linewidth*numDirs);
if (linesBuffer == NULL) {
linesBuffer = new LineData[buffersize];
for (int i=0; i<buffersize; ++i)
linesBuffer[i].L = new AccumCost[maxNumDisp];
LineData* line(linesBuffer-linewidth+1);
for (int idxDir=0; idxDir<numDirs; ++idxDir) {
lines[idxDir][0] = (line+=linewidth);
lines[idxDir][1] = (line+=linewidth);
}
}
for (int i=0; i<buffersize; ++i) {
LineData& line = linesBuffer[i];
memset(line.L, 0, sizeof(AccumCost)*maxNumDisp);
line.R.minDisp = line.R.maxDisp = 0;
}
}
void NextLine() {
for (int idxDir=0; idxDir<numDirs; ++idxDir)
std::swap(lines[idxDir][0], lines[idxDir][1]);
}
const LineData& operator() (int idxDir, int r, int c) const { return lines[idxDir][r][c]; }
LineData& operator() (int idxDir, int r, int c) { return lines[idxDir][r][c]; }
};
AccumLines lines(maxNumDisp);
#define ACCUM_PIXELS(dx, dy, _x) \
const int idx(r*sizeValid.width+c); \
const PixelData& pixel = imagePixels[idx]; \
if (!pixel.range.isValid()) \
continue; \
const Cost* costs = imageCosts.cdata()+pixel.idx; \
AccumCost* accums = imageAccumCosts.data()+pixel.idx; \
for (int idxDir=0; idxDir<numDirs; ++idxDir) { \
const ImageRef& dir = dirs[idxDir]; \
const LineData& Lp = lines(idxDir,1+dir.y,_x+dir.x); \
LineData& Ls = lines(idxDir,1,_x); \
Ls.R = pixel.range; \
const ImageRef xp(c+dx, r+dy); \
const ImageGray::Type DI(leftImage.imageGray(r,c)-(leftImage.imageGray.isInside(xp)?leftImage.imageGray(xp):Igray)); \
pixelAccum(costs, Lp, Ls, accums, DI); \
}
lines.Init(sizeValid.width);
for (int r=0; r<sizeValid.height; ++r) {
for (int c=0; c<sizeValid.width; ++c) {
ACCUM_PIXELS(dir.x, dir.y, c);
}
lines.NextLine();
}
lines.Init(sizeValid.width);
for (int r=sizeValid.height; --r>=0; ) {
for (int c=sizeValid.width; --c>=0; ) {
ACCUM_PIXELS(-dir.x, -dir.y, sizeValid.width-1-c);
}
lines.NextLine();
}
#undef ACCUM_PIXELS
}
}
// select best disparity and cost
{
disparityMap.create(sizeValid);
costMap.create(sizeValid);
auto pixel = [&](int idx) {
const PixelData& pixel = imagePixels[idx];
if (pixel.range.isValid()) {
const AccumCost* accums = imageAccumCosts.cdata()+pixel.idx;
const AccumCost* bestAccum = accums;
for (const AccumCost *accum=accums+1, *accumEnd=accums+pixel.range.numDisp(); accum<accumEnd; ++accum) {
if (*bestAccum > *accum)
bestAccum = accum;
}
disparityMap(idx) = pixel.range.minDisp+(Disparity)(bestAccum-accums);
costMap(idx) = *bestAccum;
} else {
disparityMap(idx) = pixel.range.minDisp;
costMap(idx) = NO_ACCUMCOST;
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(sizeValid.area(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<sizeValid.height; ++r)
for (int c=0; c<sizeValid.width; ++c)
pixel(r*sizeValid.width+c);
}
}
#if SGM_SIMILARITY == SGM_SIMILARITY_CENSUS
// Compute the census bit-mask for all the pixels of the image
void SemiGlobalMatcher::CensusTransform(const Image8U& imageGray, CensusMap& imageCensus)
{
ASSERT(!imageGray.empty());
const cv::Size size(imageGray.size());
const cv::Size sizeValid(size.width-2*halfWindowSizeX, size.height-2*halfWindowSizeY);
imageCensus.create(sizeValid);
#if 0
// pre-process input image to easy CENSUS task
ImageGray image;
PrefilterXSobel(imageGray, image);
#else
const Image8U& image = imageGray;
#endif
auto pixel = [&](int, int r, int c) {
const ImageRef u(c+halfWindowSizeX, r+halfWindowSizeY);
const uint8_t g(image(u));
Census& cs = imageCensus(r,c);
cs = 0;
for (int i=-halfWindowSizeY; i<=halfWindowSizeY; ++i) {
for (int j=-halfWindowSizeX; j<=halfWindowSizeX; ++j) {
cs <<= 1;
if (g <= image(u.y+i, u.x+j))
cs += 1;
}
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(imageCensus.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<imageCensus.rows; ++r)
for (int c=0; c<imageCensus.cols; ++c)
pixel(-1, r, c);
}
#endif
// Compute search range from the given disparity-map and setup pixel-map at twice the scale;
// the validity mask-map is considered as well and upscaled in the same time;
// return the total size of the disparities searched
SemiGlobalMatcher::Index SemiGlobalMatcher::Disparity2RangeMap(const DisparityMap& disparityMap, const MaskMap& maskMap, Disparity minNumDisp, Disparity minNumDispInvalid)
{
ASSERT(!disparityMap.empty() && disparityMap.width()<maskMap.width() && disparityMap.height()<maskMap.height());
const cv::Size size2x(maskMap.size());
imagePixels.resize(size2x.area());
Index numCosts(0);
maxNumDisp = 0;
CLISTDEF0IDX(Disparity,Disparity) disps(31*31);
for (int r=0; r<disparityMap.rows; ++r) {
const int r2(r == 0 ? 0 : r*2+halfWindowSizeY);
ASSERT(r2 < size2x.height);
const int offset(r2*size2x.width);
int c2e(halfWindowSizeX);
const Mask* pm(maskMap.ptr<const Mask>(r*2+halfWindowSizeY, halfWindowSizeX));
for (int c=0, c2=0; c<disparityMap.cols; ++c, pm+=2) {
Disparity numDisp; Range range;
if (*pm == INVALID) {
// set empty range
range = Range{NO_DISP,NO_DISP};
numDisp = 0;
} else {
// set range based on the estimates around this location
const bool bInvalid(disparityMap(r,c) == NO_DISP);
// search range around 41x41 or 7x7 window
disps.Empty();
const int hw(bInvalid ? 20 : 3);
for (int i=-hw; i<=hw; ++i) {
for (int j=-hw; j<=hw; ++j) {
const ImageRef u(c+j,r+i);
if (disparityMap.isInside(u)) {
const Disparity d(disparityMap(u));
if (d != NO_DISP)
disps.push_back(d);
}
}
}
// set search range
if (disps.size() < 3) {
range.maxDisp = MINF((Disparity)(disparityMap.width()*2/3), minNumDispInvalid);
range.minDisp = -range.maxDisp;
numDisp = range.numDisp();
} else {
const Disparity disp(disps.GetMedian<Disparity>()*2);
const auto minmax(disps.GetMinMax());
numDisp = (minmax.second-minmax.first)*2;
if (numDisp < minNumDisp) {
numDisp = minNumDisp;
range.minDisp = disp-numDisp/2;
range.maxDisp = disp+(numDisp+1)/2;
} else {
const Disparity maxNumDisp(bInvalid ? 64 : 32);
if (numDisp > maxNumDisp) {
range.minDisp = disp-(maxNumDisp*(disp-minmax.first*2)+1)/numDisp;
range.maxDisp = disp+(maxNumDisp*(minmax.second*2+1-disp)+1)/numDisp;
numDisp = range.numDisp();
} else {
range.minDisp = disp-numDisp/2;
range.maxDisp = disp+(numDisp+1)/2;
}
}
}
ASSERT(range.numDisp() == numDisp);
if (maxNumDisp < numDisp)
maxNumDisp = numDisp;
}
c2e += 2;
do {
ASSERT(c2 < size2x.width);
PixelData& pixel = imagePixels[offset+c2];
pixel.range = range;
pixel.idx = numCosts;
numCosts += numDisp;
} while (++c2 < c2e);
}
ASSERT(c2e < size2x.width);
do {
const PixelData& pixel = imagePixels[offset+c2e-1];
PixelData& _pixel = imagePixels[offset+c2e];
_pixel.range = pixel.range;
_pixel.idx = numCosts;
numCosts += pixel.range.numDisp();
} while (++c2e < size2x.width);
const int _offsete((r+1 == disparityMap.rows ? size2x.height : r*2+halfWindowSizeY+2)*size2x.width);
for (int _offset=offset+size2x.width; _offset<_offsete; _offset+=size2x.width) {
for (int c2=0; c2<size2x.width; ++c2) {
const PixelData& pixel = imagePixels[offset+c2];
PixelData& _pixel = imagePixels[_offset+c2];
_pixel.range = pixel.range;
_pixel.idx = numCosts;
numCosts += pixel.range.numDisp();
}
}
}
return numCosts;
}
// Check for consistency between a left-to-right and right-to-left pair of stereo results;
// the results are expected to be opposite in sign but equal in magnitude;
// the valid disparities are returned in the left map
void SemiGlobalMatcher::ConsistencyCrossCheck(DisparityMap& l2r, const DisparityMap& r2l, Disparity thCross)
{
ASSERT(thCross >= 0);
ASSERT(!l2r.empty() && !r2l.empty());
ASSERT(l2r.height() == r2l.height());
auto pixel = [&](int, int r, int c) {
Disparity& ld = l2r(r,c);
if (ld == NO_DISP)
return;
// compute the corresponding disparity pixel according to the disparity value
const ImageRef v(c+ld,r);
// check image bounds
if (v.x < 0 || v.x >= r2l.width()) {
ld = NO_DISP;
return;
}
// check right disparity is valid
const Disparity rd = r2l(v);
if (r2l(v) == NO_DISP) {
ld = NO_DISP;
return;
}
// check disparity consistency:
// since the left and right disparities are opposite in sign,
// we determine their similarity by *summing* them, rather
// than differencing them as you might expect
if (ABS(ld + rd) > thCross)
ld = NO_DISP;
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(l2r.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<l2r.rows; ++r)
for (int c=0; c<l2r.cols; ++c)
pixel(-1, r, c);
}
// Discard disparities that have a high similarity score
void SemiGlobalMatcher::FilterByCost(DisparityMap& disparityMap, const AccumCostMap& costMap, AccumCost th)
{
ASSERT(th > 0);
ASSERT(!disparityMap.empty() && disparityMap.size() == costMap.size());
auto pixel = [&](int, int r, int c) {
Disparity& d = disparityMap(r,c);
if (d == NO_DISP)
return;
if (costMap(r,c) > th)
d = NO_DISP;
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(disparityMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
for (int c=0; c<disparityMap.cols; ++c)
pixel(-1, r, c);
}
// Mark empty regions on the border of the disparity-map as invalid;
// thValid is the number of valid disparities encountered to consider the valid region starts
void SemiGlobalMatcher::ExtractMask(const DisparityMap& disparityMap, MaskMap& maskMap, int thValid)
{
ASSERT(!disparityMap.empty());
ASSERT(maskMap.empty() || disparityMap.size() == maskMap.size());
if (disparityMap.size() != maskMap.size()) {
maskMap.create(disparityMap.size());
maskMap.setTo(VALID);
}
#define MASK_PIXEL() \
Mask& m = maskMap(r,c); \
if (m == INVALID) \
continue; \
m = INVALID; \
if (disparityMap(r,c) == NO_DISP) \
continue; \
if (++numValid >= thValid) \
break
// left-right direction
{
auto pixel = [&](int r) {
int numValid(0);
for (int c=0; c<disparityMap.cols; ++c) {
MASK_PIXEL();
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(disparityMap.height(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
pixel(r);
}
// right-left direction
{
auto pixel = [&](int r) {
int numValid(0);
for (int c=disparityMap.cols; --c>=0; ) {
MASK_PIXEL();
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(disparityMap.height(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
pixel(r);
}
#undef MASK_PIXEL
#if 0
#define MASK_PIXEL() \
Mask& m = maskMap(r,c); \
if (m == INVALID) \
continue; \
if (disparityMap(r,c) != NO_DISP) \
break; \
m = INVALID
// top-bottom direction
{
auto pixel = [&](int c) {
for (int r=0; r<disparityMap.rows; ++r) {
MASK_PIXEL();
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(disparityMap.width(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int c=0; c<disparityMap.cols; ++c)
pixel(c);
}
// bottom-top direction
{
auto pixel = [&](int c) {
for (int r=disparityMap.rows; --r>=0; ) {
MASK_PIXEL();
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(disparityMap.width(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int c=0; c<disparityMap.cols; ++c)
pixel(c);
}
#undef MASK_PIXEL
#endif
}
// Translate disparity-map between left-to-right and right-to-left stereo pair
// by translating to the x coordinated dictated by the disparity and negating the sign;
// the sub-pixel steps is assumed to be one and
// the input disparity-map has been cross-checked for consistency
void SemiGlobalMatcher::FlipDirection(const DisparityMap& l2r, DisparityMap& r2l)
{
ASSERT(!l2r.empty());
ASSERT(r2l.empty() || l2r.height() == r2l.height());
if (r2l.empty())
r2l.create(l2r.size());
r2l.setTo(NO_DISP);
auto pixel = [&](int r) {
for (int c=0; c<l2r.cols; ++c) {
const Disparity d = l2r(r,c);
if (d == NO_DISP)
continue;
// compute the corresponding disparity pixel according to the disparity value and set right disparity value
for (int x=MAXF(c+d-1,0), xe=MINF(c+d+2,r2l.width()); x<xe; ++x)
r2l(r,x) = -d;
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(l2r.rows, idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<l2r.rows; ++r)
pixel(r);
}
// Translate disparity-map between left-to-right and right-to-left stereo pair
// by translating to the x coordinated dictated by the disparity and negating the sign;
// the input disparity-map has been cross-checked for consistency
void SemiGlobalMatcher::UpscaleMask(MaskMap& maskMap, const cv::Size& size2x)
{
ASSERT(!maskMap.empty());
MaskMap maskMap2x(size2x, INVALID);
auto pixel = [&](int, int r, int c) {
const int r2(r*2+halfWindowSizeY), c2(c*2+halfWindowSizeX);
const Mask m(maskMap(r,c));
for (int i=0; i<2; ++i) {
for (int j=0; j<2; ++j) {
const ImageRef u(c2+j,r2+i);
if (maskMap2x.isInside(u))
maskMap2x(u) = m;
}
}
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(maskMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<maskMap.rows; ++r)
for (int c=0; c<maskMap.cols; ++c)
pixel(-1, r, c);
cv::swap(maskMap, maskMap2x);
}
// Sub-pixel disparity-map refinement based on the accumulated-cost volume
void SemiGlobalMatcher::RefineDisparityMap(DisparityMap& disparityMap) const
{
ASSERT(!disparityMap.empty());
if (subpixelSteps <= 1)
return;
if (subpixelMode == SUBPIXEL_NA) {
// simply multiply disparity to the sub-pixel steps
auto pixel = [&](int, int r, int c) {
Disparity& d = disparityMap(r,c);
if (d == NO_DISP)
return;
ASSERT((int)d*subpixelSteps > (int)std::numeric_limits<Disparity>::min());
ASSERT((int)d*subpixelSteps < (int)std::numeric_limits<Disparity>::max());
d *= subpixelSteps;
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(disparityMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
for (int c=0; c<disparityMap.cols; ++c)
pixel(-1, r, c);
return;
}
// proposed subpixelMode algorithms
typedef float real;
struct Fit {
static real linear(real x) {
return x/real(2);
}
static real poly4(real x) {
return (x*x*x*x + x)/real(4);
}
static real parabola(real x) {
return x/(x+real(1));
}
static real sine(real x) {
return real(0.5) * (SIN((x-real(1))*real(HALF_PI)) + real(1));
}
static real cosine(real x) {
return (real(1) - COS(x*(real)(PI/3.0)));
}
static real lcBlend(real x) {
const real factor(real(1.195) - COS(x*(real)(PI/2.3)));
return cosine(x)*factor + linear(x)*(real(1)-factor);
}
// subpixelMode interpolation when only two values are available
// returns fraction of distance from the primary to the other value
static real semisubpixel(AccumCost primary, AccumCost other) {
return real(0.5)*(static_cast<real>(primary) / static_cast<real>(other));
}
// compute the offset to be added to the integer disparity to get the final result
static real subpixelMode(AccumCost prev, AccumCost center, AccumCost next, SgmSubpixelMode subpixelMode) {
ASSERT(prev != NO_ACCUMCOST && center != NO_ACCUMCOST && next != NO_ACCUMCOST);
// use a lower quality two value interpolation if only two values are available
if (prev == center)
return center == next ? real(0) : semisubpixel(center, next);
if (center == next)
return prev == center ? real(0) : -semisubpixel(center, prev);
// pick which direction to interpolate in
const AccumCost ld(prev-center);
const AccumCost rd(next-center);
real x, mult;
if (ld < rd) {
x = static_cast<real>(ld) / static_cast<real>(rd);
mult = real(1);
} else {
x = static_cast<real>(rd) / static_cast<real>(ld);
mult = real(-1);
}
// use the selected subpixelMode function
real value(0);
switch (subpixelMode) {
case SUBPIXEL_LINEAR: value = linear(x); break;
case SUBPIXEL_POLY4: value = poly4(x); break;
case SUBPIXEL_PARABOLA: value = parabola(x); break;
case SUBPIXEL_SINE: value = sine(x); break;
case SUBPIXEL_COSINE: value = cosine(x); break;
case SUBPIXEL_LC_BLEND: value = lcBlend(x); break;
};
// complete computation
return (value - real(0.5))*mult;
}
};
// estimate sub-pixel disparity based on the cost values
auto pixel = [&](int idx) {
const PixelData& pixel = imagePixels[idx];
if (pixel.range.numDisp() < 2)
return;
Disparity& d = disparityMap(idx);
if (d == NO_DISP)
return;
const AccumCost* accums = imageAccumCosts.cdata()+pixel.idx;
const int idxDisp(d-pixel.range.minDisp);
real disparity((real)d);
if (d == pixel.range.minDisp)
disparity += Fit::semisubpixel(accums[idxDisp], accums[idxDisp+1]);
else if (d+1 == pixel.range.maxDisp)
disparity -= Fit::semisubpixel(accums[idxDisp], accums[idxDisp-1]);
else
disparity += Fit::subpixelMode(accums[idxDisp-1], accums[idxDisp], accums[idxDisp+1], subpixelMode);
ASSERT(ROUND2INT(disparity*subpixelSteps) > (int)std::numeric_limits<Disparity>::min());
ASSERT(ROUND2INT(disparity*subpixelSteps) < (int)std::numeric_limits<Disparity>::max());
d = (Disparity)ROUND2INT(disparity*subpixelSteps);
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelAccumInc(disparityMap.size().area(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
for (int c=0; c<disparityMap.cols; ++c)
pixel(r*disparityMap.cols+c);
}
#ifndef _RELEASE
// extract disparity and range from the pixel-map
void SemiGlobalMatcher::DisplayState(const cv::Size& size) const
{
Image8U disparity(size);
Image8U range(size);
for (int idx=0; idx<size.area(); ++idx) {
const PixelData& pixel = imagePixels[idx];
disparity(idx) = (uint8_t)(CLAMP(pixel.range.avgDisp(), Disparity(-128), Disparity(127))+Disparity(128));
range(idx) = (uint8_t)CLAMP(pixel.range.numDisp(), Disparity(0), Disparity(255));
}
disparity.Show("Disparity", -1, false);
range.Show("Range", -1, false);
char c = 'a';
while (std::tolower(c) != 'q')
c = cv::waitKey();
cv::destroyAllWindows();
}
#endif
// Compute the disparity-map for the rectified image from the given depth-map of the un-rectified image;
// the disparity map needs to be already constructed at the desired size (valid size, excluding the border)
void SemiGlobalMatcher::Depth2DisparityMap(const DepthMap& depthMap, const Matrix3x3& invH, const Matrix4x4& invQ, Disparity subpixelSteps, DisparityMap& disparityMap)
{
auto pixel = [&](int, int r, int c) {
const ImageRef x(c+halfWindowSizeX,r+halfWindowSizeY); Point2f u;
ProjectVertex_3x3_2_2(invH.val, x.ptr(), u.ptr());
float depth, disparity;
if (!depthMap.sampleSafe(depth, u, [](Depth d) { return d > 0; }) || !Image::Depth2Disparity(invQ, u, depth, disparity))
disparityMap(r,c) = NO_DISP;
else
disparityMap(r,c) = (Disparity)ROUND2INT(disparity*subpixelSteps);
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(disparityMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<disparityMap.rows; ++r)
for (int c=0; c<disparityMap.cols; ++c)
pixel(-1, r, c);
}
// Compute the depth-map for the un-rectified image from the given disparity-map of the rectified image
void SemiGlobalMatcher::Disparity2DepthMap(const DisparityMap& disparityMap, const AccumCostMap& costMap, const Matrix3x3& H, const Matrix4x4& Q, Disparity subpixelSteps, DepthMap& depthMap, ConfidenceMap& confMap)
{
ASSERT(costMap.empty() || costMap.size() == disparityMap.size());
ASSERT(!depthMap.empty());
ASSERT(confMap.empty() || confMap.size() == depthMap.size());
if (!costMap.empty()) {
confMap.create(depthMap.size());
auto pixel = [&](int, int r, int c) {
const ImageRef x(c,r); Point2f u;
ProjectVertex_3x3_2_2(H.val, x.ptr(), u.ptr());
u.x -= (float)halfWindowSizeX;
u.y -= (float)halfWindowSizeY;
float disparity;
if (!disparityMap.sampleSafe(disparity, u, [](Disparity d) { return d != NO_DISP; })) {
depthMap(x) = 0;
confMap(x) = 0;
return;
}
float cost;
costMap.sampleSafe(cost, u, [](AccumCost c) { return c != NO_ACCUMCOST; });
depthMap(x) = Image::Disparity2Depth(Q, u, disparity/subpixelSteps);
confMap(x) = 1.f/(cost+1);
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(depthMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<depthMap.rows; ++r)
for (int c=0; c<depthMap.cols; ++c)
pixel(-1, r, c);
} else {
auto pixel = [&](int, int r, int c) {
const ImageRef x(c,r); Point2f u;
ProjectVertex_3x3_2_2(H.val, x.ptr(), u.ptr());
u.x -= (float)halfWindowSizeX;
u.y -= (float)halfWindowSizeY;
float disparity;
if (!disparityMap.sampleSafe(disparity, u, [](Disparity d) { return d != NO_DISP; }))
depthMap(x) = 0;
else
depthMap(x) = Image::Disparity2Depth(Q, u, disparity/subpixelSteps);
};
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
volatile Thread::safe_t idxPixel(-1);
FOREACH(i, threads)
threads.AddEvent(new EVTPixelProcess(depthMap.size(), idxPixel, pixel));
WaitThreadWorkers(threads.size());
} else
for (int r=0; r<depthMap.rows; ++r)
for (int c=0; c<depthMap.cols; ++c)
pixel(-1, r, c);
}
}
// Compute the depth-map for the un-rectified image from the given disparity-map of the rectified image
// by projecting the point-cloud to the image and setting the pixel depth as the average of the closest 4 points;
// return false if the disparity map is completely empty
bool SemiGlobalMatcher::ProjectDisparity2DepthMap(const DisparityMap& disparityMap, const AccumCostMap& costMap, const Matrix4x4& Q, Disparity subpixelSteps, DepthMap& depthMap, DepthRangeMap& depthRangeMap, ConfidenceMap& confMap)
{
ASSERT(costMap.empty() || costMap.size() == disparityMap.size());
ASSERT(!depthMap.empty());
ASSERT(confMap.empty() || confMap.size() == depthMap.size());
// store 4 depths per pixel, the closest one from each side, ordered as:
// - left, right, top, bottom (x pointing right and y pointing down)
// with a small overlap border
const float overlapBorder(0.5f+0.25f);
struct DepthData {
Image32F distMap;
DepthMap depthMap;
DepthRangeMap depthRangeMap;
ConfidenceMap confMap;
void Init(const cv::Size& size) {
distMap.create(size);
depthMap.create(size);
depthRangeMap.create(size);
confMap.create(size);
depthMap.memset(0);
}
} depthDatas[4];
for (int i=0; i<4; ++i)
depthDatas[i].Init(depthMap.size());
for (int r=0; r<disparityMap.rows; ++r) {
for (int c=0; c<disparityMap.cols; ++c) {
const Disparity disparityInt(disparityMap(r,c));
if (disparityInt == NO_DISP)
continue;
const float disparity((float)disparityInt/subpixelSteps);
Point2f u, v;
const ImageRef dx(c+halfWindowSizeX,r+halfWindowSizeY);
const Depth depth(Image::Disparity2Depth(Q, dx, disparity, u));
if (depth <= 0)
continue;
const Disparity disparityCenter((Disparity)FLOOR2INT(disparity));
const DepthRange depthRange(
Image::Disparity2Depth(Q, dx, (float)(disparityCenter-1)),
Image::Disparity2Depth(Q, dx, (float)(disparityCenter+1))
);
ASSERT(ISINSIDE(depth, depthRange.x, depthRange.y));
const float cost(costMap.empty()?0.f:1.f/(costMap(r,c)+1));
const ImageRef x(FLOOR2INT(u));
u.x -= 0.5f; u.y -= 0.5f;
for (int i=-1; i<=1; ++i) {
for (int j=-1; j<=1; ++j) {
const ImageRef nx(x.x+j,x.y+i);
if (!depthMap.isInside(nx))
continue;
const Point2f dist((float)nx.x-u.x,(float)nx.y-u.y);
const Point2f absDist(ABS(dist));
if (absDist.x > overlapBorder || absDist.y > overlapBorder)
continue;
const int idx((dist.x<0?1:0)+(dist.y<0?2:0));
DepthData& depthData = depthDatas[idx];
const float deistSq(normSq(dist));
float& ndeistSq = depthData.distMap(nx);
Depth& ndepth = depthData.depthMap(nx);
if (ndepth > 0 && ndeistSq <= deistSq)
continue;
ndeistSq = deistSq;
ndepth = depth;
depthData.depthRangeMap(nx) = depthRange;
depthData.confMap(nx) = cost;
}
}
}
}
typedef TAccumulator<Vec4f> ValueAccumulator;
depthRangeMap.create(depthMap.size());
confMap.create(depthMap.size());
const float thDist(SQUARE(0.75f));
const Depth thDepth(0.02f);
unsigned numDepths(0);
for (int r=0; r<depthMap.rows; ++r) {
for (int c=0; c<depthMap.cols; ++c) {
float distCenter(std::numeric_limits<float>::max());
Depth depthCenter;
for (int i=0; i<4; ++i) {
const DepthData& depthData = depthDatas[i];
const Depth depth(depthData.depthMap(r,c));
if (depth <= 0)
continue;
const float dist(depthData.distMap(r,c));
if (distCenter > dist) {
distCenter = dist;
depthCenter = depth;
}
}
if (distCenter > thDist) {
depthMap(r,c) = Depth(0);
continue;
}
ValueAccumulator acc(Vec4f::ZERO, 0.f);
for (int i=0; i<4; ++i) {
const DepthData& depthData = depthDatas[i];
const Depth depth(depthData.depthMap(r,c));
if (depth <= 0 || !IsDepthSimilar(depthCenter, depth, thDepth))
continue;
const DepthRange& depthRange(depthData.depthRangeMap(r,c));
acc.Add(Vec4f((float)depth,depthRange.x,depthRange.y,depthData.confMap(r,c)), SQRT(depthData.distMap(r,c)));
}
ASSERT(!acc.IsEmpty());
const Vec4f value(acc.Normalized());
depthMap(r,c) = value(0);
depthRangeMap(r,c) = DepthRange(value(1),value(2));
confMap(r,c) = value(3);
++numDepths;
}
}
if (costMap.empty())
confMap.release();
return numDepths > 0;
}
EventThreadPool SemiGlobalMatcher::threads;
Semaphore SemiGlobalMatcher::sem;
// start worker threads
void SemiGlobalMatcher::CreateThreads(unsigned nMaxThreads)
{
ASSERT(nMaxThreads > 0);
ASSERT(threads.IsEmpty() && threads.empty());
if (nMaxThreads > 1) {
threads.resize(nMaxThreads);
threads.start(ThreadWorker);
}
}
// destroy worker threads
void SemiGlobalMatcher::DestroyThreads()
{
ASSERT(threads.IsEmpty());
if (!threads.empty()) {
FOREACH(i, threads)
threads.AddEvent(new EVTClose());
threads.Release();
}
}
void* SemiGlobalMatcher::ThreadWorker(void*) {
while (true) {
CAutoPtr<Event> evt(threads.GetEvent());
switch (evt->GetID()) {
case EVT_JOB:
evt->Run();
break;
case EVT_CLOSE:
return NULL;
default:
ASSERT("Should not happen!" == NULL);
}
sem.Signal();
}
return NULL;
}
void SemiGlobalMatcher::WaitThreadWorkers(unsigned nJobs)
{
while (nJobs-- > 0)
sem.Wait();
ASSERT(threads.IsEmpty());
}
/*----------------------------------------------------------------*/
// S T R U C T S ///////////////////////////////////////////////////
bool SemiGlobalMatcher::ExportDisparityDataRaw(const String& fileName, const DisparityMap& disparityMap, const AccumCostMap& costMap, const cv::Size& imageSize, const Matrix3x3& H, const Matrix4x4& Q, Disparity subpixelSteps)
{
ASSERT(!disparityMap.empty());
ASSERT(costMap.empty() || disparityMap.size() == costMap.size());
FILE *f = fopen(fileName, "wb");
if (f == NULL)
return false;
// write info
fwrite(&imageSize.width, sizeof(int), 2, f);
fwrite(H.val, sizeof(REAL), 9, f);
fwrite(Q.val, sizeof(REAL), 16, f);
fwrite(&subpixelSteps, sizeof(Disparity), 1, f);
// write resolution
fwrite(&disparityMap.cols, sizeof(int), 1, f);
fwrite(&disparityMap.rows, sizeof(int), 1, f);
// write disparity-map
fwrite(disparityMap.getData(), sizeof(Disparity), disparityMap.area(), f);
// write cost-map
if (!costMap.empty())
fwrite(costMap.getData(), sizeof(AccumCost), costMap.area(), f);
fclose(f);
return true;
} // ExportDisparityDataRaw
// same as above, but exports also the empty border
bool SemiGlobalMatcher::ExportDisparityDataRawFull(const String& fileName, const DisparityMap& disparityMap, const AccumCostMap& costMap, const cv::Size& imageSize, const Matrix3x3& H, const Matrix4x4& Q, Disparity subpixelSteps)
{
ASSERT(!disparityMap.empty());
ASSERT(costMap.empty() || disparityMap.size() == costMap.size());
const cv::Size size(disparityMap.width()+2*halfWindowSizeX,disparityMap.height()+2*halfWindowSizeY);
const cv::Rect ROI(halfWindowSizeX,halfWindowSizeY, disparityMap.width(),disparityMap.height());
DisparityMap disparityMapFull(size, NO_DISP);
disparityMap.copyTo(disparityMapFull(ROI));
if (costMap.empty())
return ExportDisparityDataRaw(fileName, disparityMapFull, costMap, imageSize, H, Q, subpixelSteps);
AccumCostMap costMapFull(size, NO_ACCUMCOST);
costMap.copyTo(costMapFull(ROI));
return ExportDisparityDataRaw(fileName, disparityMapFull, costMapFull, imageSize, H, Q, subpixelSteps);
} // ExportDisparityDataRawFull
bool SemiGlobalMatcher::ImportDisparityDataRaw(const String& fileName, DisparityMap& disparityMap, AccumCostMap& costMap, cv::Size& imageSize, Matrix3x3& H, Matrix4x4& Q, Disparity& subpixelSteps)
{
FILE *f = fopen(fileName, "rb");
if (f == NULL)
return false;
// read info
fread(&imageSize.width, sizeof(int), 2, f);
fread(H.val, sizeof(REAL), 9, f);
fread(Q.val, sizeof(REAL), 16, f);
fread(&subpixelSteps, sizeof(Disparity), 1, f);
ASSERT(imageSize.width > 0 && imageSize.height > 0);
// read resolution
int w, h;
fread(&w, sizeof(int), 1, f);
fread(&h, sizeof(int), 1, f);
ASSERT(w > 0 && h > 0);
// read disparity-map
disparityMap.create(h,w);
fread(disparityMap.getData(), sizeof(Disparity), w*h, f);
// read cost-map
if (fgetc(f) != EOF) {
fseek(f, -1, SEEK_CUR);
costMap.create(h,w);
fread(costMap.getData(), sizeof(AccumCost), w*h, f);
}
fclose(f);
return true;
} // ImportDisparityDataRaw
// same as above, but imports also the empty border
bool SemiGlobalMatcher::ImportDisparityDataRawFull(const String& fileName, DisparityMap& disparityMap, AccumCostMap& costMap, cv::Size& imageSize, Matrix3x3& H, Matrix4x4& Q, Disparity& subpixelSteps)
{
if (!ImportDisparityDataRaw(fileName, disparityMap, costMap, imageSize, H, Q, subpixelSteps))
return false;
const cv::Size sizeValid(disparityMap.width()-2*halfWindowSizeX,disparityMap.height()-2*halfWindowSizeY);
const cv::Rect ROI(halfWindowSizeX,halfWindowSizeY, sizeValid.width,sizeValid.height);
disparityMap = disparityMap(ROI).clone();
if (!costMap.empty())
costMap = costMap(ROI).clone();
return true;
} // ImportDisparityDataRawFull
// export disparity-map as an image (red - maximum disparity, blue - minimum disparity)
Image8U3 SemiGlobalMatcher::DisparityMap2Image(const DisparityMap& disparityMap, Disparity minDisparity, Disparity maxDisparity)
{
ASSERT(!disparityMap.empty());
// find min and max values
if (minDisparity == NO_DISP || maxDisparity == NO_DISP) {
CLISTDEF0(Disparity) disparities(0, disparityMap.area());
for (int i=disparityMap.area(); i-- > 0; ) {
const Disparity disparity = disparityMap[i];
if (disparity != NO_DISP)
disparities.emplace_back(disparity);
}
if (!disparities.empty()) {
const std::pair<float,float> th(ComputeX84Threshold<Disparity,float>(disparities.data(), disparities.size(), 5.2f));
minDisparity = (Disparity)ROUND2INT(th.first-th.second);
maxDisparity = (Disparity)ROUND2INT(th.first+th.second);
}
DEBUG_ULTIMATE("\tdisparity range: [%d, %d]", minDisparity, maxDisparity);
}
const float sclDepth(1.f/(float)(maxDisparity - minDisparity));
// create color image
Image8U3 img(cv::Size(disparityMap.width()+2*halfWindowSizeX,disparityMap.height()+2*halfWindowSizeY), Pixel8U::BLACK);
for (int r=0; r<disparityMap.rows; ++r) {
for (int c=0; c<disparityMap.cols; ++c) {
const Disparity disparity(disparityMap(r,c));
if (disparity == NO_DISP)
continue;
img(r+halfWindowSizeY,c+halfWindowSizeX) = Pixel8U::gray2color(CLAMP((float)(maxDisparity-disparity)*sclDepth, 0.f, 1.f));
}
}
return img;
} // DisparityMap2Image
bool SemiGlobalMatcher::ExportDisparityMap(const String& fileName, const DisparityMap& disparityMap, Disparity minDisparity, Disparity maxDisparity)
{
if (disparityMap.empty())
return false;
return DisparityMap2Image(disparityMap, minDisparity, maxDisparity).Save(fileName);
} // ExportDisparityMap
// export point cloud
bool SemiGlobalMatcher::ExportPointCloud(const String& fileName, const Image& imageData, const DisparityMap& disparityMap, const Matrix4x4& Q, Disparity subpixelSteps)
{
ASSERT(!disparityMap.empty());
// vertex definition
struct Vertex {
float x,y,z;
uint8_t r,g,b;
};
// list of property information for a vertex
static PLY::PlyProperty vert_props[] = {
{"x", PLY::Float32, PLY::Float32, offsetof(Vertex,x), 0, 0, 0, 0},
{"y", PLY::Float32, PLY::Float32, offsetof(Vertex,y), 0, 0, 0, 0},
{"z", PLY::Float32, PLY::Float32, offsetof(Vertex,z), 0, 0, 0, 0},
{"red", PLY::Uint8, PLY::Uint8, offsetof(Vertex,r), 0, 0, 0, 0},
{"green", PLY::Uint8, PLY::Uint8, offsetof(Vertex,g), 0, 0, 0, 0},
{"blue", PLY::Uint8, PLY::Uint8, offsetof(Vertex,b), 0, 0, 0, 0},
};
// list of the kinds of elements in the PLY
static const char* elem_names[] = {
"vertex"
};
// create PLY object
ASSERT(!fileName.empty());
Util::ensureFolder(fileName);
const size_t memBufferSize(disparityMap.area()*(8*3/*pos*/+3*3/*color*/+7/*space*/+2/*eol*/) + 2048/*extra size*/);
PLY ply;
if (!ply.write(fileName, 1, elem_names, PLY::BINARY_LE, memBufferSize))
return false;
// describe what properties go into the vertex elements
ply.describe_property("vertex", 6, vert_props);
// export the array of 3D points
Vertex vertex;
for (int r=0; r<disparityMap.rows; ++r) {
for (int c=0; c<disparityMap.cols; ++c) {
const Disparity& disparity = disparityMap(r,c);
if (disparity == NO_DISP)
continue;
Point2f u;
Depth depth(Image::Disparity2Depth(Q, ImageRef(c+halfWindowSizeX,r+halfWindowSizeY), (float)disparity/subpixelSteps, u));
if (depth <= 0)
continue;
if (!imageData.image.isInsideWithBorder<float,1>(u))
continue;
const Point3f X(imageData.camera.TransformPointI2W(Point3(u,depth)));
vertex.x = X.x; vertex.y = X.y; vertex.z = X.z;
const Pixel8U C(imageData.image.empty() ? Pixel8U::WHITE : imageData.image.sample(u));
vertex.r = C.r; vertex.g = C.g; vertex.b = C.b;
ply.put_element(&vertex);
}
}
if (ply.get_current_element_count() == 0)
return false;
// write to file
return ply.header_complete();
} // ExportPointCloud
// imports a DIMAP file and converts it to point-cloud
bool SemiGlobalMatcher::ImportPointCloud(const String& fileName, const ImageArr& images, PointCloud& pointcloud)
{
// load disparity-map
Disparity subpixelSteps;
cv::Size imageSize; Matrix3x3 H; Matrix4x4 Q;
DisparityMap disparityMap; AccumCostMap costMap;
if (!ImportDisparityDataRawFull(fileName, disparityMap, costMap, imageSize, H, Q, subpixelSteps))
return false;
// parse image index from the file name
const String name(Util::getFileName(fileName));
IIndex idxImage(NO_ID), idxImagePair(NO_ID);
if (sscanf(name, "%u_%u", &idxImage, &idxImagePair) != 2 || idxImage == NO_ID || idxImagePair == NO_ID)
return false;
const Image& imageData = images[idxImage];
ASSERT(imageData.image.size() == imageSize);
// import the array of 3D points
for (int r=0; r<disparityMap.rows; ++r) {
for (int c=0; c<disparityMap.cols; ++c) {
const Disparity& disparity = disparityMap(r,c);
if (disparity == NO_DISP)
continue;
Point2f u;
Depth depth(Image::Disparity2Depth(Q, ImageRef(c+halfWindowSizeX,r+halfWindowSizeY), (float)disparity/subpixelSteps, u));
if (depth <= 0)
continue;
if (!imageData.image.isInsideWithBorder<float,1>(u))
continue;
pointcloud.points.emplace_back(Cast<PointCloud::Point::Type>(imageData.camera.TransformPointI2W(Point3(u,depth))));
pointcloud.colors.emplace_back(imageData.image.empty() ? Pixel8U::WHITE : imageData.image.sample(u));
}
}
return true;
} // ImportPointCloud
/*----------------------------------------------------------------*/
bool MVS::STEREO::ExportCamerasEngin(const Scene& scene, const String& fileName)
{
ASSERT(!scene.IsEmpty());
File f(fileName, File::WRITE, File::CREATE | File::TRUNCATE);
if (!f.isOpen())
return false;
// write header
f.print("n_cameras %u\n", scene.images.size());
f.print("n_points %u\n", 0);
// write cameras
for (const Image& image: scene.images) {
if (!image.IsValid())
continue;
const Point3 t(image.camera.GetT());
f.print("%u %u %u %s "
"%g %g %g %g "
"%g %g %g %g %g %g %g %g %g "
"%g %g %g "
"%g %g "
"%u",
image.ID, image.width, image.height, image.name.c_str(),
image.camera.K(0,0), image.camera.K(1,1), image.camera.K(0,2), image.camera.K(1,2),
image.camera.R(0,0), image.camera.R(0,1), image.camera.R(0,2),
image.camera.R(1,0), image.camera.R(1,1), image.camera.R(1,2),
image.camera.R(2,0), image.camera.R(2,1), image.camera.R(2,2),
t.x, t.y, t.z,
0, 0,
image.neighbors.size()
);
for (const auto& neighbor: image.neighbors)
f.print(" %u", neighbor.ID);
f.print("\n");
}
return true;
} // ExportCamerasEngin
/*----------------------------------------------------------------*/