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.
2163 lines
81 KiB
2163 lines
81 KiB
/* |
|
* DepthMap.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 "DepthMap.h" |
|
#include "Mesh.h" |
|
#include "../Common/AutoEstimator.h" |
|
// CGAL: depth-map initialization |
|
#include <CGAL/Simple_cartesian.h> |
|
#include <CGAL/Delaunay_triangulation_2.h> |
|
#include <CGAL/Triangulation_vertex_base_with_info_2.h> |
|
// CGAL: estimate normals |
|
#include <CGAL/Simple_cartesian.h> |
|
#include <CGAL/property_map.h> |
|
#include <CGAL/pca_estimate_normals.h> |
|
|
|
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<Point3f>* 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<REAL>(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<nWindow; j+=nPointsStep) { |
|
const int y = ptCorner.y+j; |
|
if (y >= size.height) |
|
break; |
|
for (int i=ptCornerRel.x; i<nWindow; i+=nPointsStep) { |
|
const int x = ptCorner.x+i; |
|
if (x >= 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<nWindow; j+=nPointsStep) { |
|
const int y = ptCorner.y+j; |
|
if (y >= size.height) |
|
break; |
|
for (int i=ptCornerRel.x; i<nWindow; i+=nPointsStep) { |
|
const int x = ptCorner.x+i; |
|
if (x >= 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<Point3f>* 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; r<depthMap.rows; ++r) { |
|
for (int c=0; c<depthMap.cols; ++c) { |
|
if (mask.isSet(r,c)) |
|
continue; |
|
// discard depth-map section ignored by mask |
|
depthMap(r,c) = 0; |
|
if (!normalMap.empty()) |
|
normalMap(r,c) = Normal::ZERO; |
|
if (!confMap.empty()) |
|
confMap(r,c) = 0; |
|
} |
|
} |
|
} // ApplyIgnoreMask |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
bool DepthData::Save(const String& fileName) const |
|
{ |
|
ASSERT(IsValid() && !depthMap.empty() && !confMap.empty()); |
|
const String fileNameTmp(fileName+".tmp"); { |
|
// serialize out the current state |
|
IIndexArr IDs(0, images.size()); |
|
for (const ViewData& image: images) |
|
IDs.push_back(image.GetID()); |
|
const ViewData& image0 = GetView(); |
|
if (!ExportDepthDataRaw(fileNameTmp, image0.pImageData->name, 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<size.height; ++r) { |
|
for (int c=0; c<size.width; ++c) { |
|
if (mask(r,c) == nIgnoreMaskLabel) |
|
bmask.unset(r,c); |
|
} |
|
} |
|
} |
|
return true; |
|
} // ImportIgnoreMask |
|
|
|
// create the map for converting index to matrix position |
|
// 1 2 3 |
|
// 1 2 4 7 5 3 6 8 9 --> 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) { |
|
if (h*2 > size.height - dy) |
|
h = size.height - dy; |
|
int lastX = 0; |
|
MapRef x(MapRef::ZERO); |
|
for (int i=0, ei=w*h; i<ei; ++i) { |
|
const MapRef pt(x.x, x.y+dy); |
|
if (mask.empty() || mask.isSet(pt)) |
|
coords.Insert(pt); |
|
if (x.x-- == 0 || ++x.y == h) { |
|
if (++lastX < w) { |
|
x.x = lastX; |
|
x.y = 0; |
|
} else { |
|
x.x = w1; |
|
x.y = lastX - w1; |
|
} |
|
} |
|
} |
|
} |
|
} |
|
|
|
// replace POWI(0.5f, idxScaleRange): 0 1 2 3 4 5 6 7 8 9 10 11 |
|
const float DepthEstimator::scaleRanges[12] = {1.f, 0.5f, 0.25f, 0.125f, 0.0625f, 0.03125f, 0.015625f, 0.0078125f, 0.00390625f, 0.001953125f, 0.0009765625f, 0.00048828125f}; |
|
|
|
DepthEstimator::DepthEstimator( |
|
unsigned nIter, DepthData& _depthData0, volatile Thread::safe_t& _idx, |
|
#if DENSE_NCC == DENSE_NCC_WEIGHTED |
|
WeightMap& _weightMap0, |
|
#else |
|
const Image64F& _image0Sum, |
|
#endif |
|
const MapRefArr& _coords) |
|
: |
|
#ifndef _RELEASE |
|
rnd(SEACAVE::Random::default_seed), |
|
#endif |
|
idxPixel(_idx), |
|
neighbors(0,2), |
|
#if DENSE_SMOOTHNESS != DENSE_SMOOTHNESS_NA |
|
neighborsClose(0,4), |
|
#endif |
|
scores(_depthData0.images.size()-1), |
|
depthMap0(_depthData0.depthMap), normalMap0(_depthData0.normalMap), confMap0(_depthData0.confMap), |
|
#if DENSE_NCC == DENSE_NCC_WEIGHTED |
|
weightMap0(_weightMap0), |
|
#endif |
|
nIteration(nIter), |
|
images(_depthData0.images.begin()+1, _depthData0.images.end()), image0(_depthData0.images[0]), |
|
#if DENSE_NCC != DENSE_NCC_WEIGHTED |
|
image0Sum(_image0Sum), |
|
#endif |
|
coords(_coords), size(_depthData0.images.First().image.size()), |
|
dMin(_depthData0.dMin), dMax(_depthData0.dMax), |
|
dMinSqr(SQRT(_depthData0.dMin)), dMaxSqr(SQRT(_depthData0.dMax)), |
|
dir(nIter%2 ? RB2LT : LT2RB), |
|
#if DENSE_AGGNCC == DENSE_AGGNCC_NTH |
|
idxScore((_depthData0.images.size()-1)/3), |
|
#elif DENSE_AGGNCC == DENSE_AGGNCC_MINMEAN |
|
idxScore(_depthData0.images.size()<=2 ? 0u : 1u), |
|
#endif |
|
smoothBonusDepth(1.f-OPTDENSE::fRandomSmoothBonus), smoothBonusNormal((1.f-OPTDENSE::fRandomSmoothBonus)*0.96f), |
|
smoothSigmaDepth(-1.f/(2.f*SQUARE(OPTDENSE::fRandomSmoothDepth))), // used in exp(-x^2 / (2*(0.02^2))) |
|
smoothSigmaNormal(-1.f/(2.f*SQUARE(FD2R(OPTDENSE::fRandomSmoothNormal)))), // used in exp(-x^2 / (2*(0.22^2))) |
|
thMagnitudeSq(OPTDENSE::fDescriptorMinMagnitudeThreshold>0?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<REAL>(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<float,1>(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<float,float,nTexels>(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<float,1>(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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(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<float>(image0.camera.TransformPointI2C(Point3(nx, ndepth))) |
|
#endif |
|
}); |
|
} |
|
} |
|
#endif |
|
} |
|
float& conf = confMap0(x0); |
|
Depth& depth = depthMap0(x0); |
|
Normal& normal = normalMap0(x0); |
|
const Normal viewDir(Cast<float>(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<OPTDENSE::nRandomIters; ++iter) { |
|
const Depth ndepth(RandomDepth(dMinSqr, dMaxSqr)); |
|
const Normal nnormal(RandomNormal(viewDir)); |
|
const float nconf(ScorePixel(ndepth, nnormal)); |
|
ASSERT(nconf >= 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<OPTDENSE::nRandomIters; ++iter) { |
|
const Depth ndepth(rnd.randomMeanRange(depth, depthRange*scaleRange)); |
|
if (!ISINSIDE(ndepth, dMin, dMax)) |
|
continue; |
|
const Point2f np(rnd.randomMeanRange(p.x, angle1Range*scaleRange), rnd.randomMeanRange(p.y, angle2Range*scaleRange)); |
|
Dir2Normal(np, nnormal); |
|
if (nnormal.dot(viewDir) >= 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<numCosts; ++idxCost) { |
|
const Depth ndepth(depths[idxCost]); |
|
const Normal nnormal(normals[idxCost]); |
|
#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; |
|
} |
|
} |
|
#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<REAL>(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<REAL>(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<float>(X0)*depth)); |
|
#else |
|
plane.m_vN = normal; |
|
plane.m_fD = -depth*normal.dot(Cast<float>(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<float>(X0)); |
|
std::uniform_real_distribution<float> 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<float,float> TriangulatePointsDelaunay(const DepthData::ViewData& image, const PointCloud& pointcloud, const IndexArr& points, Mesh& mesh, Point2fArr& projs, bool bAddCorners) |
|
{ |
|
typedef CGAL::Simple_cartesian<double> kernel_t; |
|
typedef CGAL::Triangulation_vertex_base_with_info_2<Mesh::VIndex, kernel_t> vertex_base_t; |
|
typedef CGAL::Triangulation_data_structure_2<vertex_base_t> triangulation_data_structure_t; |
|
typedef CGAL::Delaunay_triangulation_2<kernel_t, triangulation_data_structure_t> 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<float,float> 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<const Point2d&>(vcorner->point()); |
|
const Ray3d rayA(Point3d::ZERO, normalized(Cast<REAL>(mesh.vertices[vcorner->info()]))); |
|
typedef TIndexScore<float,float> 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<REAL>(mesh.vertices[fc->vertex(0)->info()]), |
|
Cast<REAL>(mesh.vertices[fc->vertex(1)->info()]), |
|
Cast<REAL>(mesh.vertices[fc->vertex(2)->info()]) |
|
); |
|
const Point3d poszB(rayA.Intersects(planeB)); |
|
if (poszB.z <= 0) |
|
continue; |
|
const Point2d posB(( |
|
reinterpret_cast<const Point2d&>(fc->vertex(0)->point())+ |
|
reinterpret_cast<const Point2d&>(fc->vertex(1)->point())+ |
|
reinterpret_cast<const Point2d&>(fc->vertex(2)->point()))/3.f |
|
); |
|
const double dist(norm(posB-posA)); |
|
depths.StoreTop<numPoints>(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<float,float> 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<RasterDepth> { |
|
typedef TRasterMeshBase<RasterDepth> 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<float,float> 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<RasterDepth> { |
|
typedef TRasterMeshBase<RasterDepth> 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 <typename TYPE> |
|
class TPlaneSolverAdaptor |
|
{ |
|
public: |
|
enum { MINIMUM_SAMPLES = 3 }; |
|
enum { MAX_MODELS = 1 }; |
|
|
|
typedef TYPE Type; |
|
typedef TPoint3<TYPE> Point; |
|
typedef CLISTDEF0(Point) Points; |
|
typedef TPlane<TYPE,3> 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<size_t>& samples, Models& models) const { |
|
ASSERT(samples.size() == MINIMUM_SAMPLES); |
|
Point points[MINIMUM_SAMPLES]; |
|
for (size_t i=0; i<MINIMUM_SAMPLES; ++i) |
|
points[i] = points_[samples[i]]; |
|
if (CheckCollinearity(points, 3)) |
|
return false; |
|
models.Resize(1); |
|
models[0] = Model(points[0], points[1], points[2]); |
|
return true; |
|
} |
|
|
|
inline void EvaluateModel(const Model& model) { |
|
model2evaluate = model; |
|
} |
|
|
|
inline double Error(size_t sample) const { |
|
return SQUARE(model2evaluate.Distance(points_[sample])); |
|
} |
|
|
|
static double Error(const Model& plane, const Points& points) { |
|
double e(0); |
|
for (const Point& X: points) |
|
e += plane.DistanceAbs(X); |
|
return e/points.size(); |
|
} |
|
|
|
inline size_t NumSamples() const { return static_cast<size_t>(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 <typename TYPE, typename Sampler, bool bFixThreshold> |
|
unsigned TEstimatePlane(const CLISTDEF0(TPoint3<TYPE>)& points, TPlane<TYPE,3>& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) |
|
{ |
|
typedef TPlaneSolverAdaptor<TYPE> PlaneSolverAdaptor; |
|
|
|
plane.Invalidate(); |
|
|
|
const unsigned nPoints = (unsigned)points.size(); |
|
if (nPoints < PlaneSolverAdaptor::MINIMUM_SAMPLES) { |
|
ASSERT("too few points" == NULL); |
|
return 0; |
|
} |
|
|
|
// normalize points |
|
TMatrix<TYPE,4,4> H; |
|
typename PlaneSolverAdaptor::Points normPoints; |
|
NormalizePoints(points, normPoints, &H); |
|
|
|
// plane robust estimation |
|
std::vector<size_t> 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<double,double> 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<TYPE> fitPlane; |
|
for (unsigned i=0; i<inliers_count; ++i) |
|
fitPlane.Update(normPoints[vec_inliers[i]]); |
|
fitPlane.GetPlane(plane); |
|
|
|
// un-normalize plane |
|
plane.m_fD = (plane.m_fD+plane.m_vN.dot(typename PlaneSolverAdaptor::Model::POINT(H(0,3),H(1,3),H(2,3))))/H(0,0); |
|
|
|
// if a list of inliers is requested, copy it |
|
if (arrInliers) { |
|
inliers_count = 0; |
|
for (unsigned i=0; i<nPoints; ++i) |
|
if ((arrInliers[i] = (plane.DistanceAbs(points[i]) <= maxThreshold)) == true) |
|
++inliers_count; |
|
} |
|
return inliers_count; |
|
} // TEstimatePlane |
|
|
|
} // namespace MVS |
|
|
|
// Robustly estimate the plane that fits best the given points |
|
unsigned MVS::EstimatePlane(const Point3dArr& points, Planed& plane, double& maxThreshold, bool arrInliers[], size_t maxIters) |
|
{ |
|
return TEstimatePlane<double,UniformSampler,false>(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<double,UniformSamplerLockFirst,false>(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<double,UniformSampler,true>(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<double,UniformSamplerLockFirst,true>(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<float,UniformSampler,false>(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<float,UniformSamplerLockFirst,false>(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<float,UniformSampler,true>(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<float,UniformSamplerLockFirst,true>(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<float,1>(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<double> kernel_t; |
|
typedef kernel_t::Point_3 point_t; |
|
typedef kernel_t::Vector_3 vector_t; |
|
typedef std::pair<point_t,vector_t> PointVectorPair; |
|
// fetch the point set |
|
std::vector<PointVectorPair> pointvectors(pointcloud.points.GetSize()); |
|
FOREACH(i, pointcloud.points) |
|
reinterpret_cast<Point3d&>(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<CGAL::Sequential_tag>( |
|
#endif |
|
pointvectors.begin(), pointvectors.end(), |
|
CGAL::First_of_pair_property_map<PointVectorPair>(), |
|
CGAL::Second_of_pair_property_map<PointVectorPair>(), |
|
numNeighbors |
|
); |
|
#else |
|
CGAL::pca_estimate_normals<CGAL::Sequential_tag>( |
|
pointvectors, numNeighbors, |
|
CGAL::parameters::point_map(CGAL::First_of_pair_property_map<PointVectorPair>()) |
|
.normal_map(CGAL::Second_of_pair_property_map<PointVectorPair>()) |
|
); |
|
#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<const Point3d&>(pointvectors[i].second); |
|
// correct normal orientation |
|
ASSERT(!views.IsEmpty()); |
|
const Image& imageData = images[views.First()]; |
|
if (normal.dot(Cast<float>(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<normalMap.rows; ++r) { |
|
for (int c=0; c<normalMap.cols; ++c) { |
|
#if 0 |
|
const Depth d(depthMap(r,c)); |
|
if (d <= 0) { |
|
normalMap(r,c) = Normal::ZERO; |
|
continue; |
|
} |
|
Depth dl, du; |
|
if (depthMap.isInside(ImageRef(c-1,r-1)) && Tool::IsDepthValid(d, dl=depthMap(r,c-1)) && Tool::IsDepthValid(d, du=depthMap(r-1,c))) |
|
normalMap(r,c) = Tool::ComputeNormal(K, c, r, d, du-d, dl-d); |
|
else |
|
if (depthMap.isInside(ImageRef(c+1,r-1)) && Tool::IsDepthValid(d, dl=depthMap(r,c+1)) && Tool::IsDepthValid(d, du=depthMap(r-1,c))) |
|
normalMap(r,c) = Tool::ComputeNormal(K, c, r, d, du-d, d-dl); |
|
else |
|
if (depthMap.isInside(ImageRef(c+1,r+1)) && Tool::IsDepthValid(d, dl=depthMap(r,c+1)) && Tool::IsDepthValid(d, du=depthMap(r+1,c))) |
|
normalMap(r,c) = Tool::ComputeNormal(K, c, r, d, d-du, d-dl); |
|
else |
|
if (depthMap.isInside(ImageRef(c-1,r+1)) && Tool::IsDepthValid(d, dl=depthMap(r,c-1)) && Tool::IsDepthValid(d, du=depthMap(r+1,c))) |
|
normalMap(r,c) = Tool::ComputeNormal(K, c, r, d, d-du, dl-d); |
|
else |
|
normalMap(r,c) = Normal(0,0,-1); |
|
#else |
|
// calculates depth gradient at x |
|
Normal& n = normalMap(r,c); |
|
if (Tool::DepthGradient(depthMap, ImageRef(c,r), n)) |
|
n = Tool::ComputeNormal(K, c, r, n.x, n.y, n.z); |
|
else |
|
n = Normal::ZERO; |
|
#endif |
|
ASSERT(normalMap(r,c).dot(K.inv()*Point3f(float(c),float(r),1.f)) <= 0); |
|
} |
|
} |
|
return true; |
|
} // EstimateNormalMap |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
// save the depth map in our .dmap file format |
|
bool MVS::SaveDepthMap(const String& fileName, const DepthMap& depthMap) |
|
{ |
|
ASSERT(!depthMap.empty()); |
|
return SerializeSave(depthMap, fileName, ARCHIVE_DEFAULT); |
|
} // SaveDepthMap |
|
/*----------------------------------------------------------------*/ |
|
// load the depth map from our .dmap file format |
|
bool MVS::LoadDepthMap(const String& fileName, DepthMap& depthMap) |
|
{ |
|
return SerializeLoad(depthMap, fileName, ARCHIVE_DEFAULT); |
|
} // LoadDepthMap |
|
/*----------------------------------------------------------------*/ |
|
|
|
// save the normal map in our .nmap file format |
|
bool MVS::SaveNormalMap(const String& fileName, const NormalMap& normalMap) |
|
{ |
|
ASSERT(!normalMap.empty()); |
|
return SerializeSave(normalMap, fileName, ARCHIVE_DEFAULT); |
|
} // SaveNormalMap |
|
/*----------------------------------------------------------------*/ |
|
// load the normal map from our .nmap file format |
|
bool MVS::LoadNormalMap(const String& fileName, NormalMap& normalMap) |
|
{ |
|
return SerializeLoad(normalMap, fileName, ARCHIVE_DEFAULT); |
|
} // LoadNormalMap |
|
/*----------------------------------------------------------------*/ |
|
|
|
// save the confidence map in our .cmap file format |
|
bool MVS::SaveConfidenceMap(const String& fileName, const ConfidenceMap& confMap) |
|
{ |
|
ASSERT(!confMap.empty()); |
|
return SerializeSave(confMap, fileName, ARCHIVE_DEFAULT); |
|
} // SaveConfidenceMap |
|
/*----------------------------------------------------------------*/ |
|
// load the confidence map from our .cmap file format |
|
bool MVS::LoadConfidenceMap(const String& fileName, ConfidenceMap& confMap) |
|
{ |
|
return SerializeLoad(confMap, fileName, ARCHIVE_DEFAULT); |
|
} // LoadConfidenceMap |
|
/*----------------------------------------------------------------*/ |
|
|
|
|
|
|
|
// export depth map as an image (dark - far depth, light - close depth) |
|
Image8U3 MVS::DepthMap2Image(const DepthMap& depthMap, Depth minDepth, Depth maxDepth) |
|
{ |
|
ASSERT(!depthMap.empty()); |
|
// find min and max values |
|
if (minDepth == FLT_MAX && maxDepth == 0) { |
|
cList<Depth,Depth,0> 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<Depth,Depth> th(ComputeX84Threshold<Depth,Depth>(depths.data(), depths.size())); |
|
const std::pair<Depth,Depth> 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<float,float> th(ComputeX84Threshold<float,float>(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<float>(depthMap.cols)/imageData.image.cols, static_cast<float>(depthMap.rows)/imageData.image.rows); |
|
for (int j=0; j<depthMap.rows; ++j) { |
|
for (int i=0; i<depthMap.cols; ++i) { |
|
const Depth& depth = depthMap(j,i); |
|
ASSERT(depth >= 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<depthMap.rows; ++j) { |
|
for (int i=0; i<depthMap.cols; ++i) { |
|
const Depth& depth = depthMap(j,i); |
|
ASSERT(depth >= 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<REAL>(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<height; ++i) { |
|
for (uint32_t j=0; j<width; ++j) { |
|
const Depth& depth = depthMap(i,j); |
|
const Depth& depthGT = depthMapGT(i,j); |
|
if (depth != 0 && depthGT == 0) { |
|
++nExtraPixels; |
|
continue; |
|
} |
|
if (depth == 0 && depthGT != 0) { |
|
++nMissingPixels; |
|
continue; |
|
} |
|
depths.Insert(depth); |
|
depthsGT.Insert(depthGT); |
|
const float error(depthGT==0 ? 0 : DepthSimilarity(depthGT, depth)); |
|
errors.Insert(error); |
|
} |
|
} |
|
const float fPSNR((float)ComputePSNR(DMatrix32F((int)depths.size(),1,depths.data()), DMatrix32F((int)depthsGT.size(),1,depthsGT.data()))); |
|
const MeanStd<float,double> ms(errors.data(), errors.size()); |
|
const float mean((float)ms.GetMean()); |
|
const float stddev((float)ms.GetStdDev()); |
|
const std::pair<float,float> th(ComputeX84Threshold<float,float>(errors.data(), errors.size())); |
|
#if TD_VERBOSE != TD_VERBOSE_OFF |
|
IDX idxPixel = 0; |
|
Image8U3 errorsVisual(depthMap.size()); |
|
for (uint32_t i=0; i<height; ++i) { |
|
for (uint32_t j=0; j<width; ++j) { |
|
Pixel8U& pix = errorsVisual(i,j); |
|
const Depth& depth = depthMap(i,j); |
|
const Depth& depthGT = depthMapGT(i,j); |
|
if (depth != 0 && depthGT == 0) { |
|
pix = Pixel8U::GREEN; |
|
continue; |
|
} |
|
if (depth == 0 && depthGT != 0) { |
|
pix = Pixel8U::BLUE; |
|
continue; |
|
} |
|
const float error = errors[idxPixel++]; |
|
if (depth == 0 && depthGT == 0) { |
|
pix = Pixel8U::BLACK; |
|
continue; |
|
} |
|
if (error > 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<height; ++i) { |
|
for (uint32_t j=0; j<width; ++j) { |
|
const Normal& normal = normalMap(i,j); |
|
const Normal& normalGT = normalMapGT(i,j); |
|
if (normal != Normal::ZERO && normalGT == Normal::ZERO) |
|
continue; |
|
if (normal == Normal::ZERO && normalGT != Normal::ZERO) |
|
continue; |
|
if (normal == Normal::ZERO && normalGT == Normal::ZERO) { |
|
errors.Insert(0.f); |
|
continue; |
|
} |
|
ASSERT(ISEQUAL(norm(normal),1.f) && ISEQUAL(norm(normalGT),1.f)); |
|
const float error(FR2D(ACOS(CLAMP(normal.dot(normalGT), -1.f, 1.f)))); |
|
errors.Insert(error); |
|
} |
|
} |
|
const MeanStd<float,double> ms(errors.Begin(), errors.GetSize()); |
|
const float mean((float)ms.GetMean()); |
|
const float stddev((float)ms.GetStdDev()); |
|
const std::pair<float,float> th(ComputeX84Threshold<float,float>(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() |
|
); |
|
} |
|
/*----------------------------------------------------------------*/
|
|
|