2164 lines
81 KiB
C++
2164 lines
81 KiB
C++
/*
|
|
* 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()
|
|
);
|
|
}
|
|
/*----------------------------------------------------------------*/
|