/* * DepthMap.cpp * * Copyright (c) 2014-2015 SEACAVE * * Author(s): * * cDc * * * 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 . * * * 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 "DepthMap.h" #include "Mesh.h" #include "../Common/AutoEstimator.h" // CGAL: depth-map initialization #include #include #include // CGAL: estimate normals #include #include #include using namespace MVS; // D E F I N E S /////////////////////////////////////////////////// #define DEFVAR_OPTDENSE_string(name, title, desc, ...) DEFVAR_string(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_bool(name, title, desc, ...) DEFVAR_bool(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_int32(name, title, desc, ...) DEFVAR_int32(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_uint32(name, title, desc, ...) DEFVAR_uint32(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_flags(name, title, desc, ...) DEFVAR_flags(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_float(name, title, desc, ...) DEFVAR_float(OPTDENSE, name, title, desc, __VA_ARGS__) #define DEFVAR_OPTDENSE_double(name, title, desc, ...) DEFVAR_double(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_string(name, title, desc, ...) DEFVAR_string(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_bool(name, title, desc, ...) DEFVAR_bool(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_int32(name, title, desc, ...) DEFVAR_int32(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_uint32(name, title, desc, ...) DEFVAR_uint32(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_flags(name, title, desc, ...) DEFVAR_flags(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_float(name, title, desc, ...) DEFVAR_float(OPTDENSE, name, title, desc, __VA_ARGS__) #define MDEFVAR_OPTDENSE_double(name, title, desc, ...) DEFVAR_double(OPTDENSE, name, title, desc, __VA_ARGS__) namespace MVS { DEFOPT_SPACE(OPTDENSE, _T("Dense")) DEFVAR_OPTDENSE_uint32(nResolutionLevel, "Resolution Level", "How many times to scale down the images before dense reconstruction", "1") MDEFVAR_OPTDENSE_uint32(nMaxResolution, "Max Resolution", "Do not scale images lower than this resolution", "3200") MDEFVAR_OPTDENSE_uint32(nMinResolution, "Min Resolution", "Do not scale images lower than this resolution", "640") DEFVAR_OPTDENSE_uint32(nSubResolutionLevels, "SubResolution levels", "Number of lower resolution levels to estimate the depth and normals", "2") DEFVAR_OPTDENSE_uint32(nMinViews, "Min Views", "minimum number of agreeing views to validate a depth", "2") MDEFVAR_OPTDENSE_uint32(nMaxViews, "Max Views", "maximum number of neighbor images used to compute the depth-map for the reference image", "12") DEFVAR_OPTDENSE_uint32(nMinViewsFuse, "Min Views Fuse", "minimum number of images that agrees with an estimate during fusion in order to consider it inlier (<2 - only merge depth-maps)", "2") DEFVAR_OPTDENSE_uint32(nMinViewsFilter, "Min Views Filter", "minimum number of images that agrees with an estimate in order to consider it inlier", "2") MDEFVAR_OPTDENSE_uint32(nMinViewsFilterAdjust, "Min Views Filter Adjust", "minimum number of images that agrees with an estimate in order to consider it inlier (0 - disabled)", "1") MDEFVAR_OPTDENSE_uint32(nMinViewsTrustPoint, "Min Views Trust Point", "min-number of views so that the point is considered for approximating the depth-maps (<2 - random initialization)", "2") MDEFVAR_OPTDENSE_uint32(nNumViews, "Num Views", "Number of views used for depth-map estimation (0 - all views available)", "0", "1", "4") MDEFVAR_OPTDENSE_uint32(nPointInsideROI, "Point Inside ROI", "consider a point shared only if inside ROI when estimating the neighbor views (0 - ignore ROI, 1 - weight more ROI points, 2 - consider only ROI points)", "1") MDEFVAR_OPTDENSE_bool(bFilterAdjust, "Filter Adjust", "adjust depth estimates during filtering", "1") MDEFVAR_OPTDENSE_bool(bAddCorners, "Add Corners", "add support points at image corners with nearest neighbor disparities", "0") MDEFVAR_OPTDENSE_bool(bInitSparse, "Init Sparse", "init depth-map only with the sparse points (no interpolation)", "1") MDEFVAR_OPTDENSE_bool(bRemoveDmaps, "Remove Dmaps", "remove depth-maps after fusion", "0") MDEFVAR_OPTDENSE_float(fViewMinScore, "View Min Score", "Min score to consider a neighbor images (0 - disabled)", "2.0") MDEFVAR_OPTDENSE_float(fViewMinScoreRatio, "View Min Score Ratio", "Min score ratio to consider a neighbor images", "0.03") MDEFVAR_OPTDENSE_float(fMinArea, "Min Area", "Min shared area for accepting the depth triangulation", "0.05") MDEFVAR_OPTDENSE_float(fMinAngle, "Min Angle", "Min angle for accepting the depth triangulation", "3.0") MDEFVAR_OPTDENSE_float(fOptimAngle, "Optim Angle", "Optimal angle for computing the depth triangulation", "12.0") MDEFVAR_OPTDENSE_float(fMaxAngle, "Max Angle", "Max angle for accepting the depth triangulation", "65.0") MDEFVAR_OPTDENSE_float(fDescriptorMinMagnitudeThreshold, "Descriptor Min Magnitude Threshold", "minimum patch texture variance accepted when matching two patches (0 - disabled)", "0.02") // 0.02: pixels with patch texture variance below 0.0004 (0.02^2) will be removed from depthmap; 0.12: patch texture variance below 0.02 (0.12^2) is considered texture-less MDEFVAR_OPTDENSE_float(fDepthDiffThreshold, "Depth Diff Threshold", "maximum variance allowed for the depths during refinement", "0.01") MDEFVAR_OPTDENSE_float(fNormalDiffThreshold, "Normal Diff Threshold", "maximum variance allowed for the normal during fusion (degrees)", "25") MDEFVAR_OPTDENSE_float(fPairwiseMul, "Pairwise Mul", "pairwise cost scale to match the unary cost", "0.3") MDEFVAR_OPTDENSE_float(fOptimizerEps, "Optimizer Eps", "MRF optimizer stop epsilon", "0.001") MDEFVAR_OPTDENSE_int32(nOptimizerMaxIters, "Optimizer Max Iters", "MRF optimizer max number of iterations", "80") MDEFVAR_OPTDENSE_uint32(nSpeckleSize, "Speckle Size", "maximal size of a speckle (small speckles get removed)", "100") MDEFVAR_OPTDENSE_uint32(nIpolGapSize, "Interpolate Gap Size", "interpolate small gaps (left<->right, top<->bottom)", "7") MDEFVAR_OPTDENSE_int32(nIgnoreMaskLabel, "Ignore Mask Label", "label id used during ignore mask filter (<0 - disabled)", "-1") DEFVAR_OPTDENSE_uint32(nOptimize, "Optimize", "should we filter the extracted depth-maps?", "7") // see DepthFlags MDEFVAR_OPTDENSE_uint32(nEstimateColors, "Estimate Colors", "should we estimate the colors for the dense point-cloud?", "2", "0", "1") MDEFVAR_OPTDENSE_uint32(nEstimateNormals, "Estimate Normals", "should we estimate the normals for the dense point-cloud?", "0", "1", "2") MDEFVAR_OPTDENSE_float(fNCCThresholdKeep, "NCC Threshold Keep", "Maximum 1-NCC score accepted for a match", "0.9", "0.5") DEFVAR_OPTDENSE_uint32(nEstimationIters, "Estimation Iters", "Number of patch-match iterations", "3") DEFVAR_OPTDENSE_uint32(nEstimationGeometricIters, "Estimation Geometric Iters", "Number of geometric consistent patch-match iterations (0 - disabled)", "2") MDEFVAR_OPTDENSE_float(fEstimationGeometricWeight, "Estimation Geometric Weight", "pairwise geometric consistency cost weight", "0.1") MDEFVAR_OPTDENSE_uint32(nRandomIters, "Random Iters", "Number of iterations for random assignment per pixel", "6") MDEFVAR_OPTDENSE_uint32(nRandomMaxScale, "Random Max Scale", "Maximum number of iterations to skip during random assignment", "2") MDEFVAR_OPTDENSE_float(fRandomDepthRatio, "Random Depth Ratio", "Depth range ratio of the current estimate for random plane assignment", "0.003", "0.004") MDEFVAR_OPTDENSE_float(fRandomAngle1Range, "Random Angle1 Range", "Angle 1 range for random plane assignment (degrees)", "16.0", "20.0") MDEFVAR_OPTDENSE_float(fRandomAngle2Range, "Random Angle2 Range", "Angle 2 range for random plane assignment (degrees)", "10.0", "12.0") MDEFVAR_OPTDENSE_float(fRandomSmoothDepth, "Random Smooth Depth", "Depth variance used during neighbor smoothness assignment (ratio)", "0.02") MDEFVAR_OPTDENSE_float(fRandomSmoothNormal, "Random Smooth Normal", "Normal variance used during neighbor smoothness assignment (degrees)", "13") MDEFVAR_OPTDENSE_float(fRandomSmoothBonus, "Random Smooth Bonus", "Score factor used to encourage smoothness (1 - disabled)", "0.93") } // S T R U C T S /////////////////////////////////////////////////// //constructor from reference of DepthData DepthData::DepthData(const DepthData& srcDepthData) : images(srcDepthData.images), neighbors(srcDepthData.neighbors), points(srcDepthData.points), mask(srcDepthData.mask), depthMap(srcDepthData.depthMap), normalMap(srcDepthData.normalMap), confMap(srcDepthData.confMap), dMin(srcDepthData.dMin), dMax(srcDepthData.dMax), references(srcDepthData.references) {} // return normal in world-space for the given pixel // the 3D points can be precomputed and passed here void DepthData::GetNormal(const ImageRef& ir, Point3f& N, const TImage* pPointMap) const { ASSERT(!IsEmpty()); ASSERT(depthMap(ir) > 0); const Camera& camera = images.First().camera; if (!normalMap.empty()) { // set available normal N = camera.R.t()*Cast(normalMap(ir)); return; } // estimate normal based on the neighbor depths const int nPointsStep = 2; const int nPointsHalf = 2; const int nPoints = 2*nPointsHalf+1; const int nWindowHalf = nPointsHalf*nPointsStep; const int nWindow = 2*nWindowHalf+1; const Image8U::Size size(depthMap.size()); const ImageRef ptCorner(ir.x-nWindowHalf, ir.y-nWindowHalf); const ImageRef ptCornerRel(ptCorner.x>=0?0:-ptCorner.x, ptCorner.y>=0?0:-ptCorner.y); Point3Arr points(1, nPoints*nPoints); if (pPointMap) { points[0] = (*pPointMap)(ir); for (int j=ptCornerRel.y; j= size.height) break; for (int i=ptCornerRel.x; i= size.width) break; if (x==ir.x && y==ir.y) continue; if (depthMap(y,x) > 0) points.Insert((*pPointMap)(y,x)); } } } else { points[0] = camera.TransformPointI2C(Point3(ir.x,ir.y,depthMap(ir))); for (int j=ptCornerRel.y; j= size.height) break; for (int i=ptCornerRel.x; i= size.width) break; if (x==ir.x && y==ir.y) continue; const Depth d = depthMap(y,x); if (d > 0) points.Insert(camera.TransformPointI2C(Point3(x,y,d))); } } } if (points.GetSize() < 3) { N = normalized(-points[0]); return; } Plane plane; if (EstimatePlaneThLockFirstPoint(points, plane, 0, NULL, 20) < 3) { N = normalized(-points[0]); return; } ASSERT(ISEQUAL(plane.m_vN.norm(),REAL(1))); // normal is defined up to sign; pick the direction that points to the camera if (plane.m_vN.dot((const Point3::EVec)points[0]) > 0) plane.Negate(); N = camera.R.t()*Point3(plane.m_vN); } void DepthData::GetNormal(const Point2f& pt, Point3f& N, const TImage* pPointMap) const { const ImageRef ir(ROUND2INT(pt)); GetNormal(ir, N, pPointMap); } // GetNormal /*----------------------------------------------------------------*/ // apply mask to the depth map void DepthData::ApplyIgnoreMask(const BitMatrix& mask) { ASSERT(IsValid() && !IsEmpty() && mask.size() == depthMap.size()); for (int r=0; rname, IDs, depthMap.size(), image0.camera.K, image0.camera.R, image0.camera.C, dMin, dMax, depthMap, normalMap, confMap, viewsMap)) return false; } if (!File::renameFile(fileNameTmp, fileName)) { DEBUG_EXTRA("error: can not access dmap file '%s'", fileName.c_str()); File::deleteFile(fileNameTmp); return false; } return true; } bool DepthData::Load(const String& fileName, unsigned flags) { // serialize in the saved state String imageFileName; IIndexArr IDs; cv::Size imageSize; Camera camera; if (!ImportDepthDataRaw(fileName, imageFileName, IDs, imageSize, camera.K, camera.R, camera.C, dMin, dMax, depthMap, normalMap, confMap, viewsMap, flags)) return false; ASSERT(!IsValid() || (IDs.size() == images.size() && IDs.front() == GetView().GetID())); ASSERT(depthMap.size() == imageSize); return true; } /*----------------------------------------------------------------*/ unsigned DepthData::GetRef() { Lock l(cs); return references; } unsigned DepthData::IncRef(const String& fileName) { Lock l(cs); ASSERT(!IsEmpty() || references==0); if (IsEmpty() && !Load(fileName)) return 0; return ++references; } unsigned DepthData::DecRef() { Lock l(cs); ASSERT(references>0); if (--references == 0) Release(); return references; } /*----------------------------------------------------------------*/ // S T R U C T S /////////////////////////////////////////////////// // try to load and apply mask to the depth map; // the mask for each image is stored in the MVS scene or next to each image with '.mask.png' extension; // the mask marks as false (or 0) pixels that should be ignored // - pMask: optional output mask; if defined, the mask is returned in this image instead of the BitMatrix bool DepthEstimator::ImportIgnoreMask(const Image& image0, const Image8U::Size& size, uint16_t nIgnoreMaskLabel, BitMatrix& bmask, Image8U* pMask) { ASSERT(image0.IsValid() && !image0.image.empty()); const String maskFileName(image0.maskName.empty() ? Util::getFileFullName(image0.name)+".mask.png" : image0.maskName); // std::cout << "maskFileName: " << maskFileName << std::endl; Image16U mask; if (!mask.Load(maskFileName)) { DEBUG("warning: can not load the segmentation mask '%s'", maskFileName.c_str()); std::cout << "warning: can not load the segmentation mask " << maskFileName << std::endl; return false; } cv::resize(mask, mask, size, 0, 0, cv::INTER_NEAREST); if (pMask) { *pMask = (mask != nIgnoreMaskLabel); } else { bmask.create(size); bmask.memset(0xFF); for (int r=0; r 4 5 6 // 7 8 9 void DepthEstimator::MapMatrix2ZigzagIdx(const Image8U::Size& size, DepthEstimator::MapRefArr& coords, const BitMatrix& mask, int rawStride) { typedef DepthEstimator::MapRef MapRef; const int w = size.width; const int w1 = size.width-1; coords.Empty(); coords.Reserve(size.area()); for (int dy=0, h=rawStride; dy size.height - dy) h = size.height - dy; int lastX = 0; MapRef x(MapRef::ZERO); for (int i=0, ei=w*h; i0?SQUARE(OPTDENSE::fDescriptorMinMagnitudeThreshold):-1.f), angle1Range(FD2R(OPTDENSE::fRandomAngle1Range)), //default 0.279252678=FD2R(20) angle2Range(FD2R(OPTDENSE::fRandomAngle2Range)), //default 0.174532920=FD2R(16) thConfSmall(OPTDENSE::fNCCThresholdKeep * 0.66f), // default 0.6 thConfBig(OPTDENSE::fNCCThresholdKeep * 0.9f), // default 0.8 thConfRand(OPTDENSE::fNCCThresholdKeep * 1.1f), // default 0.99 thRobust(OPTDENSE::fNCCThresholdKeep * 4.f / 3.f) // default 1.2 #if DENSE_REFINE == DENSE_REFINE_EXACT , thPerturbation(1.f/POW(2.f,float(nIter+1))) #endif { ASSERT(_depthData0.images.size() >= 1); } // center a patch of given size on the segment bool DepthEstimator::PreparePixelPatch(const ImageRef& x) { x0 = x; return image0.image.isInside(ImageRef(x.x-nSizeHalfWindow, x.y-nSizeHalfWindow)) && image0.image.isInside(ImageRef(x.x+nSizeHalfWindow, x.y+nSizeHalfWindow)); } // fetch the patch pixel values in the main image bool DepthEstimator::FillPixelPatch() { #if DENSE_NCC != DENSE_NCC_WEIGHTED const float mean(GetImage0Sum(x0)/nTexels); normSq0 = 0; float* pTexel0 = texels0.data(); for (int i=-nSizeHalfWindow; i<=nSizeHalfWindow; i+=nSizeStep) for (int j=-nSizeHalfWindow; j<=nSizeHalfWindow; j+=nSizeStep) normSq0 += SQUARE(*pTexel0++ = image0.image(x0.y+i, x0.x+j)-mean); #else Weight& w = weightMap0[x0.y*image0.image.width()+x0.x]; if (w.normSq0 == 0) { w.sumWeights = 0; int n = 0; const float colCenter = image0.image(x0); for (int i=-nSizeHalfWindow; i<=nSizeHalfWindow; i+=nSizeStep) { for (int j=-nSizeHalfWindow; j<=nSizeHalfWindow; j+=nSizeStep) { Weight::Pixel& pw = w.weights[n++]; w.normSq0 += (pw.tempWeight = image0.image(x0.y+i, x0.x+j)) * (pw.weight = GetWeight(ImageRef(j,i), colCenter)); w.sumWeights += pw.weight; } } ASSERT(n == nTexels); const float tm(w.normSq0/w.sumWeights); w.normSq0 = 0; n = 0; do { Weight::Pixel& pw = w.weights[n]; const float t(pw.tempWeight - tm); w.normSq0 += (pw.tempWeight = pw.weight * t) * t; } while (++n < nTexels); } normSq0 = w.normSq0; #endif if (normSq0 < thMagnitudeSq && (lowResDepthMap.empty() || lowResDepthMap(x0) <= 0)) return false; X0 = image0.camera.TransformPointI2C(Cast(x0)); return true; } // compute pixel's NCC score in the given target image float DepthEstimator::ScorePixelImage(const DepthData::ViewData& image1, Depth depth, const Normal& normal) { // center a patch of given size on the segment and fetch the pixel values in the target image Matrix3x3f H(ComputeHomographyMatrix(image1, depth, normal)); Point3f X; ProjectVertex_3x3_2_3(H.val, Point2f(float(x0.x-nSizeHalfWindow),float(x0.y-nSizeHalfWindow)).ptr(), X.ptr()); Point3f baseX(X); H *= float(nSizeStep); int n(0); float sum(0); #if DENSE_NCC != DENSE_NCC_DEFAULT float sumSq(0), num(0); #endif #if DENSE_NCC == DENSE_NCC_WEIGHTED const Weight& w = weightMap0[x0.y*image0.image.width()+x0.x]; #endif for (int i=-nSizeHalfWindow; i<=nSizeHalfWindow; i+=nSizeStep) { for (int j=-nSizeHalfWindow; j<=nSizeHalfWindow; j+=nSizeStep) { const Point2f pt(X); if (!image1.image.isInsideWithBorder(pt)) return thRobust; const float v(image1.image.sample(pt)); #if DENSE_NCC == DENSE_NCC_FAST sum += v; sumSq += SQUARE(v); num += texels0(n++)*v; #elif DENSE_NCC == DENSE_NCC_WEIGHTED const Weight::Pixel& pw = w.weights[n++]; const float vw(v*pw.weight); sum += vw; sumSq += v*vw; num += v*pw.tempWeight; #else sum += texels1(n++)=v; #endif X.x += H[0]; X.y += H[3]; X.z += H[6]; } baseX.x += H[1]; baseX.y += H[4]; baseX.z += H[7]; X = baseX; } ASSERT(n == nTexels); // score similarity of the reference and target texture patches #if DENSE_NCC == DENSE_NCC_FAST const float normSq1(sumSq-SQUARE(sum/nSizeWindow)); #elif DENSE_NCC == DENSE_NCC_WEIGHTED const float normSq1(sumSq-SQUARE(sum)/w.sumWeights); #else const float normSq1(normSqDelta(texels1.data(), sum/(float)nTexels)); #endif const float nrmSq(normSq0*normSq1); if (nrmSq <=1e-16f) return thRobust; #if DENSE_NCC == DENSE_NCC_DEFAULT const float num(texels0.dot(texels1)); #endif const float ncc(CLAMP(num/SQRT(nrmSq), -1.f, 1.f)); float score(1.f-ncc); #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA // encourage smoothness for (const NeighborEstimate& neighbor: neighborsClose) { ASSERT(neighbor.depth > 0); #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE const float factorDepth(DENSE_EXP(SQUARE(plane.Distance(neighbor.X)/depth) * smoothSigmaDepth)); #else const float factorDepth(DENSE_EXP(SQUARE((depth-neighbor.depth)/depth) * smoothSigmaDepth)); #endif const float factorNormal(DENSE_EXP(SQUARE(ACOS(ComputeAngle(normal.ptr(), neighbor.normal.ptr()))) * smoothSigmaNormal)); score *= (1.f - smoothBonusDepth * factorDepth) * (1.f - smoothBonusNormal * factorNormal); } #endif if (!image1.depthMap.empty()) { ASSERT(OPTDENSE::fEstimationGeometricWeight > 0); float consistency(4.f); const Point3f X1(image1.Tl*Point3f(float(X0.x)*depth,float(X0.y)*depth,depth)+image1.Tm); // Kj * Rj * (Ri.t() * X + Ci - Cj) if (X1.z > 0) { const Point2f x1(X1); if (image1.depthMap.isInsideWithBorder(x1)) { Depth depth1; if (image1.depthMap.sample(depth1, x1, [&X1](Depth d) { return IsDepthSimilar(X1.z, d, 0.03f); })) { const Point2f xb(image1.Tr*Point3f(x1.x*depth1,x1.y*depth1,depth1)+image1.Tn); // Ki * Ri * (Rj.t() * Kj-1 * X + Cj - Ci) const float dist(norm(Point2f(float(x0.x)-xb.x, float(x0.y)-xb.y))); consistency = MINF(SQRT(dist*(dist+2.f)), consistency); } } } score += OPTDENSE::fEstimationGeometricWeight * consistency; } // apply depth prior weight based on patch textureless if (!lowResDepthMap.empty()) { const Depth d0 = lowResDepthMap(x0); if (d0 > 0) { const float deltaDepth(MINF(DepthSimilarity(d0, depth), 0.5f)); const float smoothSigmaDepth(-1.f / (1.f * 0.02f)); // 0.12: patch texture variance below 0.02 (0.12^2) is considered texture-less const float factorDeltaDepth(DENSE_EXP(normSq0 * smoothSigmaDepth)); score = (1.f-factorDeltaDepth)*score + factorDeltaDepth*deltaDepth; } } ASSERT(ISFINITE(score)); return MIN(2.f, score); } // compute pixel's NCC score float DepthEstimator::ScorePixel(Depth depth, const Normal& normal) { ASSERT(depth > 0 && normal.dot(Cast(X0)) <= 0); // compute score for this pixel as seen in each view ASSERT(scores.size() == images.size()); FOREACH(idxView, images) scores[idxView] = ScorePixelImage(images[idxView], depth, normal); #if DENSE_AGGNCC == DENSE_AGGNCC_NTH // set score as the nth element return scores.GetNth(idxScore); #elif DENSE_AGGNCC == DENSE_AGGNCC_MEAN // set score as the average similarity #if 1 return scores.mean(); #else const float* pscore(scores.data()); const float* pescore(pscore+scores.rows()); float score(0); do { score += MINF(*pscore, thRobust); } while (++pscore <= pescore); return score/scores.rows(); #endif #elif DENSE_AGGNCC == DENSE_AGGNCC_MIN // set score as the min similarity return scores.minCoeff(); #else // set score as the min-mean similarity if (idxScore == 0) return *std::min_element(scores.cbegin(), scores.cend()); #if 0 return std::accumulate(scores.cbegin(), &scores.GetNth(idxScore), 0.f) / idxScore; #elif 1 const float* pescore(&scores.GetNth(idxScore)); const float* pscore(scores.cbegin()); int n(1); float score(*pscore); do { const float s(*(++pscore)); if (s >= thRobust) break; score += s; ++n; } while (pscore < pescore); return score/n; #else const float thScore(MAXF(*std::min_element(scores.cbegin(), scores.cend()), 0.05f)*2); const float* pscore(scores.cbegin()); const float* pescore(pscore+scores.size()); int n(0); float score(0); do { const float s(*pscore); if (s <= thScore) { score += s; ++n; } } while (++pscore <= pescore); return score/n; #endif #endif } // run propagation and random refinement cycles; // the solution belonging to the target image can be also propagated void DepthEstimator::ProcessPixel(IDX idx) { // compute pixel coordinates from pixel index and its neighbors ASSERT(dir == LT2RB || dir == RB2LT); if (!PreparePixelPatch(dir == LT2RB ? coords[idx] : coords[coords.GetSize()-1-idx]) || !FillPixelPatch()) return; // find neighbors neighbors.Empty(); #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA neighborsClose.Empty(); #endif if (dir == LT2RB) { // direction from left-top to right-bottom corner if (x0.x > nSizeHalfWindow) { const ImageRef nx(x0.x-1, x0.y); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighbors.emplace_back(nx); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); #else neighbors.emplace_back(NeighborData{nx,ndepth,normalMap0(nx)}); #endif } } if (x0.y > nSizeHalfWindow) { const ImageRef nx(x0.x, x0.y-1); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighbors.emplace_back(nx); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); #else neighbors.emplace_back(NeighborData{nx,ndepth,normalMap0(nx)}); #endif } } #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA if (x0.x < size.width-nSizeHalfWindow) { const ImageRef nx(x0.x+1, x0.y); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); } } if (x0.y < size.height-nSizeHalfWindow) { const ImageRef nx(x0.x, x0.y+1); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); } } #endif } else { ASSERT(dir == RB2LT); // direction from right-bottom to left-top corner if (x0.x < size.width-nSizeHalfWindow) { const ImageRef nx(x0.x+1, x0.y); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighbors.emplace_back(nx); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); #else neighbors.emplace_back(NeighborData{nx,ndepth,normalMap0(nx)}); #endif } } if (x0.y < size.height-nSizeHalfWindow) { const ImageRef nx(x0.x, x0.y+1); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighbors.emplace_back(nx); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); #else neighbors.emplace_back(NeighborData{nx,ndepth,normalMap0(nx)}); #endif } } #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA if (x0.x > nSizeHalfWindow) { const ImageRef nx(x0.x-1, x0.y); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); } } if (x0.y > nSizeHalfWindow) { const ImageRef nx(x0.x, x0.y-1); const Depth ndepth(depthMap0(nx)); if (ndepth > 0) { ASSERT(ISEQUAL(norm(normalMap0(nx)), 1.f)); neighborsClose.emplace_back(NeighborEstimate{ndepth,normalMap0(nx) #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE , Cast(image0.camera.TransformPointI2C(Point3(nx, ndepth))) #endif }); } } #endif } float& conf = confMap0(x0); Depth& depth = depthMap0(x0); Normal& normal = normalMap0(x0); const Normal viewDir(Cast(X0)); ASSERT(depth > 0 && normal.dot(viewDir) <= 0); #if DENSE_REFINE == DENSE_REFINE_ITER // check if any of the neighbor estimates are better then the current estimate #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA FOREACH(n, neighbors) { const ImageRef& nx = neighbors[n]; #else for (NeighborData& neighbor: neighbors) { const ImageRef& nx = neighbor.x; #endif if (confMap0(nx) >= OPTDENSE::fNCCThresholdKeep) continue; #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA NeighborEstimate neighbor = neighborsClose[n]; #endif neighbor.depth = InterpolatePixel(nx, neighbor.depth, neighbor.normal); CorrectNormal(neighbor.normal); ASSERT(neighbor.depth > 0 && neighbor.normal.dot(viewDir) <= 0); #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE InitPlane(neighbor.depth, neighbor.normal); #endif const float nconf(ScorePixel(neighbor.depth, neighbor.normal)); ASSERT(nconf >= 0 && nconf <= 2); if (conf > nconf) { conf = nconf; depth = neighbor.depth; normal = neighbor.normal; } } // try random values around the current estimate in order to refine it unsigned idxScaleRange(0); RefineIters: if (conf <= thConfSmall) idxScaleRange = 2; else if (conf <= thConfBig) idxScaleRange = 1; else if (conf >= thConfRand) { // try completely random values in order to find an initial estimate #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA neighborsClose.Empty(); #endif for (unsigned iter=0; iter= 0); if (conf > nconf) { conf = nconf; depth = ndepth; normal = nnormal; if (conf < thConfRand) goto RefineIters; } } return; } float scaleRange(scaleRanges[idxScaleRange]); const float depthRange(MaxDepthDifference(depth, OPTDENSE::fRandomDepthRatio)); Point2f p; Normal2Dir(normal, p); Normal nnormal; for (unsigned iter=0; iter= 0) continue; #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE InitPlane(ndepth, nnormal); #endif const float nconf(ScorePixel(ndepth, nnormal)); ASSERT(nconf >= 0); if (conf > nconf) { conf = nconf; depth = ndepth; normal = nnormal; p = np; scaleRange = scaleRanges[++idxScaleRange]; } } #else // current pixel estimate PixelEstimate currEstimate{depth, normal}; // propagate depth estimate from the best neighbor estimate PixelEstimate prevEstimate; float prevCost(FLT_MAX); #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA FOREACH(n, neighbors) { const ImageRef& nx = neighbors[n]; #else for (const NeighborData& neighbor: neighbors) { const ImageRef& nx = neighbor.x; #endif float nconf(confMap0(nx)); const unsigned nidxScaleRange(DecodeScoreScale(nconf)); ASSERT(nconf >= 0 && nconf <= 2); if (nconf >= OPTDENSE::fNCCThresholdKeep) continue; if (prevCost <= nconf) continue; #if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA const NeighborEstimate& neighbor = neighborsClose[n]; #endif if (neighbor.normal.dot(viewDir) >= 0) continue; prevEstimate.depth = InterpolatePixel(nx, neighbor.depth, neighbor.normal); prevEstimate.normal = neighbor.normal; CorrectNormal(prevEstimate.normal); prevCost = nconf; } if (prevCost == FLT_MAX) prevEstimate = PerturbEstimate(currEstimate, thPerturbation); // randomly sampled estimate PixelEstimate randEstimate(PerturbEstimate(currEstimate, thPerturbation)); // select best pixel estimate const int numCosts = 5; float costs[numCosts] = {0,0,0,0,0}; const Depth depths[numCosts] = { currEstimate.depth, prevEstimate.depth, randEstimate.depth, currEstimate.depth, randEstimate.depth}; const Normal normals[numCosts] = { currEstimate.normal, prevEstimate.normal, randEstimate.normal, randEstimate.normal, currEstimate.normal}; conf = FLT_MAX; for (int idxCost=0; idxCost= 0); if (conf > nconf) { conf = nconf; depth = ndepth; normal = nnormal; } } #endif } // interpolate given pixel's estimate to the current position Depth DepthEstimator::InterpolatePixel(const ImageRef& nx, Depth depth, const Normal& normal) const { ASSERT(depth > 0 && normal.dot(image0.camera.TransformPointI2C(Cast(nx))) <= 0); Depth depthNew; #if 1 // compute as intersection of the lines // {(x1, y1), (x2, y2)} from neighbor's 3D point towards normal direction // and // {(0, 0), (x4, 1)} from camera center towards current pixel direction // in the x or y plane if (x0.x == nx.x) { const float nx1((float)(((REAL)x0.y - image0.camera.K(1,2)) / image0.camera.K(1,1))); const float denom(normal.z + nx1 * normal.y); if (ISZERO(denom)) return depth; const float x1((float)(((REAL)nx.y - image0.camera.K(1,2)) / image0.camera.K(1,1))); const float nom(depth * (normal.z + x1 * normal.y)); depthNew = nom / denom; } else { ASSERT(x0.y == nx.y); const float nx1((float)(((REAL)x0.x - image0.camera.K(0,2)) / image0.camera.K(0,0))); const float denom(normal.z + nx1 * normal.x); if (ISZERO(denom)) return depth; const float x1((float)(((REAL)nx.x - image0.camera.K(0,2)) / image0.camera.K(0,0))); const float nom(depth * (normal.z + x1 * normal.x)); depthNew = nom / denom; } #else // compute as the ray - plane intersection { #if 0 const Plane plane(Cast(normal), image0.camera.TransformPointI2C(Point3(nx, depth))); const Ray3 ray(Point3::ZERO, normalized(X0)); depthNew = (Depth)ray.Intersects(plane).z(); #else const Point3 planeN(normal); const REAL planeD(planeN.dot(image0.camera.TransformPointI2C(Point3(nx, depth)))); depthNew = (Depth)(planeD / planeN.dot(X0)); #endif } #endif return ISINSIDE(depthNew,dMin,dMax) ? depthNew : depth; } #if DENSE_SMOOTHNESS == DENSE_SMOOTHNESS_PLANE // compute plane defined by current depth and normal estimate void DepthEstimator::InitPlane(Depth depth, const Normal& normal) { #if 0 plane.Set(normal, Normal(Cast(X0)*depth)); #else plane.m_vN = normal; plane.m_fD = -depth*normal.dot(Cast(X0)); #endif } #endif #if DENSE_REFINE == DENSE_REFINE_EXACT DepthEstimator::PixelEstimate DepthEstimator::PerturbEstimate(const PixelEstimate& est, float perturbation) { PixelEstimate ptbEst; // perturb depth const float minDepth = est.depth * (1.f-perturbation); const float maxDepth = est.depth * (1.f+perturbation); ptbEst.depth = CLAMP(rnd.randomUniform(minDepth, maxDepth), dMin, dMax); // perturb normal const Normal viewDir(Cast(X0)); std::uniform_real_distribution urd(-1.f, 1.f); const int numMaxTrials = 3; int numTrials = 0; perturbation *= FHALF_PI; while(true) { // generate random perturbation rotation const RMatrixBaseF R(urd(rnd)*perturbation, urd(rnd)*perturbation, urd(rnd)*perturbation); // perturb normal vector ptbEst.normal = R * est.normal; // make sure the perturbed normal is still looking towards the camera, // otherwise try again with a smaller perturbation if (ptbEst.normal.dot(viewDir) < 0.f) break; if (++numTrials == numMaxTrials) { ptbEst.normal = est.normal; return ptbEst; } perturbation *= 0.5f; } ASSERT(ISEQUAL(norm(ptbEst.normal), 1.f)); return ptbEst; } #endif /*----------------------------------------------------------------*/ // S T R U C T S /////////////////////////////////////////////////// namespace CGAL { } // triangulate in-view points, generating a 2D mesh // return also the estimated depth boundaries (min and max depth) std::pair TriangulatePointsDelaunay(const DepthData::ViewData& image, const PointCloud& pointcloud, const IndexArr& points, Mesh& mesh, Point2fArr& projs, bool bAddCorners) { typedef CGAL::Simple_cartesian kernel_t; typedef CGAL::Triangulation_vertex_base_with_info_2 vertex_base_t; typedef CGAL::Triangulation_data_structure_2 triangulation_data_structure_t; typedef CGAL::Delaunay_triangulation_2 Delaunay; typedef Delaunay::Face_circulator FaceCirculator; typedef Delaunay::Face_handle FaceHandle; typedef Delaunay::Vertex_handle VertexHandle; typedef kernel_t::Point_2 CPoint; ASSERT(sizeof(Point3) == sizeof(X3D)); ASSERT(sizeof(Point2) == sizeof(CPoint)); std::pair depthBounds(FLT_MAX, 0.f); mesh.vertices.reserve((Mesh::VIndex)points.size()+4); projs.reserve(mesh.vertices.capacity()); Delaunay delaunay; for (uint32_t idx: points) { const Point3f pt(image.camera.ProjectPointP3(pointcloud.points[idx])); const Point3f x(pt.x/pt.z, pt.y/pt.z, pt.z); delaunay.insert(CPoint(x.x, x.y))->info() = mesh.vertices.size(); mesh.vertices.emplace_back(image.camera.TransformPointI2C(x)); projs.emplace_back(x.x, x.y); if (depthBounds.first > pt.z) depthBounds.first = pt.z; if (depthBounds.second < pt.z) depthBounds.second = pt.z; } // if full size depth-map requested const size_t numPoints(3); if (bAddCorners && points.size() >= numPoints) { // add the four image corners at the average depth ASSERT(image.pImageData->IsValid() && ISINSIDE(image.pImageData->avgDepth, depthBounds.first, depthBounds.second)); const Mesh::VIndex idxFirstVertex = mesh.vertices.size(); VertexHandle vcorners[4]; for (const Point2f x: {Point2i(0, 0), Point2i(image.image.width()-1, 0), Point2i(0, image.image.height()-1), Point2i(image.image.width()-1, image.image.height()-1)}) { const Mesh::VIndex i(mesh.vertices.size() - idxFirstVertex); (vcorners[i] = delaunay.insert(CPoint(x.x, x.y)))->info() = mesh.vertices.size(); mesh.vertices.emplace_back(image.camera.TransformPointI2C(Point3f(x, image.pImageData->avgDepth))); projs.emplace_back(x); } // compute average depth from the closest 3 directly connected faces, // weighted by the distance for (int i=0; i<4; ++i) { const VertexHandle vcorner(vcorners[i]); FaceCirculator cfc(delaunay.incident_faces(vcorner)); ASSERT(cfc != 0); const FaceCirculator done(cfc); const Point2d& posA = reinterpret_cast(vcorner->point()); const Ray3d rayA(Point3d::ZERO, normalized(Cast(mesh.vertices[vcorner->info()]))); typedef TIndexScore DepthDist; CLISTDEF0(DepthDist) depths(0, numPoints); do { const FaceHandle fc(cfc->neighbor(cfc->index(vcorner))); if (delaunay.is_infinite(fc)) continue; for (int j=0; j<4; ++j) if (fc->has_vertex(vcorners[j])) continue; // compute the depth as the intersection of the corner ray with // the plane defined by the face's vertices const Planed planeB( Cast(mesh.vertices[fc->vertex(0)->info()]), Cast(mesh.vertices[fc->vertex(1)->info()]), Cast(mesh.vertices[fc->vertex(2)->info()]) ); const Point3d poszB(rayA.Intersects(planeB)); if (poszB.z <= 0) continue; const Point2d posB(( reinterpret_cast(fc->vertex(0)->point())+ reinterpret_cast(fc->vertex(1)->point())+ reinterpret_cast(fc->vertex(2)->point()))/3.f ); const double dist(norm(posB-posA)); depths.StoreTop(DepthDist(CLAMP((float)poszB.z,depthBounds.first,depthBounds.second), INVERT((float)dist))); } while (++cfc != done); ASSERT(depths.size() == numPoints); typedef Eigen::Map< Eigen::VectorXf, Eigen::Unaligned, Eigen::InnerStride<2> > FloatMap; FloatMap vecDists(&depths[0].score, numPoints); vecDists *= 1.f/vecDists.sum(); FloatMap vecDepths(&depths[0].idx, numPoints); const float depth(vecDepths.dot(vecDists)); mesh.vertices[idxFirstVertex+i] = image.camera.TransformPointI2C(Point3(posA, depth)); } } mesh.faces.reserve(Mesh::FIndex(std::distance(delaunay.finite_faces_begin(),delaunay.finite_faces_end()))); for (Delaunay::Face_iterator it=delaunay.faces_begin(); it!=delaunay.faces_end(); ++it) { const Delaunay::Face& face = *it; mesh.faces.emplace_back(face.vertex(2)->info(), face.vertex(1)->info(), face.vertex(0)->info()); } return depthBounds; } // roughly estimate depth and normal maps by triangulating the sparse point cloud // and interpolating normal and depth for all pixels bool MVS::TriangulatePoints2DepthMap( const DepthData::ViewData& image, const PointCloud& pointcloud, const IndexArr& points, DepthMap& depthMap, NormalMap& normalMap, Depth& dMin, Depth& dMax, bool bAddCorners, bool bSparseOnly) { ASSERT(image.pImageData != NULL); // triangulate in-view points Mesh mesh; Point2fArr projs; const std::pair thDepth(TriangulatePointsDelaunay(image, pointcloud, points, mesh, projs, bAddCorners)); dMin = thDepth.first; dMax = thDepth.second; // create rough depth-map by interpolating inside triangles const Camera& camera = image.camera; mesh.ComputeNormalVertices(); depthMap.create(image.image.size()); normalMap.create(image.image.size()); if (!bAddCorners || bSparseOnly) { depthMap.memset(0); normalMap.memset(0); } if (bSparseOnly) { // just project sparse pointcloud onto depthmap FOREACH(i, mesh.vertices) { const Point2f& x(projs[i]); const Point2i ix(FLOOR2INT(x)); const Depth z(mesh.vertices[i].z); const Normal& normal(mesh.vertexNormals[i]); for (const Point2i dx : {Point2i(0,0),Point2i(1,0),Point2i(0,1),Point2i(1,1)}) { const Point2i ax(ix + dx); if (!depthMap.isInside(ax)) continue; depthMap(ax) = z; normalMap(ax) = normal; } } } else { // rasterize triangles onto depthmap struct RasterDepth : TRasterMeshBase { typedef TRasterMeshBase Base; using Base::Triangle; using Base::camera; using Base::depthMap; const Mesh::NormalArr& vertexNormals; NormalMap& normalMap; Mesh::Face face; RasterDepth(const Mesh::NormalArr& _vertexNormals, const Camera& _camera, DepthMap& _depthMap, NormalMap& _normalMap) : Base(_camera, _depthMap), vertexNormals(_vertexNormals), normalMap(_normalMap) {} inline void Raster(const ImageRef& pt, const Triangle& t, const Point3f& bary) { const Point3f pbary(PerspectiveCorrectBarycentricCoordinates(t, bary)); const Depth z(ComputeDepth(t, pbary)); ASSERT(z > Depth(0)); depthMap(pt) = z; normalMap(pt) = normalized( vertexNormals[face[0]] * pbary[0]+ vertexNormals[face[1]] * pbary[1]+ vertexNormals[face[2]] * pbary[2] ); } }; RasterDepth rasterer {mesh.vertexNormals, camera, depthMap, normalMap}; RasterDepth::Triangle triangle; RasterDepth::TriangleRasterizer triangleRasterizer(triangle, rasterer); for (const Mesh::Face& face : mesh.faces) { rasterer.face = face; triangle.ptc[0].z = mesh.vertices[face[0]].z; triangle.ptc[1].z = mesh.vertices[face[1]].z; triangle.ptc[2].z = mesh.vertices[face[2]].z; Image8U::RasterizeTriangleBary( projs[face[0]], projs[face[1]], projs[face[2]], triangleRasterizer); } } return true; } // TriangulatePoints2DepthMap // same as above, but does not estimate the normal-map bool MVS::TriangulatePoints2DepthMap( const DepthData::ViewData& image, const PointCloud& pointcloud, const IndexArr& points, DepthMap& depthMap, Depth& dMin, Depth& dMax, bool bAddCorners, bool bSparseOnly) { ASSERT(image.pImageData != NULL); // triangulate in-view points Mesh mesh; Point2fArr projs; const std::pair thDepth(TriangulatePointsDelaunay(image, pointcloud, points, mesh, projs, bAddCorners)); dMin = thDepth.first; dMax = thDepth.second; // create rough depth-map by interpolating inside triangles const Camera& camera = image.camera; depthMap.create(image.image.size()); if (!bAddCorners || bSparseOnly) depthMap.memset(0); if (bSparseOnly) { // just project sparse pointcloud onto depthmap FOREACH(i, mesh.vertices) { const Point2f& x(projs[i]); const Point2i ix(FLOOR2INT(x)); const Depth z(mesh.vertices[i].z); for (const Point2i dx : {Point2i(0,0),Point2i(1,0),Point2i(0,1),Point2i(1,1)}) { const Point2i ax(ix + dx); if (!depthMap.isInside(ax)) continue; depthMap(ax) = z; } } } else { // rasterize triangles onto depthmap struct RasterDepth : TRasterMeshBase { typedef TRasterMeshBase Base; using Base::depthMap; RasterDepth(const Camera& _camera, DepthMap& _depthMap) : Base(_camera, _depthMap) {} inline void Raster(const ImageRef& pt, const Triangle& t, const Point3f& bary) { const Point3f pbary(PerspectiveCorrectBarycentricCoordinates(t, bary)); const Depth z(ComputeDepth(t, pbary)); ASSERT(z > Depth(0)); depthMap(pt) = z; } }; RasterDepth rasterer {camera, depthMap}; RasterDepth::Triangle triangle; RasterDepth::TriangleRasterizer triangleRasterizer(triangle, rasterer); for (const Mesh::Face& face : mesh.faces) { triangle.ptc[0].z = mesh.vertices[face[0]].z; triangle.ptc[1].z = mesh.vertices[face[1]].z; triangle.ptc[2].z = mesh.vertices[face[2]].z; Image8U::RasterizeTriangleBary( projs[face[0]], projs[face[1]], projs[face[2]], triangleRasterizer); } } return true; } // TriangulatePoints2DepthMap /*----------------------------------------------------------------*/ namespace MVS { template class TPlaneSolverAdaptor { public: enum { MINIMUM_SAMPLES = 3 }; enum { MAX_MODELS = 1 }; typedef TYPE Type; typedef TPoint3 Point; typedef CLISTDEF0(Point) Points; typedef TPlane Model; typedef CLISTDEF0(Model) Models; TPlaneSolverAdaptor(const Points& points) : points_(points) { } TPlaneSolverAdaptor(const Points& points, float w, float h, float d) : points_(points) { // LogAlpha0 is used to make error data scale invariant // Ratio of containing diagonal image rectangle over image area const float D = SQRT(w*w + h*h + d*d); // diameter const float A = w*h*d+1.f; // volume logalpha0_ = LOG10(2.f*D/A*0.5f); } inline bool Fit(const std::vector& samples, Models& models) const { ASSERT(samples.size() == MINIMUM_SAMPLES); Point points[MINIMUM_SAMPLES]; for (size_t i=0; i(points_.size()); } inline double logalpha0() const { return logalpha0_; } inline double multError() const { return 0.5; } protected: const Points& points_; // Normalized input data double logalpha0_; // Alpha0 is used to make the error adaptive to the image size Model model2evaluate; // current model to be evaluated }; // Robustly estimate the plane that fits best the given points template unsigned TEstimatePlane(const CLISTDEF0(TPoint3)& points, TPlane& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) { typedef TPlaneSolverAdaptor PlaneSolverAdaptor; plane.Invalidate(); const unsigned nPoints = (unsigned)points.size(); if (nPoints < PlaneSolverAdaptor::MINIMUM_SAMPLES) { ASSERT("too few points" == NULL); return 0; } // normalize points TMatrix H; typename PlaneSolverAdaptor::Points normPoints; NormalizePoints(points, normPoints, &H); // plane robust estimation std::vector vec_inliers; Sampler sampler; if (bFixThreshold) { if (maxThreshold == 0) maxThreshold = 0.35/H(0,0); PlaneSolverAdaptor kernel(normPoints); RANSAC(kernel, sampler, vec_inliers, plane, maxThreshold*H(0,0), 0.99, maxIters); DEBUG_LEVEL(3, "Robust plane: %u/%u points", vec_inliers.size(), nPoints); } else { if (maxThreshold != DBL_MAX) maxThreshold *= H(0,0); PlaneSolverAdaptor kernel(normPoints, 1, 1, 1); const std::pair ACRansacOut(ACRANSAC(kernel, sampler, vec_inliers, plane, maxThreshold, 0.99, maxIters)); const double& thresholdSq = ACRansacOut.first; maxThreshold = SQRT(thresholdSq)/H(0,0); DEBUG_LEVEL(3, "Auto-robust plane: %u/%u points (%g threshold)", vec_inliers.size(), nPoints, maxThreshold); } unsigned inliers_count = (unsigned)vec_inliers.size(); if (inliers_count < PlaneSolverAdaptor::MINIMUM_SAMPLES) return 0; // fit plane to all the inliers FitPlaneOnline fitPlane; for (unsigned i=0; i(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlane // Robustly estimate the plane that fits best the given points, making sure the first point is part of the solution (if any) unsigned MVS::EstimatePlaneLockFirstPoint(const Point3dArr& points, Planed& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneLockFirstPoint // Robustly estimate the plane that fits best the given points using a known threshold unsigned MVS::EstimatePlaneTh(const Point3dArr& points, Planed& plane, double maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneTh // Robustly estimate the plane that fits best the given points using a known threshold, making sure the first point is part of the solution (if any) unsigned MVS::EstimatePlaneThLockFirstPoint(const Point3dArr& points, Planed& plane, double maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneThLockFirstPoint /*----------------------------------------------------------------*/ // Robustly estimate the plane that fits best the given points unsigned MVS::EstimatePlane(const Point3fArr& points, Planef& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlane // Robustly estimate the plane that fits best the given points, making sure the first point is part of the solution (if any) unsigned MVS::EstimatePlaneLockFirstPoint(const Point3fArr& points, Planef& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneLockFirstPoint // Robustly estimate the plane that fits best the given points using a known threshold unsigned MVS::EstimatePlaneTh(const Point3fArr& points, Planef& plane, double maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneTh // Robustly estimate the plane that fits best the given points using a known threshold, making sure the first point is part of the solution (if any) unsigned MVS::EstimatePlaneThLockFirstPoint(const Point3fArr& points, Planef& plane, double maxThreshold, bool arrInliers[], size_t maxIters) { return TEstimatePlane(points, plane, maxThreshold, arrInliers, maxIters); } // EstimatePlaneThLockFirstPoint /*----------------------------------------------------------------*/ // estimate the colors of the given dense point cloud void MVS::EstimatePointColors(const ImageArr& images, PointCloud& pointcloud) { TD_TIMER_START(); pointcloud.colors.Resize(pointcloud.points.GetSize()); FOREACH(i, pointcloud.colors) { PointCloud::Color& color = pointcloud.colors[i]; const PointCloud::Point& point = pointcloud.points[i]; const PointCloud::ViewArr& views= pointcloud.pointViews[i]; // compute vertex color REAL bestDistance(FLT_MAX); const Image* pImageData(NULL); FOREACHPTR(pView, views) { const Image& imageData = images[*pView]; ASSERT(imageData.IsValid()); if (imageData.image.empty()) continue; // compute the distance from the 3D point to the image const REAL distance(imageData.camera.PointDepth(point)); ASSERT(distance > 0); if (bestDistance > distance) { bestDistance = distance; pImageData = &imageData; } } if (pImageData == NULL) { // set a dummy color color = Pixel8U::WHITE; } else { // get image color const Point2f proj(pImageData->camera.ProjectPointP(point)); color = (pImageData->image.isInsideWithBorder(proj) ? pImageData->image.sample(proj) : Pixel8U::WHITE); } } DEBUG_ULTIMATE("Estimate dense point cloud colors: %u colors (%s)", pointcloud.colors.GetSize(), TD_TIMER_GET_FMT().c_str()); } // EstimatePointColors /*----------------------------------------------------------------*/ // estimates the normals through PCA over the K nearest neighbors void MVS::EstimatePointNormals(const ImageArr& images, PointCloud& pointcloud, int numNeighbors /*K-nearest neighbors*/) { TD_TIMER_START(); ASSERT(pointcloud.IsValid()); typedef CGAL::Simple_cartesian kernel_t; typedef kernel_t::Point_3 point_t; typedef kernel_t::Vector_3 vector_t; typedef std::pair PointVectorPair; // fetch the point set std::vector pointvectors(pointcloud.points.GetSize()); FOREACH(i, pointcloud.points) reinterpret_cast(pointvectors[i].first) = pointcloud.points[i]; // estimates normals direction; // Note: pca_estimate_normals() requires an iterator over points // as well as property maps to access each point's position and normal. #if CGAL_VERSION_NR < 1041301000 #if CGAL_VERSION_NR < 1040800000 CGAL::pca_estimate_normals( #else CGAL::pca_estimate_normals( #endif pointvectors.begin(), pointvectors.end(), CGAL::First_of_pair_property_map(), CGAL::Second_of_pair_property_map(), numNeighbors ); #else CGAL::pca_estimate_normals( pointvectors, numNeighbors, CGAL::parameters::point_map(CGAL::First_of_pair_property_map()) .normal_map(CGAL::Second_of_pair_property_map()) ); #endif // store the point normals pointcloud.normals.Resize(pointcloud.points.GetSize()); FOREACH(i, pointcloud.normals) { PointCloud::Normal& normal = pointcloud.normals[i]; const PointCloud::Point& point = pointcloud.points[i]; const PointCloud::ViewArr& views= pointcloud.pointViews[i]; normal = reinterpret_cast(pointvectors[i].second); // correct normal orientation ASSERT(!views.IsEmpty()); const Image& imageData = images[views.First()]; if (normal.dot(Cast(imageData.camera.C)-point) < 0) normal = -normal; } DEBUG_ULTIMATE("Estimate dense point cloud normals: %u normals (%s)", pointcloud.normals.GetSize(), TD_TIMER_GET_FMT().c_str()); } // EstimatePointNormals /*----------------------------------------------------------------*/ bool MVS::EstimateNormalMap(const Matrix3x3f& K, const DepthMap& depthMap, NormalMap& normalMap) { normalMap.create(depthMap.size()); struct Tool { static bool IsDepthValid(Depth d, Depth nd) { return nd > 0 && IsDepthSimilar(d, nd, Depth(0.03f)); } // computes depth gradient (first derivative) at current pixel static bool DepthGradient(const DepthMap& depthMap, const ImageRef& ir, Point3f& ws) { float& w = ws[0]; float& wx = ws[1]; float& wy = ws[2]; w = depthMap(ir); if (w <= 0) return false; // loop over neighborhood and finding least squares plane, // the coefficients of which give gradient of depth int whxx(0), whxy(0), whyy(0); float wgx(0), wgy(0); const int Radius(1); int n(0); for (int y = -Radius; y <= Radius; ++y) { for (int x = -Radius; x <= Radius; ++x) { if (x == 0 && y == 0) continue; const ImageRef pt(ir.x+x, ir.y+y); if (!depthMap.isInside(pt)) continue; const float wi(depthMap(pt)); if (!IsDepthValid(w, wi)) continue; whxx += x*x; whxy += x*y; whyy += y*y; wgx += (wi - w)*x; wgy += (wi - w)*y; ++n; } } if (n < 3) return false; // solve 2x2 system, generated from depth gradient const int det(whxx*whyy - whxy*whxy); if (det == 0) return false; const float invDet(1.f/float(det)); wx = (float( whyy)*wgx - float(whxy)*wgy)*invDet; wy = (float(-whxy)*wgx + float(whxx)*wgy)*invDet; return true; } // computes normal to the surface given the depth and its gradient static Normal ComputeNormal(const Matrix3x3f& K, int x, int y, Depth d, Depth dx, Depth dy) { ASSERT(ISZERO(K(0,1))); return normalized(Normal( K(0,0)*dx, K(1,1)*dy, (K(0,2)-float(x))*dx+(K(1,2)-float(y))*dy-d )); } }; for (int r=0; r depths(0, depthMap.area()); for (int i=depthMap.area(); --i >= 0; ) { const Depth depth = depthMap[i]; ASSERT(depth == 0 || depth > 0); if (depth > 0) depths.Insert(depth); } if (!depths.empty()) { const std::pair th(ComputeX84Threshold(depths.data(), depths.size())); const std::pair mm(depths.GetMinMax()); maxDepth = MINF(th.first+th.second, mm.second); minDepth = MAXF(th.first-th.second, mm.first); } DEBUG_ULTIMATE("\tdepth range: [%g, %g]", minDepth, maxDepth); } const Depth sclDepth(Depth(1)/(maxDepth - minDepth)); // create color image Image8U3 img(depthMap.size()); for (int i=depthMap.area(); --i >= 0; ) { const Depth depth = depthMap[i]; img[i] = (depth > 0 ? Pixel8U::gray2color(CLAMP((maxDepth-depth)*sclDepth, Depth(0), Depth(1))) : Pixel8U::BLACK); } return img; } // DepthMap2Image bool MVS::ExportDepthMap(const String& fileName, const DepthMap& depthMap, Depth minDepth, Depth maxDepth) { if (depthMap.empty()) return false; return DepthMap2Image(depthMap, minDepth, maxDepth).Save(fileName); } // ExportDepthMap /*----------------------------------------------------------------*/ // export normal map as an image bool MVS::ExportNormalMap(const String& fileName, const NormalMap& normalMap) { if (normalMap.empty()) return false; Image8U3 img(normalMap.size()); for (int i=normalMap.area(); --i >= 0; ) { img[i] = [](const Normal& n) { return ISZERO(n) ? Image8U3::Type::BLACK : Image8U3::Type( CLAMP(ROUND2INT((1.f-n.x)*127.5f), 0, 255), CLAMP(ROUND2INT((1.f-n.y)*127.5f), 0, 255), CLAMP(ROUND2INT( -n.z *255.0f), 0, 255) ); } (normalMap[i]); } return img.Save(fileName); } // ExportNormalMap /*----------------------------------------------------------------*/ // export confidence map as an image (dark - low confidence, light - high confidence) bool MVS::ExportConfidenceMap(const String& fileName, const ConfidenceMap& confMap) { // find min and max values FloatArr confs(0, confMap.area()); for (int i=confMap.area(); --i >= 0; ) { const float conf = confMap[i]; ASSERT(conf == 0 || conf > 0); if (conf > 0) confs.Insert(conf); } if (confs.IsEmpty()) return false; const std::pair th(ComputeX84Threshold(confs.Begin(), confs.GetSize())); float minConf = th.first-th.second; float maxConf = th.first+th.second; if (minConf < 0.1f) minConf = 0.1f; if (maxConf < 0.1f) maxConf = 30.f; DEBUG_ULTIMATE("\tconfidence range: [%g, %g]", minConf, maxConf); const float deltaConf = maxConf - minConf; // save image Image8U img(confMap.size()); for (int i=confMap.area(); --i >= 0; ) { const float conf = confMap[i]; img[i] = (conf > 0 ? (uint8_t)CLAMP((conf-minConf)*255.f/deltaConf, 0.f, 255.f) : 0); } return img.Save(fileName); } // ExportConfidenceMap /*----------------------------------------------------------------*/ // export point cloud bool MVS::ExportPointCloud(const String& fileName, const Image& imageData, const DepthMap& depthMap, const NormalMap& normalMap) { ASSERT(!depthMap.empty()); const Camera& P0 = imageData.camera; if (normalMap.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.IsEmpty()); Util::ensureFolder(fileName); const size_t bufferSize = depthMap.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, bufferSize)) 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; const Point2f scaleImage(static_cast(depthMap.cols)/imageData.image.cols, static_cast(depthMap.rows)/imageData.image.rows); for (int j=0; j= 0); if (depth <= 0) continue; const Point3f X(P0.TransformPointI2W(Point3(i,j,depth))); vertex.x = X.x; vertex.y = X.y; vertex.z = X.z; const Pixel8U c(imageData.image.empty() ? Pixel8U::WHITE : imageData.image(ROUND2INT(scaleImage.y*j),ROUND2INT(scaleImage.x*i))); 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 if (!ply.header_complete()) return false; } else { // vertex definition struct Vertex { float x,y,z; float nx,ny,nz; 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}, {"nx", PLY::Float32, PLY::Float32, offsetof(Vertex,nx), 0, 0, 0, 0}, {"ny", PLY::Float32, PLY::Float32, offsetof(Vertex,ny), 0, 0, 0, 0}, {"nz", PLY::Float32, PLY::Float32, offsetof(Vertex,nz), 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.IsEmpty()); Util::ensureFolder(fileName); const size_t bufferSize = depthMap.area()*(8*3/*pos*/+8*3/*normal*/+3*3/*color*/+8/*space*/+2/*eol*/) + 2048/*extra size*/; PLY ply; if (!ply.write(fileName, 1, elem_names, PLY::BINARY_LE, bufferSize)) return false; // describe what properties go into the vertex elements ply.describe_property("vertex", 9, vert_props); // export the array of 3D points Vertex vertex; for (int j=0; j= 0); if (depth <= 0) continue; const Point3f X(P0.TransformPointI2W(Point3(i,j,depth))); vertex.x = X.x; vertex.y = X.y; vertex.z = X.z; const Point3f N(P0.R.t() * Cast(normalMap(j,i))); vertex.nx = N.x; vertex.ny = N.y; vertex.nz = N.z; const Pixel8U c(imageData.image.empty() ? Pixel8U::WHITE : imageData.image(j, i)); 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 if (!ply.header_complete()) return false; } return true; } // ExportPointCloud /*----------------------------------------------------------------*/ // - IDs are the reference view ID and neighbor view IDs used to estimate the depth-map (global ID) bool MVS::ExportDepthDataRaw(const String& fileName, const String& imageFileName, const IIndexArr& IDs, const cv::Size& imageSize, const KMatrix& K, const RMatrix& R, const CMatrix& C, Depth dMin, Depth dMax, const DepthMap& depthMap, const NormalMap& normalMap, const ConfidenceMap& confMap, const ViewsMap& viewsMap) { ASSERT(IDs.size() > 1 && IDs.size() < 256); ASSERT(!depthMap.empty()); ASSERT(confMap.empty() || depthMap.size() == confMap.size()); ASSERT(viewsMap.empty() || depthMap.size() == viewsMap.size()); ASSERT(depthMap.width() <= imageSize.width && depthMap.height() <= imageSize.height); FILE* f = fopen(fileName, "wb"); if (f == NULL) { DEBUG("error: opening file '%s' for writing depth-data", fileName.c_str()); return false; } // write header HeaderDepthDataRaw header; header.name = HeaderDepthDataRaw::HeaderDepthDataRawName(); header.type = HeaderDepthDataRaw::HAS_DEPTH; header.imageWidth = (uint32_t)imageSize.width; header.imageHeight = (uint32_t)imageSize.height; header.depthWidth = (uint32_t)depthMap.cols; header.depthHeight = (uint32_t)depthMap.rows; header.dMin = dMin; header.dMax = dMax; if (!normalMap.empty()) header.type |= HeaderDepthDataRaw::HAS_NORMAL; if (!confMap.empty()) header.type |= HeaderDepthDataRaw::HAS_CONF; if (!viewsMap.empty()) header.type |= HeaderDepthDataRaw::HAS_VIEWS; fwrite(&header, sizeof(HeaderDepthDataRaw), 1, f); // write image file name STATIC_ASSERT(sizeof(String::value_type) == sizeof(char)); const String FileName(MAKE_PATH_REL(Util::getFullPath(Util::getFilePath(fileName)), Util::getFullPath(imageFileName))); const uint16_t nFileNameSize((uint16_t)FileName.length()); fwrite(&nFileNameSize, sizeof(uint16_t), 1, f); fwrite(FileName.c_str(), sizeof(char), nFileNameSize, f); // write neighbor IDs STATIC_ASSERT(sizeof(uint32_t) == sizeof(IIndex)); const uint32_t nIDs(IDs.size()); fwrite(&nIDs, sizeof(IIndex), 1, f); fwrite(IDs.data(), sizeof(IIndex), nIDs, f); // write pose STATIC_ASSERT(sizeof(double) == sizeof(REAL)); fwrite(K.val, sizeof(REAL), 9, f); fwrite(R.val, sizeof(REAL), 9, f); fwrite(C.ptr(), sizeof(REAL), 3, f); // write depth-map fwrite(depthMap.getData(), sizeof(float), depthMap.area(), f); // write normal-map if ((header.type & HeaderDepthDataRaw::HAS_NORMAL) != 0) fwrite(normalMap.getData(), sizeof(float)*3, normalMap.area(), f); // write confidence-map if ((header.type & HeaderDepthDataRaw::HAS_CONF) != 0) fwrite(confMap.getData(), sizeof(float), confMap.area(), f); // write views-map if ((header.type & HeaderDepthDataRaw::HAS_VIEWS) != 0) fwrite(viewsMap.getData(), sizeof(uint8_t)*4, viewsMap.area(), f); const bool bRet(ferror(f) == 0); fclose(f); return bRet; } // ExportDepthDataRaw bool MVS::ImportDepthDataRaw(const String& fileName, String& imageFileName, IIndexArr& IDs, cv::Size& imageSize, KMatrix& K, RMatrix& R, CMatrix& C, Depth& dMin, Depth& dMax, DepthMap& depthMap, NormalMap& normalMap, ConfidenceMap& confMap, ViewsMap& viewsMap, unsigned flags) { FILE* f = fopen(fileName, "rb"); if (f == NULL) { DEBUG("error: opening file '%s' for reading depth-data", fileName.c_str()); return false; } // read header HeaderDepthDataRaw header; if (fread(&header, sizeof(HeaderDepthDataRaw), 1, f) != 1 || header.name != HeaderDepthDataRaw::HeaderDepthDataRawName() || (header.type & HeaderDepthDataRaw::HAS_DEPTH) == 0 || header.depthWidth <= 0 || header.depthHeight <= 0 || header.imageWidth < header.depthWidth || header.imageHeight < header.depthHeight) { DEBUG("error: invalid depth-data file '%s'", fileName.c_str()); return false; } // read image file name STATIC_ASSERT(sizeof(String::value_type) == sizeof(char)); uint16_t nFileNameSize; fread(&nFileNameSize, sizeof(uint16_t), 1, f); imageFileName.resize(nFileNameSize); fread(imageFileName.data(), sizeof(char), nFileNameSize, f); // read neighbor IDs STATIC_ASSERT(sizeof(uint32_t) == sizeof(IIndex)); uint32_t nIDs; fread(&nIDs, sizeof(IIndex), 1, f); ASSERT(nIDs > 0 && nIDs < 256); IDs.resize(nIDs); fread(IDs.data(), sizeof(IIndex), nIDs, f); // read pose STATIC_ASSERT(sizeof(double) == sizeof(REAL)); fread(K.val, sizeof(REAL), 9, f); fread(R.val, sizeof(REAL), 9, f); fread(C.ptr(), sizeof(REAL), 3, f); // read depth-map dMin = header.dMin; dMax = header.dMax; imageSize.width = header.imageWidth; imageSize.height = header.imageHeight; if ((flags & HeaderDepthDataRaw::HAS_DEPTH) != 0) { depthMap.create(header.depthHeight, header.depthWidth); fread(depthMap.getData(), sizeof(float), depthMap.area(), f); } else { fseek(f, sizeof(float)*header.depthWidth*header.depthHeight, SEEK_CUR); } // read normal-map if ((header.type & HeaderDepthDataRaw::HAS_NORMAL) != 0) { if ((flags & HeaderDepthDataRaw::HAS_NORMAL) != 0) { normalMap.create(header.depthHeight, header.depthWidth); fread(normalMap.getData(), sizeof(float)*3, normalMap.area(), f); } else { fseek(f, sizeof(float)*3*header.depthWidth*header.depthHeight, SEEK_CUR); } } // read confidence-map if ((header.type & HeaderDepthDataRaw::HAS_CONF) != 0) { if ((flags & HeaderDepthDataRaw::HAS_CONF) != 0) { confMap.create(header.depthHeight, header.depthWidth); fread(confMap.getData(), sizeof(float), confMap.area(), f); } else { fseek(f, sizeof(float)*header.depthWidth*header.depthHeight, SEEK_CUR); } } // read visibility-map if ((header.type & HeaderDepthDataRaw::HAS_VIEWS) != 0) { if ((flags & HeaderDepthDataRaw::HAS_VIEWS) != 0) { viewsMap.create(header.depthHeight, header.depthWidth); fread(viewsMap.getData(), sizeof(uint8_t)*4, viewsMap.area(), f); } } const bool bRet(ferror(f) == 0); fclose(f); return bRet; } // ImportDepthDataRaw /*----------------------------------------------------------------*/ // compare the estimated and ground-truth depth-maps void MVS::CompareDepthMaps(const DepthMap& depthMap, const DepthMap& depthMapGT, uint32_t idxImage, float threshold) { TD_TIMER_START(); const uint32_t width = (uint32_t)depthMap.width(); const uint32_t height = (uint32_t)depthMap.height(); // compute depth errors for each pixel cv::resize(depthMapGT, depthMapGT, depthMap.size()); unsigned nErrorPixels(0); unsigned nExtraPixels(0); unsigned nMissingPixels(0); FloatArr depths(0, depthMap.area()); FloatArr depthsGT(0, depthMap.area()); FloatArr errors(0, depthMap.area()); for (uint32_t i=0; i ms(errors.data(), errors.size()); const float mean((float)ms.GetMean()); const float stddev((float)ms.GetStdDev()); const std::pair th(ComputeX84Threshold(errors.data(), errors.size())); #if TD_VERBOSE != TD_VERBOSE_OFF IDX idxPixel = 0; Image8U3 errorsVisual(depthMap.size()); for (uint32_t i=0; i threshold) { pix = Pixel8U::RED; ++nErrorPixels; continue; } const uint8_t gray((uint8_t)CLAMP((1.f-SAFEDIVIDE(ABS(error), threshold))*255.f, 0.f, 255.f)); pix = Pixel8U(gray, gray, gray); } } errorsVisual.Save(ComposeDepthFilePath(idxImage, "errors.png")); #endif VERBOSE("Depth-maps compared for image % 3u: %.4f PSNR; %g median %g mean %g stddev error; %u (%.2f%%%%) error %u (%.2f%%%%) missing %u (%.2f%%%%) extra pixels (%s)", idxImage, fPSNR, th.first, mean, stddev, nErrorPixels, (float)nErrorPixels*100.f/depthMap.area(), nMissingPixels, (float)nMissingPixels*100.f/depthMap.area(), nExtraPixels, (float)nExtraPixels*100.f/depthMap.area(), TD_TIMER_GET_FMT().c_str() ); } // compare the estimated and ground-truth normal-maps void MVS::CompareNormalMaps(const NormalMap& normalMap, const NormalMap& normalMapGT, uint32_t idxImage) { TD_TIMER_START(); // load normal data const uint32_t width = (uint32_t)normalMap.width(); const uint32_t height = (uint32_t)normalMap.height(); // compute normal errors for each pixel cv::resize(normalMapGT, normalMapGT, normalMap.size()); FloatArr errors(0, normalMap.area()); for (uint32_t i=0; i ms(errors.Begin(), errors.GetSize()); const float mean((float)ms.GetMean()); const float stddev((float)ms.GetStdDev()); const std::pair th(ComputeX84Threshold(errors.Begin(), errors.GetSize())); VERBOSE("Normal-maps compared for image % 3u: %.2f median %.2f mean %.2f stddev error (%s)", idxImage, th.first, mean, stddev, TD_TIMER_GET_FMT().c_str() ); } /*----------------------------------------------------------------*/