Files
opencv-MIRROR/modules/ptcloud/src/kinfu_frame.cpp
Alexander Smorkalov fc3803c67b Merge pull request #29224 from asmorkalov:as/ptcloud2
Dedicated pointcloud module #29224

OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [ ] The PR is proposed to the proper branch
- [ ] There is a reference to the original bug report and related work
- [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [ ] The feature is well documented and sample code can be built with the project CMake
2026-06-04 12:19:02 +03:00

784 lines
24 KiB
C++

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html
// Partially rewritten from https://github.com/Nerei/kinfu_remake
// Copyright(c) 2012, Anatoly Baksheev. All rights reserved.
#include "precomp.hpp"
#include "opencv2/ptcloud/detail/kinfu_frame.hpp"
#include "utils.hpp"
#include "opencl_kernels_ptcloud.hpp"
namespace cv {
static void computePointsNormals(const cv::Intr, float depthFactor, const Depth, Points, Normals );
void computePointsNormalsColors(const Intr, const Intr, float, const Depth, const Colors, Points, Normals, Colors);
static Depth pyrDownBilateral(const Depth depth, float sigma);
static void pyrDownPointsNormals(const Points p, const Normals n, Points& pdown, Normals& ndown);
template<int p>
inline float specPow(float x)
{
if(p % 2 == 0)
{
float v = specPow<p/2>(x);
return v*v;
}
else
{
float v = specPow<(p-1)/2>(x);
return v*v*x;
}
}
template<>
inline float specPow<0>(float /*x*/)
{
return 1.f;
}
template<>
inline float specPow<1>(float x)
{
return x;
}
struct RenderInvoker : ParallelLoopBody
{
RenderInvoker(const Points& _points, const Normals& _normals, Mat_<Vec4b>& _img, Vec3f _lightPt, Size _sz) :
ParallelLoopBody(),
points(_points),
normals(_normals),
img(_img),
lightPt(_lightPt),
sz(_sz)
{ }
virtual void operator ()(const Range& range) const override
{
for(int y = range.start; y < range.end; y++)
{
Vec4b* imgRow = img[y];
const ptype* ptsRow = points[y];
const ptype* nrmRow = normals[y];
for(int x = 0; x < sz.width; x++)
{
Point3f p = fromPtype(ptsRow[x]);
Point3f n = fromPtype(nrmRow[x]);
Vec4b color;
if(isNaN(p))
{
color = Vec4b(0, 32, 0, 0);
}
else
{
const float Ka = 0.3f; //ambient coeff
const float Kd = 0.5f; //diffuse coeff
const float Ks = 0.2f; //specular coeff
const int sp = 20; //specular power
const float Ax = 1.f; //ambient color, can be RGB
const float Dx = 1.f; //diffuse color, can be RGB
const float Sx = 1.f; //specular color, can be RGB
const float Lx = 1.f; //light color
Point3f l = normalize(lightPt - Vec3f(p));
Point3f v = normalize(-Vec3f(p));
Point3f r = normalize(Vec3f(2.f*n*n.dot(l) - l));
uchar ix = (uchar)((Ax*Ka*Dx + Lx*Kd*Dx*max(0.f, n.dot(l)) +
Lx*Ks*Sx*specPow<sp>(max(0.f, r.dot(v))))*255.f);
color = Vec4b(ix, ix, ix, 0);
}
imgRow[x] = color;
}
}
}
const Points& points;
const Normals& normals;
Mat_<Vec4b>& img;
Vec3f lightPt;
Size sz;
};
struct RenderColorInvoker : ParallelLoopBody
{
RenderColorInvoker(const Points& _points, const Colors& _colors, Mat_<Vec4b>& _img, Size _sz) :
ParallelLoopBody(),
points(_points),
colors(_colors),
img(_img),
sz(_sz)
{ }
virtual void operator ()(const Range& range) const override
{
for(int y = range.start; y < range.end; y++)
{
Vec4b* imgRow = img[y];
const ptype* ptsRow = points[y];
const ptype* clrRow = colors[y];
for(int x = 0; x < sz.width; x++)
{
Point3f p = fromPtype(ptsRow[x]);
Point3f c = fromPtype(clrRow[x]);
Vec4b color;
if(isNaN(p) || isNaN(c))
{
color = Vec4b(0, 32, 0, 0);
}
else
{
color = Vec4b((uchar)c.x, (uchar)c.y, (uchar)c.z, (uchar)0);
}
imgRow[x] = color;
}
}
}
const Points& points;
const Colors& colors;
Mat_<Vec4b>& img;
Size sz;
};
void pyrDownPointsNormals(const Points p, const Normals n, Points &pdown, Normals &ndown)
{
CV_TRACE_FUNCTION();
for(int y = 0; y < pdown.rows; y++)
{
ptype* ptsRow = pdown[y];
ptype* nrmRow = ndown[y];
const ptype* pUpRow0 = p[2*y];
const ptype* pUpRow1 = p[2*y+1];
const ptype* nUpRow0 = n[2*y];
const ptype* nUpRow1 = n[2*y+1];
for(int x = 0; x < pdown.cols; x++)
{
Point3f point = nan3, normal = nan3;
Point3f d00 = fromPtype(pUpRow0[2*x]);
Point3f d01 = fromPtype(pUpRow0[2*x+1]);
Point3f d10 = fromPtype(pUpRow1[2*x]);
Point3f d11 = fromPtype(pUpRow1[2*x+1]);
if(!(isNaN(d00) || isNaN(d01) || isNaN(d10) || isNaN(d11)))
{
point = (d00 + d01 + d10 + d11)*0.25f;
Point3f n00 = fromPtype(nUpRow0[2*x]);
Point3f n01 = fromPtype(nUpRow0[2*x+1]);
Point3f n10 = fromPtype(nUpRow1[2*x]);
Point3f n11 = fromPtype(nUpRow1[2*x+1]);
normal = (n00 + n01 + n10 + n11)*0.25f;
}
ptsRow[x] = toPtype(point);
nrmRow[x] = toPtype(normal);
}
}
}
struct PyrDownBilateralInvoker : ParallelLoopBody
{
PyrDownBilateralInvoker(const Depth& _depth, Depth& _depthDown, float _sigma) :
ParallelLoopBody(),
depth(_depth),
depthDown(_depthDown),
sigma(_sigma)
{ }
virtual void operator ()(const Range& range) const override
{
float sigma3 = sigma*3;
const int D = 5;
for(int y = range.start; y < range.end; y++)
{
depthType* downRow = depthDown[y];
const depthType* srcCenterRow = depth[2*y];
for(int x = 0; x < depthDown.cols; x++)
{
depthType center = srcCenterRow[2*x];
int sx = max(0, 2*x - D/2), ex = min(2*x - D/2 + D, depth.cols-1);
int sy = max(0, 2*y - D/2), ey = min(2*y - D/2 + D, depth.rows-1);
depthType sum = 0;
int count = 0;
for(int iy = sy; iy < ey; iy++)
{
const depthType* srcRow = depth[iy];
for(int ix = sx; ix < ex; ix++)
{
depthType val = srcRow[ix];
if(abs(val - center) < sigma3)
{
sum += val; count ++;
}
}
}
downRow[x] = (count == 0) ? 0 : sum / count;
}
}
}
const Depth& depth;
Depth& depthDown;
float sigma;
};
Depth pyrDownBilateral(const Depth depth, float sigma)
{
CV_TRACE_FUNCTION();
Depth depthDown(depth.rows/2, depth.cols/2);
PyrDownBilateralInvoker pdi(depth, depthDown, sigma);
Range range(0, depthDown.rows);
const int nstripes = -1;
parallel_for_(range, pdi, nstripes);
return depthDown;
}
struct ComputePointsNormalsInvoker : ParallelLoopBody
{
ComputePointsNormalsInvoker(const Depth& _depth, Points& _points, Normals& _normals,
const Intr::Reprojector& _reproj, float _dfac) :
ParallelLoopBody(),
depth(_depth),
points(_points),
normals(_normals),
reproj(_reproj),
dfac(_dfac)
{ }
virtual void operator ()(const Range& range) const override
{
for(int y = range.start; y < range.end; y++)
{
const depthType* depthRow0 = depth[y];
const depthType* depthRow1 = (y < depth.rows - 1) ? depth[y + 1] : 0;
ptype *ptsRow = points[y];
ptype *normRow = normals[y];
for(int x = 0; x < depth.cols; x++)
{
depthType d00 = depthRow0[x];
depthType z00 = d00*dfac;
Point3f v00 = reproj(Point3f((float)x, (float)y, z00));
Point3f p = nan3, n = nan3;
if(x < depth.cols - 1 && y < depth.rows - 1)
{
depthType d01 = depthRow0[x+1];
depthType d10 = depthRow1[x];
depthType z01 = d01*dfac;
depthType z10 = d10*dfac;
// before it was
//if(z00*z01*z10 != 0)
if(z00 != 0 && z01 != 0 && z10 != 0)
{
Point3f v01 = reproj(Point3f((float)(x+1), (float)(y+0), z01));
Point3f v10 = reproj(Point3f((float)(x+0), (float)(y+1), z10));
cv::Vec3f vec = (v01-v00).cross(v10-v00);
n = -normalize(vec);
p = v00;
}
}
ptsRow[x] = toPtype(p);
normRow[x] = toPtype(n);
}
}
}
const Depth& depth;
Points& points;
Normals& normals;
const Intr::Reprojector& reproj;
float dfac;
};
void computePointsNormals(const Intr intr, float depthFactor, const Depth depth,
Points points, Normals normals)
{
CV_TRACE_FUNCTION();
CV_Assert(!points.empty() && !normals.empty());
CV_Assert(depth.size() == points.size());
CV_Assert(depth.size() == normals.size());
// conversion to meters
// before it was:
//float dfac = 0.001f/depthFactor;
float dfac = 1.f/depthFactor;
Intr::Reprojector reproj = intr.makeReprojector();
ComputePointsNormalsInvoker ci(depth, points, normals, reproj, dfac);
Range range(0, depth.rows);
const int nstripes = -1;
parallel_for_(range, ci, nstripes);
}
///////// GPU implementation /////////
#ifdef HAVE_OPENCL
static bool ocl_renderPointsNormals(const UMat points, const UMat normals, UMat image, Vec3f lightLoc);
static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
const Intr intr, int levels, float depthFactor,
float sigmaDepth, float sigmaSpatial, int kernelSize,
float truncateThreshold);
static bool ocl_buildPyramidPointsNormals(const UMat points, const UMat normals,
OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
int levels);
static bool computePointsNormalsGpu(const Intr intr, float depthFactor, const UMat& depth, UMat& points, UMat& normals);
static bool pyrDownBilateralGpu(const UMat& depth, UMat& depthDown, float sigma);
static bool customBilateralFilterGpu(const UMat src, UMat& dst, int kernelSize, float sigmaDepth, float sigmaSpatial);
static bool pyrDownPointsNormalsGpu(const UMat p, const UMat n, UMat &pdown, UMat &ndown);
bool computePointsNormalsGpu(const Intr intr, float depthFactor, const UMat& depth,
UMat& points, UMat& normals)
{
CV_TRACE_FUNCTION();
CV_Assert(!points.empty() && !normals.empty());
CV_Assert(depth.size() == points.size());
CV_Assert(depth.size() == normals.size());
CV_Assert(depth.type() == DEPTH_TYPE);
CV_Assert(points.type() == POINT_TYPE);
CV_Assert(normals.type() == POINT_TYPE);
// conversion to meters
float dfac = 1.f/depthFactor;
Intr::Reprojector reproj = intr.makeReprojector();
cv::String errorStr;
cv::String name = "computePointsNormals";
ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if(k.empty())
return false;
Vec2f fxyinv(reproj.fxinv, reproj.fyinv), cxy(reproj.cx, reproj.cy);
k.args(ocl::KernelArg::WriteOnlyNoSize(points),
ocl::KernelArg::WriteOnlyNoSize(normals),
ocl::KernelArg::ReadOnly(depth),
fxyinv.val,
cxy.val,
dfac);
size_t globalSize[2];
globalSize[0] = (size_t)depth.cols;
globalSize[1] = (size_t)depth.rows;
return k.run(2, globalSize, NULL, true);
}
bool pyrDownBilateralGpu(const UMat& depth, UMat& depthDown, float sigma)
{
CV_TRACE_FUNCTION();
depthDown.create(depth.rows/2, depth.cols/2, DEPTH_TYPE);
cv::String errorStr;
cv::String name = "pyrDownBilateral";
ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if(k.empty())
return false;
k.args(ocl::KernelArg::ReadOnly(depth),
ocl::KernelArg::WriteOnly(depthDown),
sigma);
size_t globalSize[2];
globalSize[0] = (size_t)depthDown.cols;
globalSize[1] = (size_t)depthDown.rows;
return k.run(2, globalSize, NULL, true);
}
//TODO: remove it when OpenCV's bilateral processes 32f on GPU
bool customBilateralFilterGpu(const UMat src /* udepth */, UMat& dst /* smooth */,
int kernelSize, float sigmaDepth, float sigmaSpatial)
{
CV_TRACE_FUNCTION();
Size frameSize = src.size();
CV_Assert(frameSize.area() > 0);
CV_Assert(src.type() == DEPTH_TYPE);
dst.create(frameSize, DEPTH_TYPE);
cv::String errorStr;
cv::String name = "customBilateral";
ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if(k.empty())
return false;
k.args(ocl::KernelArg::ReadOnlyNoSize(src),
ocl::KernelArg::WriteOnlyNoSize(dst),
frameSize,
kernelSize,
0.5f / (sigmaSpatial * sigmaSpatial),
0.5f / (sigmaDepth * sigmaDepth));
size_t globalSize[2];
globalSize[0] = (size_t)src.cols;
globalSize[1] = (size_t)src.rows;
return k.run(2, globalSize, NULL, true);
}
bool pyrDownPointsNormalsGpu(const UMat p, const UMat n, UMat &pdown, UMat &ndown)
{
CV_TRACE_FUNCTION();
cv::String errorStr;
cv::String name = "pyrDownPointsNormals";
ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if(k.empty())
return false;
Size downSize = pdown.size();
k.args(ocl::KernelArg::ReadOnlyNoSize(p),
ocl::KernelArg::ReadOnlyNoSize(n),
ocl::KernelArg::WriteOnlyNoSize(pdown),
ocl::KernelArg::WriteOnlyNoSize(ndown),
downSize);
size_t globalSize[2];
globalSize[0] = (size_t)pdown.cols;
globalSize[1] = (size_t)pdown.rows;
return k.run(2, globalSize, NULL, true);
}
static bool ocl_renderPointsNormals(const UMat points, const UMat normals,
UMat img, Vec3f lightLoc)
{
CV_TRACE_FUNCTION();
cv::String errorStr;
cv::String name = "render";
ocl::ProgramSource source = ocl::ptcloud::kinfu_frame_oclsrc;
cv::String options = "-cl-mad-enable";
ocl::Kernel k;
k.create(name.c_str(), source, options, &errorStr);
if(k.empty())
return false;
Vec4f lightPt(lightLoc[0], lightLoc[1], lightLoc[2]);
Size frameSize = points.size();
k.args(ocl::KernelArg::ReadOnlyNoSize(points),
ocl::KernelArg::ReadOnlyNoSize(normals),
ocl::KernelArg::WriteOnlyNoSize(img),
frameSize,
lightPt.val);
size_t globalSize[2];
globalSize[0] = (size_t)points.cols;
globalSize[1] = (size_t)points.rows;
return k.run(2, globalSize, NULL, true);
}
static bool ocl_makeFrameFromDepth(const UMat depth, OutputArrayOfArrays points, OutputArrayOfArrays normals,
const Intr intr, int levels, float depthFactor,
float sigmaDepth, float sigmaSpatial, int kernelSize,
float truncateThreshold)
{
CV_TRACE_FUNCTION();
// looks like OpenCV's bilateral filter works the same as KinFu's
UMat smooth;
//TODO: fix that
// until 32f isn't implemented in OpenCV in OpenCL, we should use our workarounds
//bilateralFilter(udepth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial);
if(!customBilateralFilterGpu(depth, smooth, kernelSize, sigmaDepth*depthFactor, sigmaSpatial))
return false;
// depth truncation can be used in some scenes
UMat depthThreshold;
if(truncateThreshold > 0.f)
threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV);
else
depthThreshold = smooth;
UMat scaled = depthThreshold;
Size sz = smooth.size();
points.create(levels, 1, POINT_TYPE);
normals.create(levels, 1, POINT_TYPE);
for(int i = 0; i < levels; i++)
{
UMat& p = points.getUMatRef(i);
UMat& n = normals.getUMatRef(i);
p.create(sz, POINT_TYPE);
n.create(sz, POINT_TYPE);
if(!computePointsNormalsGpu(intr.scale(i), depthFactor, scaled, p, n))
return false;
if(i < levels - 1)
{
sz.width /= 2, sz.height /= 2;
UMat halfDepth(sz, DEPTH_TYPE);
pyrDownBilateralGpu(scaled, halfDepth, sigmaDepth*depthFactor);
scaled = halfDepth;
}
}
return true;
}
static bool ocl_buildPyramidPointsNormals(const UMat points, const UMat normals,
OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
int levels)
{
CV_TRACE_FUNCTION();
pyrPoints .create(levels, 1, POINT_TYPE);
pyrNormals.create(levels, 1, POINT_TYPE);
pyrPoints .getUMatRef(0) = points;
pyrNormals.getUMatRef(0) = normals;
Size sz = points.size();
for(int i = 1; i < levels; i++)
{
UMat p1 = pyrPoints .getUMat(i-1);
UMat n1 = pyrNormals.getUMat(i-1);
sz.width /= 2; sz.height /= 2;
UMat& p0 = pyrPoints .getUMatRef(i);
UMat& n0 = pyrNormals.getUMatRef(i);
p0.create(sz, POINT_TYPE);
n0.create(sz, POINT_TYPE);
if(!pyrDownPointsNormalsGpu(p1, n1, p0, n0))
return false;
}
return true;
}
#endif
namespace detail {
void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Vec3f lightLoc)
{
CV_TRACE_FUNCTION();
CV_Assert(_points.size().area() > 0);
CV_Assert(_points.size() == _normals.size());
Size sz = _points.size();
image.create(sz, CV_8UC4);
CV_OCL_RUN(_points.isUMat() && _normals.isUMat() && image.isUMat(),
ocl_renderPointsNormals(_points.getUMat(),
_normals.getUMat(),
image.getUMat(), lightLoc))
Points points = _points.getMat();
Normals normals = _normals.getMat();
Mat_<Vec4b> img = image.getMat();
RenderInvoker ri(points, normals, img, lightLoc, sz);
Range range(0, sz.height);
const int nstripes = -1;
parallel_for_(range, ri, nstripes);
}
void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image)
{
CV_TRACE_FUNCTION();
CV_Assert(_points.size().area() > 0);
CV_Assert(_points.size() == _normals.size());
Size sz = _points.size();
image.create(sz, CV_8UC4);
Points points = _points.getMat();
Normals normals = _normals.getMat();
Colors colors = _colors.getMat();
Mat_<Vec4b> img = image.getMat();
RenderColorInvoker ri(points, colors, img, sz);
Range range(0, sz.height);
const int nstripes = -1;
parallel_for_(range, ri, nstripes);
}
} // namespace detail
void makeFrameFromDepth(InputArray _depth,
OutputArray pyrPoints, OutputArray pyrNormals,
const Matx33f _intr, int levels, float depthFactor,
float sigmaDepth, float sigmaSpatial, int kernelSize,
float truncateThreshold)
{
CV_TRACE_FUNCTION();
CV_Assert(_depth.type() == DEPTH_TYPE);
Intr intr(_intr);
CV_OCL_RUN(_depth.isUMat() && pyrPoints.isUMatVector() && pyrNormals.isUMatVector(),
ocl_makeFrameFromDepth(_depth.getUMat(), pyrPoints, pyrNormals,
intr, levels, depthFactor,
sigmaDepth, sigmaSpatial, kernelSize,
truncateThreshold));
int kp = pyrPoints.kind(), kn = pyrNormals.kind();
// There can be UMats in the container (when OpenCL is off)
CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT || kp == _InputArray::STD_VECTOR_UMAT);
CV_Assert(kn == _InputArray::STD_ARRAY_MAT || kn == _InputArray::STD_VECTOR_MAT || kp == _InputArray::STD_VECTOR_UMAT);
Depth depth = _depth.getMat();
// looks like OpenCV's bilateral filter works the same as KinFu's
Depth smooth;
Depth depthNoNans = depth.clone();
patchNaNs(depthNoNans);
bilateralFilter(depthNoNans, smooth, kernelSize, sigmaDepth * depthFactor, sigmaSpatial);
// depth truncation can be used in some scenes
Depth depthThreshold;
if(truncateThreshold > 0.f)
threshold(smooth, depthThreshold, truncateThreshold * depthFactor, 0.0, THRESH_TOZERO_INV);
else
depthThreshold = smooth;
// we don't need depth pyramid outside this method
// if we do, the code is to be refactored
Depth scaled = depthThreshold;
Size sz = smooth.size();
pyrPoints.create(levels, 1, POINT_TYPE);
pyrNormals.create(levels, 1, POINT_TYPE);
for(int i = 0; i < levels; i++)
{
pyrPoints .create(sz, POINT_TYPE, i);
pyrNormals.create(sz, POINT_TYPE, i);
// There can be UMats in the container (when OpenCL is off),
// getMatRef() is not applicable
Points p = pyrPoints. getMat(i);
Normals n = pyrNormals.getMat(i);
computePointsNormals(intr.scale(i), depthFactor, scaled, p, n);
if(i < levels - 1)
{
sz.width /= 2; sz.height /= 2;
scaled = pyrDownBilateral(scaled, sigmaDepth*depthFactor);
}
}
}
void buildPyramidPointsNormals(InputArray _points, InputArray _normals,
OutputArrayOfArrays pyrPoints, OutputArrayOfArrays pyrNormals,
int levels)
{
CV_TRACE_FUNCTION();
CV_Assert(_points.type() == POINT_TYPE);
CV_Assert(_points.type() == _normals.type());
CV_Assert(_points.size() == _normals.size());
CV_OCL_RUN(_points.isUMat() && _normals.isUMat() &&
pyrPoints.isUMatVector() && pyrNormals.isUMatVector(),
ocl_buildPyramidPointsNormals(_points.getUMat(), _normals.getUMat(),
pyrPoints, pyrNormals,
levels));
int kp = pyrPoints.kind(), kn = pyrNormals.kind();
CV_Assert(kp == _InputArray::STD_ARRAY_MAT || kp == _InputArray::STD_VECTOR_MAT);
CV_Assert(kn == _InputArray::STD_ARRAY_MAT || kn == _InputArray::STD_VECTOR_MAT);
Mat p0 = _points.getMat(), n0 = _normals.getMat();
pyrPoints .create(levels, 1, POINT_TYPE);
pyrNormals.create(levels, 1, POINT_TYPE);
pyrPoints .getMatRef(0) = p0;
pyrNormals.getMatRef(0) = n0;
Size sz = _points.size();
for(int i = 1; i < levels; i++)
{
Points p1 = pyrPoints .getMat(i-1);
Normals n1 = pyrNormals.getMat(i-1);
sz.width /= 2; sz.height /= 2;
pyrPoints .create(sz, POINT_TYPE, i);
pyrNormals.create(sz, POINT_TYPE, i);
Points pd = pyrPoints. getMatRef(i);
Normals nd = pyrNormals.getMatRef(i);
pyrDownPointsNormals(p1, n1, pd, nd);
}
}
} // namespace cv