mirror of
https://github.com/opencv/opencv.git
synced 2026-09-25 04:09:57 +03:00
Alexander Smorkalov
fc3803c67b
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
1449 lines
52 KiB
C++
1449 lines
52 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 "tsdf_functions.hpp"
|
|
#include "opencl_kernels_ptcloud.hpp"
|
|
|
|
namespace cv {
|
|
|
|
void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
|
|
Point2f fl(intrinsics.fx, intrinsics.fy);
|
|
Point2f pp(intrinsics.cx, intrinsics.cy);
|
|
pixNorm = Mat(size.height, size.width, CV_32F);
|
|
std::vector<float> x(size.width);
|
|
std::vector<float> y(size.height);
|
|
for (int i = 0; i < size.width; i++)
|
|
x[i] = (i - pp.x) / fl.x;
|
|
for (int i = 0; i < size.height; i++)
|
|
y[i] = (i - pp.y) / fl.y;
|
|
|
|
for (int i = 0; i < size.height; i++)
|
|
{
|
|
for (int j = 0; j < size.width; j++)
|
|
{
|
|
pixNorm.at<float>(i, j) = sqrtf(x[j] * x[j] + y[i] * y[i] + 1.0f);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
#ifdef HAVE_OPENCL
|
|
void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm)
|
|
{
|
|
// calculating this on CPU then uploading to GPU is faster than calculating this on GPU
|
|
Mat cpuPixNorm;
|
|
preCalculationPixNorm(size, intrinsics, cpuPixNorm);
|
|
cpuPixNorm.copyTo(pixNorm);
|
|
|
|
}
|
|
#endif
|
|
|
|
// Integrate
|
|
|
|
void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
|
|
InputArray _depth, InputArray _pixNorms, InputArray _volume)
|
|
{
|
|
Matx44f volumePose;
|
|
settings.getVolumePose(volumePose);
|
|
integrateTsdfVolumeUnit(settings, volumePose, cameraPose, _depth, _pixNorms, _volume);
|
|
}
|
|
|
|
|
|
void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
|
|
InputArray _depth, InputArray _pixNorms, InputArray _volume)
|
|
{
|
|
Depth depth = _depth.getMat();
|
|
Mat volume = _volume.getMat();
|
|
Mat pixNorms = _pixNorms.getMat();
|
|
|
|
TsdfVoxel* volDataStart = volume.ptr<TsdfVoxel>();
|
|
|
|
Vec4i volStrides;
|
|
settings.getVolumeStrides(volStrides);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
float voxelSize = settings.getVoxelSize();
|
|
|
|
const Affine3f pose = Affine3f(volumePose);
|
|
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
|
|
|
|
Matx33f intr;
|
|
settings.getCameraIntegrateIntrinsics(intr);
|
|
const Intr::Projector proj = Intr(intr).makeProjector();
|
|
const float dfac(1.f / settings.getDepthFactor());
|
|
const float truncDist = settings.getTsdfTruncateDistance();
|
|
const float truncDistInv = 1.f / truncDist;
|
|
const int maxWeight = settings.getMaxWeight();
|
|
|
|
Range integrateRange(0, volResolution.x);
|
|
|
|
#if USE_INTRINSICS
|
|
auto IntegrateInvoker = [&](const Range& range)
|
|
{
|
|
// zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt;
|
|
Point3f zStepPt = Point3f(vol2cam.matrix(0, 2),
|
|
vol2cam.matrix(1, 2),
|
|
vol2cam.matrix(2, 2)) * voxelSize;
|
|
|
|
v_float32x4 zStep(zStepPt.x, zStepPt.y, zStepPt.z, 0);
|
|
v_float32x4 vfxy(proj.fx, proj.fy, 0.f, 0.f), vcxy(proj.cx, proj.cy, 0.f, 0.f);
|
|
const v_float32x4 upLimits = v_cvt_f32(v_int32x4(depth.cols - 1, depth.rows - 1, 0, 0));
|
|
|
|
for (int x = range.start; x < range.end; x++)
|
|
{
|
|
TsdfVoxel* volDataX = volDataStart + x * volStrides[0];
|
|
for (int y = 0; y < volResolution.y; y++)
|
|
{
|
|
TsdfVoxel* volDataY = volDataX + y * volStrides[1];
|
|
// optimization of camSpace transformation (vector addition instead of matmul at each z)
|
|
Point3f basePt = vol2cam * (Point3f((float)x, (float)y, 0) * voxelSize);
|
|
v_float32x4 camSpacePt(basePt.x, basePt.y, basePt.z, 0);
|
|
|
|
int startZ, endZ;
|
|
if (abs(zStepPt.z) > 1e-5)
|
|
{
|
|
int baseZ = (int)(-basePt.z / zStepPt.z);
|
|
if (zStepPt.z > 0)
|
|
{
|
|
startZ = baseZ;
|
|
endZ = volResolution.z;
|
|
}
|
|
else
|
|
{
|
|
startZ = 0;
|
|
endZ = baseZ;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (basePt.z > 0)
|
|
{
|
|
startZ = 0;
|
|
endZ = volResolution.z;
|
|
}
|
|
else
|
|
{
|
|
// z loop shouldn't be performed
|
|
startZ = endZ = 0;
|
|
}
|
|
}
|
|
startZ = max(0, startZ);
|
|
endZ = min(int(volResolution.z), endZ);
|
|
for (int z = startZ; z < endZ; z++)
|
|
{
|
|
// optimization of the following:
|
|
//Point3f volPt = Point3f(x, y, z)*voxelSize;
|
|
//Point3f camSpacePt = vol2cam * volPt;
|
|
camSpacePt = v_add(camSpacePt, zStep);
|
|
|
|
float zCamSpace = v_get0(v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(camSpacePt))));
|
|
if (zCamSpace <= 0.f)
|
|
continue;
|
|
|
|
v_float32x4 camPixVec = v_div(camSpacePt, v_setall_f32(zCamSpace));
|
|
v_float32x4 projected = v_muladd(camPixVec, vfxy, vcxy);
|
|
// leave only first 2 lanes
|
|
projected = v_reinterpret_as_f32(v_and(v_reinterpret_as_u32(projected),
|
|
v_uint32x4(0xFFFFFFFF, 0xFFFFFFFF, 0, 0)));
|
|
|
|
depthType v;
|
|
// bilinearly interpolate depth at projected
|
|
{
|
|
const v_float32x4& pt = projected;
|
|
// check coords >= 0 and < imgSize
|
|
v_uint32x4 limits = v_or(v_reinterpret_as_u32(v_lt(pt, v_setzero_f32())),
|
|
v_reinterpret_as_u32(v_ge(pt, upLimits)));
|
|
limits = v_or(limits, v_rotate_right<1>(limits));
|
|
if (v_get0(limits))
|
|
continue;
|
|
|
|
// xi, yi = floor(pt)
|
|
v_int32x4 ip = v_floor(pt);
|
|
v_int32x4 ipshift = ip;
|
|
int xi = v_get0(ipshift);
|
|
ipshift = v_rotate_right<1>(ipshift);
|
|
int yi = v_get0(ipshift);
|
|
|
|
const depthType* row0 = depth[yi + 0];
|
|
const depthType* row1 = depth[yi + 1];
|
|
|
|
// v001 = [v(xi + 0, yi + 0), v(xi + 1, yi + 0)]
|
|
v_float32x4 v001 = v_load_low(row0 + xi);
|
|
// v101 = [v(xi + 0, yi + 1), v(xi + 1, yi + 1)]
|
|
v_float32x4 v101 = v_load_low(row1 + xi);
|
|
|
|
v_float32x4 vall = v_combine_low(v001, v101);
|
|
|
|
// assume correct depth is positive
|
|
// don't fix missing data
|
|
if (v_check_all(v_gt(vall, v_setzero_f32())))
|
|
{
|
|
v_float32x4 t = v_sub(pt, v_cvt_f32(ip));
|
|
float tx = v_get0(t);
|
|
t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t)));
|
|
v_float32x4 ty = v_setall_f32(v_get0(t));
|
|
// vx is y-interpolated between rows 0 and 1
|
|
v_float32x4 vx = v_add(v001, v_mul(ty, v_sub(v101, v001)));
|
|
float v0 = v_get0(vx);
|
|
vx = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vx)));
|
|
float v1 = v_get0(vx);
|
|
v = v0 + tx * (v1 - v0);
|
|
}
|
|
else
|
|
continue;
|
|
}
|
|
|
|
// norm(camPixVec) produces double which is too slow
|
|
int _u = (int)v_get0(projected);
|
|
int _v = (int)v_get0(v_rotate_right<1>(projected));
|
|
if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows))
|
|
continue;
|
|
float pixNorm = pixNorms.at<float>(_v, _u);
|
|
// float pixNorm = sqrt(v_reduce_sum(camPixVec*camPixVec));
|
|
// difference between distances of point and of surface to camera
|
|
float sdf = pixNorm * (v * dfac - zCamSpace);
|
|
// possible alternative is:
|
|
// kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1);
|
|
|
|
if (sdf >= -truncDist)
|
|
{
|
|
TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv));
|
|
|
|
TsdfVoxel& voxel = volDataY[z * volStrides[2]];
|
|
WeightType& weight = voxel.weight;
|
|
TsdfType& value = voxel.tsdf;
|
|
|
|
// update TSDF
|
|
value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1));
|
|
weight = (weight + 1) < maxWeight ? (weight + 1) : (WeightType)maxWeight;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
#else
|
|
auto IntegrateInvoker = [&](const Range& range)
|
|
{
|
|
for (int x = range.start; x < range.end; x++)
|
|
{
|
|
TsdfVoxel* volDataX = volDataStart + x * volStrides[0];
|
|
for (int y = 0; y < volResolution.y; y++)
|
|
{
|
|
TsdfVoxel* volDataY = volDataX + y * volStrides[1];
|
|
// optimization of camSpace transformation (vector addition instead of matmul at each z)
|
|
Point3f basePt = vol2cam * (Point3f(float(x), float(y), 0.0f) * voxelSize);
|
|
Point3f camSpacePt = basePt;
|
|
// zStep == vol2cam*(Point3f(x, y, 1)*voxelSize) - basePt;
|
|
// zStep == vol2cam*[Point3f(x, y, 1) - Point3f(x, y, 0)]*voxelSize
|
|
Point3f zStep = Point3f(vol2cam.matrix(0, 2),
|
|
vol2cam.matrix(1, 2),
|
|
vol2cam.matrix(2, 2)) * voxelSize;
|
|
int startZ, endZ;
|
|
if (abs(zStep.z) > 1e-5)
|
|
{
|
|
int baseZ = int(-basePt.z / zStep.z);
|
|
if (zStep.z > 0)
|
|
{
|
|
startZ = baseZ;
|
|
endZ = volResolution.z;
|
|
}
|
|
else
|
|
{
|
|
startZ = 0;
|
|
endZ = baseZ;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (basePt.z > 0)
|
|
{
|
|
startZ = 0;
|
|
endZ = volResolution.z;
|
|
}
|
|
else
|
|
{
|
|
// z loop shouldn't be performed
|
|
startZ = endZ = 0;
|
|
}
|
|
}
|
|
startZ = max(0, startZ);
|
|
endZ = min(int(volResolution.z), endZ);
|
|
|
|
for (int z = startZ; z < endZ; z++)
|
|
{
|
|
// optimization of the following:
|
|
//Point3f volPt = Point3f(x, y, z)*volume.voxelSize;
|
|
//Point3f camSpacePt = vol2cam * volPt;
|
|
|
|
camSpacePt += zStep;
|
|
if (camSpacePt.z <= 0)
|
|
continue;
|
|
|
|
Point3f camPixVec;
|
|
Point2f projected = proj(camSpacePt, camPixVec);
|
|
|
|
depthType v = bilinearDepth(depth, projected);
|
|
if (v == 0) {
|
|
continue;
|
|
}
|
|
int _u = (int)projected.x;
|
|
int _v = (int)projected.y;
|
|
if (!(_u >= 0 && _u < depth.cols && _v >= 0 && _v < depth.rows))
|
|
continue;
|
|
|
|
float pixNorm = pixNorms.at<float>(_v, _u);
|
|
|
|
// difference between distances of point and of surface to camera
|
|
float sdf = pixNorm * (v * dfac - camSpacePt.z);
|
|
// possible alternative is:
|
|
// kftype sdf = norm(camSpacePt)*(v*dfac/camSpacePt.z - 1);
|
|
if (sdf >= -truncDist)
|
|
{
|
|
TsdfType tsdf = floatToTsdf(fmin(1.f, sdf * truncDistInv));
|
|
|
|
TsdfVoxel& voxel = volDataY[z * volStrides[2]];
|
|
WeightType& weight = voxel.weight;
|
|
TsdfType& value = voxel.tsdf;
|
|
|
|
// update TSDF
|
|
value = floatToTsdf((tsdfToFloat(value) * weight + tsdfToFloat(tsdf)) / (weight + 1));
|
|
weight = (WeightType)min(weight + 1, maxWeight);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
#endif
|
|
parallel_for_(integrateRange, IntegrateInvoker);
|
|
//IntegrateInvoker(integrateRange);
|
|
}
|
|
|
|
#ifdef HAVE_OPENCL
|
|
void ocl_integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
|
|
InputArray _depth, InputArray _pixNorms, InputArray _volume)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
CV_Assert(!_depth.empty());
|
|
|
|
UMat depth = _depth.getUMat();
|
|
UMat volume = _volume.getUMat();
|
|
UMat pixNorms = _pixNorms.getUMat();
|
|
|
|
String errorStr;
|
|
String name = "integrate";
|
|
ocl::ProgramSource source = ocl::ptcloud::tsdf_oclsrc;
|
|
String options = "-cl-mad-enable";
|
|
ocl::Kernel k;
|
|
k.create(name.c_str(), source, options, &errorStr);
|
|
|
|
if (k.empty())
|
|
throw std::runtime_error("Failed to create kernel: " + errorStr);
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
UMat vol2camGpu;
|
|
Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
|
|
Mat(vol2cam.matrix).copyTo(vol2camGpu);
|
|
|
|
float dfac = 1.f / settings.getDepthFactor();
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z);
|
|
Matx33f intr;
|
|
settings.getCameraIntegrateIntrinsics(intr);
|
|
Intr intrinsics(intr);
|
|
Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy);
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
|
|
const float voxelSize = settings.getVoxelSize();
|
|
const float truncatedDistance = settings.getTsdfTruncateDistance();
|
|
const int maxWeight = settings.getMaxWeight();
|
|
|
|
// TODO: optimization possible
|
|
// Use sampler for depth (mask needed)
|
|
k.args(ocl::KernelArg::ReadOnly(depth),
|
|
ocl::KernelArg::PtrReadWrite(volume),
|
|
ocl::KernelArg::PtrReadOnly(vol2camGpu),
|
|
voxelSize,
|
|
volResGpu.val,
|
|
volDims.val,
|
|
fxy.val,
|
|
cxy.val,
|
|
dfac,
|
|
truncatedDistance,
|
|
maxWeight,
|
|
ocl::KernelArg::PtrReadOnly(pixNorms));
|
|
|
|
size_t globalSize[2];
|
|
globalSize[0] = (size_t)volResolution.x;
|
|
globalSize[1] = (size_t)volResolution.y;
|
|
|
|
if (!k.run(2, globalSize, NULL, true))
|
|
throw std::runtime_error("Failed to run kernel");
|
|
|
|
}
|
|
#endif
|
|
|
|
|
|
// Raycast
|
|
|
|
#if USE_INTRINSICS
|
|
// all coordinate checks should be done in inclosing cycle
|
|
inline float interpolateTsdfVoxel(const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords,
|
|
const v_float32x4& p)
|
|
{
|
|
// tx, ty, tz = floor(p)
|
|
v_int32x4 ip = v_floor(p);
|
|
v_float32x4 t = v_sub(p, v_cvt_f32(ip));
|
|
float tx = v_get0(t);
|
|
t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t)));
|
|
float ty = v_get0(t);
|
|
t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t)));
|
|
float tz = v_get0(t);
|
|
|
|
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
|
|
const TsdfVoxel* volData = volume.ptr<TsdfVoxel>();
|
|
|
|
int ix = v_get0(ip);
|
|
ip = v_rotate_right<1>(ip);
|
|
int iy = v_get0(ip);
|
|
ip = v_rotate_right<1>(ip);
|
|
int iz = v_get0(ip);
|
|
|
|
int coordBase = ix * xdim + iy * ydim + iz * zdim;
|
|
|
|
TsdfType vx[8];
|
|
for (int i = 0; i < 8; i++)
|
|
vx[i] = volData[neighbourCoords[i] + coordBase].tsdf;
|
|
|
|
v_float32x4 v0246 = tsdfToFloat_INTR(v_int32x4(vx[0], vx[2], vx[4], vx[6]));
|
|
v_float32x4 v1357 = tsdfToFloat_INTR(v_int32x4(vx[1], vx[3], vx[5], vx[7]));
|
|
v_float32x4 vxx = v_add(v0246, v_mul(v_setall_f32(tz), v_sub(v1357, v0246)));
|
|
|
|
v_float32x4 v00_10 = vxx;
|
|
v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx)));
|
|
|
|
v_float32x4 v0_1 = v_add(v00_10, v_mul(v_setall_f32(ty), v_sub(v01_11, v00_10)));
|
|
float v0 = v_get0(v0_1);
|
|
v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1)));
|
|
float v1 = v_get0(v0_1);
|
|
|
|
return v0 + tx * (v1 - v0);
|
|
}
|
|
|
|
inline float interpolateTsdfVoxel( const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords,
|
|
const Point3f& _p)
|
|
{
|
|
v_float32x4 p(_p.x, _p.y, _p.z, 0);
|
|
return interpolateTsdfVoxel(volume, volDims, neighbourCoords, p);
|
|
}
|
|
|
|
#else
|
|
inline float interpolateTsdfVoxel( const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords,
|
|
const Point3f& p)
|
|
{
|
|
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
|
|
|
|
int ix = cvFloor(p.x);
|
|
int iy = cvFloor(p.y);
|
|
int iz = cvFloor(p.z);
|
|
|
|
float tx = p.x - ix;
|
|
float ty = p.y - iy;
|
|
float tz = p.z - iz;
|
|
|
|
int coordBase = ix * xdim + iy * ydim + iz * zdim;
|
|
const TsdfVoxel* volData = volume.ptr<TsdfVoxel>();
|
|
|
|
float vx[8];
|
|
for (int i = 0; i < 8; i++)
|
|
vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase].tsdf);
|
|
|
|
float v00 = vx[0] + tz * (vx[1] - vx[0]);
|
|
float v01 = vx[2] + tz * (vx[3] - vx[2]);
|
|
float v10 = vx[4] + tz * (vx[5] - vx[4]);
|
|
float v11 = vx[6] + tz * (vx[7] - vx[6]);
|
|
|
|
float v0 = v00 + ty * (v01 - v00);
|
|
float v1 = v10 + ty * (v11 - v10);
|
|
|
|
return v0 + tx * (v1 - v0);
|
|
|
|
}
|
|
#endif
|
|
|
|
|
|
#if USE_INTRINSICS
|
|
//gradientDeltaFactor is fixed at 1.0 of voxel size
|
|
inline v_float32x4 getNormalVoxel( const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
|
|
const v_float32x4& p)
|
|
{
|
|
if (v_check_any(v_lt(p, v_float32x4(1.f, 1.f, 1.f, 0.f))) ||
|
|
v_check_any(v_ge(p, v_float32x4((float)(volResolution.x - 2),
|
|
(float)(volResolution.y - 2),
|
|
(float)(volResolution.z - 2), 1.f)))
|
|
)
|
|
return nanv;
|
|
|
|
v_int32x4 ip = v_floor(p);
|
|
v_float32x4 t = v_sub(p, v_cvt_f32(ip));
|
|
float tx = v_get0(t);
|
|
t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t)));
|
|
float ty = v_get0(t);
|
|
t = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(t)));
|
|
float tz = v_get0(t);
|
|
|
|
const int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
|
|
const TsdfVoxel* volData = volume.ptr<TsdfVoxel>();
|
|
|
|
int ix = v_get0(ip); ip = v_rotate_right<1>(ip);
|
|
int iy = v_get0(ip); ip = v_rotate_right<1>(ip);
|
|
int iz = v_get0(ip);
|
|
|
|
int coordBase = ix * xdim + iy * ydim + iz * zdim;
|
|
|
|
float CV_DECL_ALIGNED(16) an[4];
|
|
an[0] = an[1] = an[2] = an[3] = 0.f;
|
|
for (int c = 0; c < 3; c++)
|
|
{
|
|
const int dim = volDims[c];
|
|
float& nv = an[c];
|
|
|
|
float vx[8];
|
|
for (int i = 0; i < 8; i++)
|
|
vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) -
|
|
tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf);
|
|
|
|
v_float32x4 v0246(vx[0], vx[2], vx[4], vx[6]);
|
|
v_float32x4 v1357(vx[1], vx[3], vx[5], vx[7]);
|
|
v_float32x4 vxx = v_add(v0246, v_mul(v_setall_f32(tz), v_sub(v1357, v0246)));
|
|
|
|
v_float32x4 v00_10 = vxx;
|
|
v_float32x4 v01_11 = v_reinterpret_as_f32(v_rotate_right<1>(v_reinterpret_as_u32(vxx)));
|
|
|
|
v_float32x4 v0_1 = v_add(v00_10, v_mul(v_setall_f32(ty), v_sub(v01_11, v00_10)));
|
|
float v0 = v_get0(v0_1);
|
|
v0_1 = v_reinterpret_as_f32(v_rotate_right<2>(v_reinterpret_as_u32(v0_1)));
|
|
float v1 = v_get0(v0_1);
|
|
|
|
nv = v0 + tx * (v1 - v0);
|
|
}
|
|
|
|
v_float32x4 n = v_load_aligned(an);
|
|
v_float32x4 Norm = v_sqrt(v_setall_f32(v_reduce_sum(v_mul(n, n))));
|
|
|
|
return v_get0(Norm) < 0.0001f ? nanv : v_div(n, Norm);
|
|
}
|
|
|
|
inline Point3f getNormalVoxel( const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
|
|
const Point3f& _p)
|
|
{
|
|
v_float32x4 p(_p.x, _p.y, _p.z, 0.f);
|
|
v_float32x4 result = getNormalVoxel(volume, volDims, neighbourCoords, volResolution, p);
|
|
float CV_DECL_ALIGNED(16) ares[4];
|
|
v_store_aligned(ares, result);
|
|
return Point3f(ares[0], ares[1], ares[2]);
|
|
}
|
|
#else
|
|
inline Point3f getNormalVoxel( const Mat& volume,
|
|
const Vec4i& volDims, const Vec8i& neighbourCoords, const Point3i volResolution,
|
|
const Point3f& p)
|
|
{
|
|
int xdim = volDims[0], ydim = volDims[1], zdim = volDims[2];
|
|
const TsdfVoxel* volData = volume.ptr<TsdfVoxel>();
|
|
|
|
if (p.x < 1 || p.x >= volResolution.x - 2 ||
|
|
p.y < 1 || p.y >= volResolution.y - 2 ||
|
|
p.z < 1 || p.z >= volResolution.z - 2)
|
|
return nan3;
|
|
|
|
int ix = cvFloor(p.x);
|
|
int iy = cvFloor(p.y);
|
|
int iz = cvFloor(p.z);
|
|
|
|
float tx = p.x - ix;
|
|
float ty = p.y - iy;
|
|
float tz = p.z - iz;
|
|
|
|
int coordBase = ix * xdim + iy * ydim + iz * zdim;
|
|
|
|
Vec3f an;
|
|
for (int c = 0; c < 3; c++)
|
|
{
|
|
const int dim = volDims[c];
|
|
float& nv = an[c];
|
|
|
|
float vx[8];
|
|
for (int i = 0; i < 8; i++)
|
|
vx[i] = tsdfToFloat(volData[neighbourCoords[i] + coordBase + 1 * dim].tsdf) -
|
|
tsdfToFloat(volData[neighbourCoords[i] + coordBase - 1 * dim].tsdf);
|
|
|
|
float v00 = vx[0] + tz * (vx[1] - vx[0]);
|
|
float v01 = vx[2] + tz * (vx[3] - vx[2]);
|
|
float v10 = vx[4] + tz * (vx[5] - vx[4]);
|
|
float v11 = vx[6] + tz * (vx[7] - vx[6]);
|
|
|
|
float v0 = v00 + ty * (v01 - v00);
|
|
float v1 = v10 + ty * (v11 - v10);
|
|
|
|
nv = v0 + tx * (v1 - v0);
|
|
}
|
|
|
|
float nv = sqrt(an[0] * an[0] +
|
|
an[1] * an[1] +
|
|
an[2] * an[2]);
|
|
return nv < 0.0001f ? nan3 : an / nv;
|
|
}
|
|
#endif
|
|
|
|
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
|
|
int height, int width, InputArray intr,
|
|
InputArray _volume, OutputArray _points, OutputArray _normals)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
|
|
const Size frameSize(width, height);
|
|
CV_Assert(frameSize.area() > 0);
|
|
|
|
Matx33f mintr(intr.getMat());
|
|
|
|
_points.create(frameSize, POINT_TYPE);
|
|
_normals.create(frameSize, POINT_TYPE);
|
|
|
|
Points points = _points.getMat();
|
|
Normals normals = _normals.getMat();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
|
|
const Point3f boxMax(volSize - Point3f(settings.getVoxelSize(), settings.getVoxelSize(), settings.getVoxelSize()));
|
|
const Point3f boxMin = Point3f(0, 0, 0);
|
|
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
|
|
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
|
|
|
|
const Mat volume = _volume.getMat();
|
|
float voxelSize = settings.getVoxelSize();
|
|
float voxelSizeInv = 1.0f / voxelSize;
|
|
const Intr::Reprojector reproj = Intr(mintr).makeReprojector();
|
|
float tstep = settings.getTsdfTruncateDistance() * settings.getRaycastStepFactor();
|
|
|
|
Range raycastRange = Range(0, points.rows);
|
|
//TODO:: swap realization, they are missplaced :)
|
|
#if USE_INTRINSICS
|
|
auto RaycastInvoker = [&](const Range& range)
|
|
{
|
|
const v_float32x4 vfxy(reproj.fxinv, reproj.fyinv, 0, 0);
|
|
const v_float32x4 vcxy(reproj.cx, reproj.cy, 0, 0);
|
|
|
|
const float(&cm)[16] = cam2vol.matrix.val;
|
|
const v_float32x4 camRot0(cm[0], cm[4], cm[8], 0);
|
|
const v_float32x4 camRot1(cm[1], cm[5], cm[9], 0);
|
|
const v_float32x4 camRot2(cm[2], cm[6], cm[10], 0);
|
|
const v_float32x4 camTrans(cm[3], cm[7], cm[11], 0);
|
|
|
|
const v_float32x4 boxDown(boxMin.x, boxMin.y, boxMin.z, 0.f);
|
|
const v_float32x4 boxUp(boxMax.x, boxMax.y, boxMax.z, 0.f);
|
|
|
|
const v_float32x4 invVoxelSize = v_float32x4(voxelSizeInv, voxelSizeInv, voxelSizeInv, 1.f);
|
|
|
|
const float(&vm)[16] = vol2cam.matrix.val;
|
|
const v_float32x4 volRot0(vm[0], vm[4], vm[8], 0);
|
|
const v_float32x4 volRot1(vm[1], vm[5], vm[9], 0);
|
|
const v_float32x4 volRot2(vm[2], vm[6], vm[10], 0);
|
|
const v_float32x4 volTrans(vm[3], vm[7], vm[11], 0);
|
|
|
|
for (int y = range.start; y < range.end; y++)
|
|
{
|
|
ptype* ptsRow = points[y];
|
|
ptype* nrmRow = normals[y];
|
|
|
|
for (int x = 0; x < points.cols; x++)
|
|
{
|
|
v_float32x4 point = nanv, normal = nanv;
|
|
|
|
v_float32x4 orig = camTrans;
|
|
|
|
// get direction through pixel in volume space:
|
|
|
|
// 1. reproject (x, y) on projecting plane where z = 1.f
|
|
v_float32x4 planed = v_mul(v_sub(v_float32x4((float)x, (float)y, 0.f, 0.f), vcxy), vfxy);
|
|
planed = v_combine_low(planed, v_float32x4(1.f, 0.f, 0.f, 0.f));
|
|
|
|
// 2. rotate to volume space
|
|
planed = v_matmuladd(planed, camRot0, camRot1, camRot2, v_setzero_f32());
|
|
|
|
// 3. normalize
|
|
v_float32x4 invNorm = v_invsqrt(v_setall_f32(v_reduce_sum(v_mul(planed, planed))));
|
|
v_float32x4 dir = v_mul(planed, invNorm);
|
|
|
|
// compute intersection of ray with all six bbox planes
|
|
v_float32x4 rayinv = v_div(v_setall_f32(1.f), dir);
|
|
// div by zero should be eliminated by these products
|
|
v_float32x4 tbottom = v_mul(rayinv, v_sub(boxDown, orig));
|
|
v_float32x4 ttop = v_mul(rayinv, v_sub(boxUp, orig));
|
|
|
|
// re-order intersections to find smallest and largest on each axis
|
|
v_float32x4 minAx = v_min(ttop, tbottom);
|
|
v_float32x4 maxAx = v_max(ttop, tbottom);
|
|
|
|
// near clipping plane
|
|
const float clip = 0.f;
|
|
float _minAx[4], _maxAx[4];
|
|
v_store(_minAx, minAx);
|
|
v_store(_maxAx, maxAx);
|
|
float tmin = max({ _minAx[0], _minAx[1], _minAx[2], clip });
|
|
float tmax = min({ _maxAx[0], _maxAx[1], _maxAx[2] });
|
|
|
|
// precautions against getting coordinates out of bounds
|
|
tmin = tmin + tstep;
|
|
tmax = tmax - tstep;
|
|
|
|
if (tmin < tmax)
|
|
{
|
|
// interpolation optimized a little
|
|
orig = v_mul(orig, invVoxelSize);
|
|
dir = v_mul(dir, invVoxelSize);
|
|
|
|
int xdim = volDims[0];
|
|
int ydim = volDims[1];
|
|
int zdim = volDims[2];
|
|
v_float32x4 rayStep = v_mul(dir, v_setall_f32(tstep));
|
|
v_float32x4 next = v_add(orig, v_mul(dir, v_setall_f32(tmin)));
|
|
float f = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
float fnext = f;
|
|
|
|
//raymarch
|
|
int steps = 0;
|
|
int nSteps = cvFloor((tmax - tmin) / tstep);
|
|
for (; steps < nSteps; steps++)
|
|
{
|
|
next = v_add(next, rayStep);
|
|
v_int32x4 ip = v_round(next);
|
|
int ix = v_get0(ip); ip = v_rotate_right<1>(ip);
|
|
int iy = v_get0(ip); ip = v_rotate_right<1>(ip);
|
|
int iz = v_get0(ip);
|
|
int coord = ix * xdim + iy * ydim + iz * zdim;
|
|
|
|
fnext = tsdfToFloat(volume.at<TsdfVoxel>(coord).tsdf);
|
|
if (fnext != f)
|
|
{
|
|
fnext = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
|
|
// when ray crosses a surface
|
|
if (std::signbit(f) != std::signbit(fnext))
|
|
break;
|
|
|
|
f = fnext;
|
|
}
|
|
}
|
|
|
|
// if ray penetrates a surface from outside
|
|
// linearly interpolate t between two f values
|
|
if (f > 0.f && fnext < 0.f)
|
|
{
|
|
v_float32x4 tp = v_sub(next, rayStep);
|
|
float ft = interpolateTsdfVoxel(volume, volDims, neighbourCoords, tp);
|
|
float ftdt = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
float ts = tmin + tstep * (steps - ft / (ftdt - ft));
|
|
|
|
// avoid division by zero
|
|
if (!cvIsNaN(ts) && !cvIsInf(ts))
|
|
{
|
|
v_float32x4 pv = v_add(orig, v_mul(dir, v_setall_f32(ts)));
|
|
v_float32x4 nv = getNormalVoxel(volume, volDims, neighbourCoords, volResolution, pv);
|
|
|
|
if (!isNaN(nv))
|
|
{
|
|
//convert pv and nv to camera space
|
|
normal = v_matmuladd(nv, volRot0, volRot1, volRot2, v_setzero_f32());
|
|
// interpolation optimized a little
|
|
point = v_matmuladd(v_mul(pv, v_float32x4(voxelSize, voxelSize, voxelSize, 1.f)),
|
|
volRot0, volRot1, volRot2, volTrans);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
v_store((float*)(&ptsRow[x]), point);
|
|
v_store((float*)(&nrmRow[x]), normal);
|
|
}
|
|
}
|
|
};
|
|
#else
|
|
auto RaycastInvoker = [&](const Range& range)
|
|
{
|
|
const Point3f camTrans = cam2vol.translation();
|
|
const Matx33f camRot = cam2vol.rotation();
|
|
const Matx33f volRot = vol2cam.rotation();
|
|
|
|
for (int y = range.start; y < range.end; y++)
|
|
{
|
|
ptype* ptsRow = points[y];
|
|
ptype* nrmRow = normals[y];
|
|
|
|
for (int x = 0; x < points.cols; x++)
|
|
{
|
|
Point3f point = nan3, normal = nan3;
|
|
|
|
Point3f orig = camTrans;
|
|
// direction through pixel in volume space
|
|
Point3f dir = normalize(Vec3f(camRot * reproj(Point3f(float(x), float(y), 1.f))));
|
|
|
|
// compute intersection of ray with all six bbox planes
|
|
Vec3f rayinv(1.f / dir.x, 1.f / dir.y, 1.f / dir.z);
|
|
Point3f tbottom = rayinv.mul(boxMin - orig);
|
|
Point3f ttop = rayinv.mul(boxMax - orig);
|
|
|
|
// re-order intersections to find smallest and largest on each axis
|
|
Point3f minAx(min(ttop.x, tbottom.x), min(ttop.y, tbottom.y), min(ttop.z, tbottom.z));
|
|
Point3f maxAx(max(ttop.x, tbottom.x), max(ttop.y, tbottom.y), max(ttop.z, tbottom.z));
|
|
|
|
// near clipping plane
|
|
const float clip = 0.f;
|
|
//float tmin = max(max(max(minAx.x, minAx.y), max(minAx.x, minAx.z)), clip);
|
|
//float tmax = min(min(maxAx.x, maxAx.y), min(maxAx.x, maxAx.z));
|
|
float tmin = max({ minAx.x, minAx.y, minAx.z, clip });
|
|
float tmax = min({ maxAx.x, maxAx.y, maxAx.z });
|
|
|
|
// precautions against getting coordinates out of bounds
|
|
tmin = tmin + tstep;
|
|
tmax = tmax - tstep;
|
|
|
|
if (tmin < tmax)
|
|
{
|
|
// interpolation optimized a little
|
|
orig = orig * voxelSizeInv;
|
|
dir = dir * voxelSizeInv;
|
|
|
|
Point3f rayStep = dir * tstep;
|
|
Point3f next = (orig + dir * tmin);
|
|
float f = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
float fnext = f;
|
|
|
|
//raymarch
|
|
int steps = 0;
|
|
int nSteps = int(floor((tmax - tmin) / tstep));
|
|
for (; steps < nSteps; steps++)
|
|
{
|
|
next += rayStep;
|
|
int xdim = volDims[0];
|
|
int ydim = volDims[1];
|
|
int zdim = volDims[2];
|
|
int ix = cvRound(next.x);
|
|
int iy = cvRound(next.y);
|
|
int iz = cvRound(next.z);
|
|
fnext = tsdfToFloat(volume.at<TsdfVoxel>(ix * xdim + iy * ydim + iz * zdim).tsdf);
|
|
if (fnext != f)
|
|
{
|
|
fnext = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
// when ray crosses a surface
|
|
if (std::signbit(f) != std::signbit(fnext))
|
|
break;
|
|
|
|
f = fnext;
|
|
}
|
|
}
|
|
// if ray penetrates a surface from outside
|
|
// linearly interpolate t between two f values
|
|
if (f > 0.f && fnext < 0.f)
|
|
{
|
|
Point3f tp = next - rayStep;
|
|
float ft = interpolateTsdfVoxel(volume, volDims, neighbourCoords, tp);
|
|
float ftdt = interpolateTsdfVoxel(volume, volDims, neighbourCoords, next);
|
|
// float t = tmin + steps*tstep;
|
|
// float ts = t - tstep*ft/(ftdt - ft);
|
|
float ts = tmin + tstep * (steps - ft / (ftdt - ft));
|
|
|
|
// avoid division by zero
|
|
if (!cvIsNaN(ts) && !cvIsInf(ts))
|
|
{
|
|
Point3f pv = (orig + dir * ts);
|
|
Point3f nv = getNormalVoxel(volume, volDims, neighbourCoords, volResolution, pv);
|
|
|
|
if (!isNaN(nv))
|
|
{
|
|
//convert pv and nv to camera space
|
|
normal = volRot * nv;
|
|
// interpolation optimized a little
|
|
point = vol2cam * (pv * voxelSize);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
ptsRow[x] = toPtype(point);
|
|
nrmRow[x] = toPtype(normal);
|
|
}
|
|
}
|
|
};
|
|
|
|
#endif
|
|
parallel_for_(raycastRange, RaycastInvoker);
|
|
}
|
|
|
|
|
|
#ifdef HAVE_OPENCL
|
|
void ocl_raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose,
|
|
int height, int width, InputArray intr,
|
|
InputArray _volume, OutputArray _points, OutputArray _normals)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
|
|
const Size frameSize(width, height);
|
|
CV_Assert(frameSize.area() > 0);
|
|
|
|
Matx33f mintr(intr.getMat());
|
|
|
|
String errorStr;
|
|
String name = "raycast";
|
|
ocl::ProgramSource source = ocl::ptcloud::tsdf_oclsrc;
|
|
String options = "-cl-mad-enable";
|
|
ocl::Kernel k;
|
|
k.create(name.c_str(), source, options, &errorStr);
|
|
|
|
if (k.empty())
|
|
throw std::runtime_error("Failed to create kernel: " + errorStr);
|
|
|
|
_points.create(frameSize, CV_32FC4);
|
|
_normals.create(frameSize, CV_32FC4);
|
|
|
|
UMat points = _points.getUMat();
|
|
UMat normals = _normals.getUMat();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
const Point3f volSize = Point3f(volResolution) * settings.getVoxelSize();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
|
|
UMat vol2camGpu, cam2volGpu;
|
|
Affine3f vol2cam = Affine3f(cameraPose.inv()) * pose;
|
|
Affine3f cam2vol = pose.inv() * Affine3f(cameraPose);
|
|
Mat(cam2vol.matrix).copyTo(cam2volGpu);
|
|
Mat(vol2cam.matrix).copyTo(vol2camGpu);
|
|
|
|
Intr intrinsics(mintr);
|
|
Intr::Reprojector r = intrinsics.makeReprojector();
|
|
|
|
const UMat volume = _volume.getUMat();
|
|
float voxelSize = settings.getVoxelSize();
|
|
float raycastStepFactor = settings.getRaycastStepFactor();
|
|
float truncatedDistance = settings.getTsdfTruncateDistance();
|
|
|
|
// We do subtract voxel size to minimize checks after
|
|
// Note: origin of volume coordinate is placed
|
|
// in the center of voxel (0,0,0), not in the corner of the voxel!
|
|
Vec4f boxMin, boxMax(volSize.x - voxelSize,
|
|
volSize.y - voxelSize,
|
|
volSize.z - voxelSize);
|
|
Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy);
|
|
float tstep = truncatedDistance * raycastStepFactor;
|
|
|
|
Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z);
|
|
|
|
k.args(ocl::KernelArg::WriteOnlyNoSize(points),
|
|
ocl::KernelArg::WriteOnlyNoSize(normals),
|
|
frameSize,
|
|
ocl::KernelArg::PtrReadOnly(volume),
|
|
ocl::KernelArg::PtrReadOnly(vol2camGpu),
|
|
ocl::KernelArg::PtrReadOnly(cam2volGpu),
|
|
finv.val, cxy.val,
|
|
boxMin.val, boxMax.val,
|
|
tstep,
|
|
voxelSize,
|
|
volResGpu.val,
|
|
volDims.val,
|
|
neighbourCoords.val);
|
|
|
|
size_t globalSize[2];
|
|
globalSize[0] = (size_t)frameSize.width;
|
|
globalSize[1] = (size_t)frameSize.height;
|
|
|
|
if (!k.run(2, globalSize, NULL, true))
|
|
throw std::runtime_error("Failed to run kernel");
|
|
}
|
|
#endif
|
|
|
|
// Fetch
|
|
|
|
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, InputArray _points, OutputArray _normals)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
CV_Assert(!_points.empty());
|
|
if (!_normals.needed())
|
|
return;
|
|
|
|
Points points = _points.getMat();
|
|
CV_Assert(points.type() == POINT_TYPE);
|
|
|
|
_normals.createSameSize(_points, _points.type());
|
|
Normals normals = _normals.getMat();
|
|
|
|
const Mat volume = _volume.getMat();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
Affine3f invPose(pose.inv());
|
|
Matx33f r = pose.rotation();
|
|
float voxelSizeInv = 1.f / settings.getVoxelSize();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
|
|
auto PushNormals = [&](const ptype& pp, const int* position)
|
|
{
|
|
Point3f p = fromPtype(pp);
|
|
Point3f n = nan3;
|
|
if (!isNaN(p))
|
|
{
|
|
Point3f voxPt = (invPose * p);
|
|
voxPt = voxPt * voxelSizeInv;
|
|
n = r * getNormalVoxel(volume, volDims, neighbourCoords, volResolution, voxPt);
|
|
}
|
|
normals(position[0], position[1]) = toPtype(n);
|
|
};
|
|
points.forEach(PushNormals);
|
|
}
|
|
|
|
#ifdef HAVE_OPENCL
|
|
void ocl_fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, InputArray _points, OutputArray _normals)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
CV_Assert(!_points.empty());
|
|
if (!_normals.needed())
|
|
return;
|
|
|
|
UMat points = _points.getUMat();
|
|
CV_Assert(points.type() == POINT_TYPE);
|
|
|
|
_normals.createSameSize(_points, POINT_TYPE);
|
|
UMat normals = _normals.getUMat();
|
|
|
|
const UMat volume = _volume.getUMat();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
float voxelSizeInv = 1.f / settings.getVoxelSize();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
|
|
String errorStr;
|
|
String name = "getNormals";
|
|
ocl::ProgramSource source = ocl::ptcloud::tsdf_oclsrc;
|
|
String options = "-cl-mad-enable";
|
|
ocl::Kernel k;
|
|
k.create(name.c_str(), source, options, &errorStr);
|
|
|
|
if (k.empty())
|
|
throw std::runtime_error("Failed to create kernel: " + errorStr);
|
|
|
|
UMat volPoseGpu, invPoseGpu;
|
|
Mat(pose.matrix).copyTo(volPoseGpu);
|
|
Mat(pose.inv().matrix).copyTo(invPoseGpu);
|
|
Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z);
|
|
Size frameSize = points.size();
|
|
|
|
k.args(ocl::KernelArg::ReadOnlyNoSize(points),
|
|
ocl::KernelArg::WriteOnlyNoSize(normals),
|
|
frameSize,
|
|
ocl::KernelArg::PtrReadOnly(volume),
|
|
ocl::KernelArg::PtrReadOnly(volPoseGpu),
|
|
ocl::KernelArg::PtrReadOnly(invPoseGpu),
|
|
voxelSizeInv,
|
|
volResGpu.val,
|
|
volDims.val,
|
|
neighbourCoords.val);
|
|
|
|
size_t globalSize[2];
|
|
globalSize[0] = (size_t)points.cols;
|
|
globalSize[1] = (size_t)points.rows;
|
|
|
|
if (!k.run(2, globalSize, NULL, true))
|
|
throw std::runtime_error("Failed to run kernel");
|
|
|
|
}
|
|
#endif
|
|
|
|
inline void coord(const Mat& volume, const TsdfVoxel* volDataStart, std::vector<ptype>& points, std::vector<ptype>& normals,
|
|
const Point3i volResolution, const Vec4i volDims, const Vec8i neighbourCoords, const Affine3f pose,
|
|
const float voxelSize, const float voxelSizeInv, bool needNormals, int x, int y, int z, Point3f V, float v0, int axis)
|
|
{
|
|
// 0 for x, 1 for y, 2 for z
|
|
bool limits = false;
|
|
Point3i shift;
|
|
float Vc = 0.f;
|
|
if (axis == 0)
|
|
{
|
|
shift = Point3i(1, 0, 0);
|
|
limits = (x + 1 < volResolution.x);
|
|
Vc = V.x;
|
|
}
|
|
if (axis == 1)
|
|
{
|
|
shift = Point3i(0, 1, 0);
|
|
limits = (y + 1 < volResolution.y);
|
|
Vc = V.y;
|
|
}
|
|
if (axis == 2)
|
|
{
|
|
shift = Point3i(0, 0, 1);
|
|
limits = (z + 1 < volResolution.z);
|
|
Vc = V.z;
|
|
}
|
|
|
|
if (limits)
|
|
{
|
|
const TsdfVoxel &voxeld = volDataStart[(x + shift.x) * volDims[0] +
|
|
(y + shift.y) * volDims[1] +
|
|
(z + shift.z) * volDims[2]];
|
|
float vd = tsdfToFloat(voxeld.tsdf);
|
|
if (voxeld.weight != 0 && vd != 1.f)
|
|
{
|
|
if ((v0 > 0 && vd < 0) || (v0 < 0 && vd > 0))
|
|
{
|
|
//linearly interpolate coordinate
|
|
float Vn = Vc + voxelSize;
|
|
float dinv = 1.f / (abs(v0) + abs(vd));
|
|
float inter = (Vc * abs(vd) + Vn * abs(v0)) * dinv;
|
|
|
|
Point3f p(shift.x ? inter : V.x,
|
|
shift.y ? inter : V.y,
|
|
shift.z ? inter : V.z);
|
|
{
|
|
points.push_back(toPtype(pose * p));
|
|
if (needNormals)
|
|
normals.push_back(toPtype(pose.rotation() *
|
|
getNormalVoxel(volume, volDims, neighbourCoords, volResolution, p * voxelSizeInv)));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, OutputArray _points, OutputArray _normals)
|
|
{
|
|
if (!_points.needed())
|
|
return;
|
|
const Mat volume = _volume.getMat();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
float voxelSize = settings.getVoxelSize();
|
|
float voxelSizeInv = 1.f / settings.getVoxelSize();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
|
|
bool needNormals = _normals.needed();
|
|
|
|
std::vector<std::vector<ptype>> pVecs, nVecs;
|
|
Range fetchRange(0, volResolution.x);
|
|
const int nstripes = -1;
|
|
const TsdfVoxel* volDataStart = volume.ptr<TsdfVoxel>();
|
|
Mutex mutex;
|
|
|
|
auto FetchPointsNormalsInvoker = [&](const Range& range) {
|
|
std::vector<ptype> points, normals;
|
|
for (int x = range.start; x < range.end; x++)
|
|
{
|
|
const TsdfVoxel* volDataX = volDataStart + x * volDims[0];
|
|
for (int y = 0; y < volResolution.y; y++)
|
|
{
|
|
const TsdfVoxel* volDataY = volDataX + y * volDims[1];
|
|
for (int z = 0; z < volResolution.z; z++)
|
|
{
|
|
const TsdfVoxel& voxel0 = volDataY[z * volDims[2]];
|
|
float v0 = tsdfToFloat(voxel0.tsdf);
|
|
if (voxel0.weight != 0 && v0 != 1.f)
|
|
{
|
|
Point3f V(Point3f((float)x + 0.5f, (float)y + 0.5f, (float)z + 0.5f) * voxelSize);
|
|
|
|
coord(volume, volDataStart, points, normals, volResolution, volDims, neighbourCoords, pose, voxelSize, voxelSizeInv, needNormals, x, y, z, V, v0, 0);
|
|
coord(volume, volDataStart, points, normals, volResolution, volDims, neighbourCoords, pose, voxelSize, voxelSizeInv, needNormals, x, y, z, V, v0, 1);
|
|
coord(volume, volDataStart, points, normals, volResolution, volDims, neighbourCoords, pose, voxelSize, voxelSizeInv, needNormals, x, y, z, V, v0, 2);
|
|
|
|
} // if voxel is not empty
|
|
}
|
|
}
|
|
}
|
|
|
|
AutoLock al(mutex);
|
|
pVecs.push_back(points);
|
|
nVecs.push_back(normals);
|
|
};
|
|
|
|
parallel_for_(fetchRange, FetchPointsNormalsInvoker, nstripes);
|
|
|
|
std::vector<ptype> points, normals;
|
|
for (size_t i = 0; i < pVecs.size(); i++)
|
|
{
|
|
points.insert(points.end(), pVecs[i].begin(), pVecs[i].end());
|
|
normals.insert(normals.end(), nVecs[i].begin(), nVecs[i].end());
|
|
}
|
|
|
|
_points.create((int)points.size(), 1, POINT_TYPE);
|
|
if (!points.empty())
|
|
Mat((int)points.size(), 1, POINT_TYPE, &points[0]).copyTo(_points.getMat());
|
|
|
|
if (_normals.needed())
|
|
{
|
|
_normals.create((int)normals.size(), 1, POINT_TYPE);
|
|
if (!normals.empty())
|
|
Mat((int)normals.size(), 1, POINT_TYPE, &normals[0]).copyTo(_normals.getMat());
|
|
}
|
|
|
|
}
|
|
|
|
|
|
#ifdef HAVE_OPENCL
|
|
void ocl_fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, OutputArray points, OutputArray normals)
|
|
{
|
|
CV_TRACE_FUNCTION();
|
|
|
|
if (!points.needed())
|
|
return;
|
|
|
|
|
|
const UMat volume = _volume.getUMat();
|
|
|
|
Matx44f _pose;
|
|
settings.getVolumePose(_pose);
|
|
const Affine3f pose = Affine3f(_pose);
|
|
|
|
float voxelSize = settings.getVoxelSize();
|
|
float voxelSizeInv = 1.f / settings.getVoxelSize();
|
|
|
|
const Vec4i volDims;
|
|
settings.getVolumeStrides(volDims);
|
|
const Vec8i neighbourCoords = Vec8i(
|
|
volDims.dot(Vec4i(0, 0, 0)),
|
|
volDims.dot(Vec4i(0, 0, 1)),
|
|
volDims.dot(Vec4i(0, 1, 0)),
|
|
volDims.dot(Vec4i(0, 1, 1)),
|
|
volDims.dot(Vec4i(1, 0, 0)),
|
|
volDims.dot(Vec4i(1, 0, 1)),
|
|
volDims.dot(Vec4i(1, 1, 0)),
|
|
volDims.dot(Vec4i(1, 1, 1))
|
|
);
|
|
|
|
Vec3i resolution;
|
|
settings.getVolumeResolution(resolution);
|
|
const Point3i volResolution = Point3i(resolution);
|
|
|
|
|
|
bool needNormals = normals.needed();
|
|
|
|
// 1. scan to count points in each group and allocate output arrays
|
|
|
|
ocl::Kernel kscan;
|
|
|
|
String errorStr;
|
|
ocl::ProgramSource source = ocl::ptcloud::tsdf_oclsrc;
|
|
String options = "-cl-mad-enable";
|
|
|
|
kscan.create("scanSize", source, options, &errorStr);
|
|
|
|
if (kscan.empty())
|
|
throw std::runtime_error("Failed to create kernel: " + errorStr);
|
|
|
|
size_t globalSize[3];
|
|
globalSize[0] = (size_t)volResolution.x;
|
|
globalSize[1] = (size_t)volResolution.y;
|
|
globalSize[2] = (size_t)volResolution.z;
|
|
|
|
const ocl::Device& device = ocl::Device::getDefault();
|
|
size_t wgsLimit = device.maxWorkGroupSize();
|
|
size_t memSize = device.localMemSize();
|
|
// local mem should keep a point (and a normal) for each thread in a group
|
|
// use 4 float per each point and normal
|
|
size_t elemSize = (sizeof(float) * 4) * (needNormals ? 2 : 1);
|
|
const size_t lcols = 8;
|
|
const size_t lrows = 8;
|
|
size_t lplanes = min(memSize / elemSize, wgsLimit) / lcols / lrows;
|
|
lplanes = roundDownPow2(lplanes);
|
|
size_t localSize[3] = { lcols, lrows, lplanes };
|
|
Vec3i ngroups((int)divUp(globalSize[0], (unsigned int)localSize[0]),
|
|
(int)divUp(globalSize[1], (unsigned int)localSize[1]),
|
|
(int)divUp(globalSize[2], (unsigned int)localSize[2]));
|
|
|
|
const size_t counterSize = sizeof(int);
|
|
size_t lszscan = localSize[0] * localSize[1] * localSize[2] * counterSize;
|
|
|
|
const int gsz[3] = { ngroups[2], ngroups[1], ngroups[0] };
|
|
UMat groupedSum(3, gsz, CV_32S, Scalar(0));
|
|
|
|
UMat volPoseGpu;
|
|
Mat(pose.matrix).copyTo(volPoseGpu);
|
|
Vec4i volResGpu(volResolution.x, volResolution.y, volResolution.z);
|
|
|
|
kscan.args(ocl::KernelArg::PtrReadOnly(volume),
|
|
volResGpu.val,
|
|
volDims.val,
|
|
neighbourCoords.val,
|
|
ocl::KernelArg::PtrReadOnly(volPoseGpu),
|
|
voxelSize,
|
|
voxelSizeInv,
|
|
ocl::KernelArg::Local(lszscan),
|
|
ocl::KernelArg::WriteOnlyNoSize(groupedSum));
|
|
|
|
if (!kscan.run(3, globalSize, localSize, true))
|
|
throw std::runtime_error("Failed to run kernel");
|
|
|
|
Mat groupedSumCpu = groupedSum.getMat(ACCESS_READ);
|
|
int gpuSum = (int)sum(groupedSumCpu)[0];
|
|
// should be no CPU copies when new kernel is executing
|
|
groupedSumCpu.release();
|
|
|
|
// 2. fill output arrays according to per-group points count
|
|
|
|
points.create(gpuSum, 1, POINT_TYPE);
|
|
UMat pts = points.getUMat();
|
|
UMat nrm;
|
|
if (needNormals)
|
|
{
|
|
normals.create(gpuSum, 1, POINT_TYPE);
|
|
nrm = normals.getUMat();
|
|
}
|
|
else
|
|
{
|
|
// it won't be accessed but empty args are forbidden
|
|
nrm = UMat(1, 1, POINT_TYPE);
|
|
}
|
|
|
|
if (gpuSum)
|
|
{
|
|
ocl::Kernel kfill;
|
|
kfill.create("fillPtsNrm", source, options, &errorStr);
|
|
|
|
if (kfill.empty())
|
|
throw std::runtime_error("Failed to create kernel: " + errorStr);
|
|
|
|
UMat atomicCtr(1, 1, CV_32S, Scalar(0));
|
|
|
|
// mem size to keep pts (and normals optionally) for all work-items in a group
|
|
size_t lszfill = localSize[0] * localSize[1] * localSize[2] * elemSize;
|
|
|
|
kfill.args(ocl::KernelArg::PtrReadOnly(volume),
|
|
volResGpu.val,
|
|
volDims.val,
|
|
neighbourCoords.val,
|
|
ocl::KernelArg::PtrReadOnly(volPoseGpu),
|
|
voxelSize,
|
|
voxelSizeInv,
|
|
((int)needNormals),
|
|
ocl::KernelArg::Local(lszfill),
|
|
ocl::KernelArg::PtrReadWrite(atomicCtr),
|
|
ocl::KernelArg::ReadOnlyNoSize(groupedSum),
|
|
ocl::KernelArg::WriteOnlyNoSize(pts),
|
|
ocl::KernelArg::WriteOnlyNoSize(nrm)
|
|
);
|
|
|
|
if (!kfill.run(3, globalSize, localSize, true))
|
|
throw std::runtime_error("Failed to run kernel");
|
|
}
|
|
}
|
|
#endif
|
|
|
|
|
|
|
|
} // namespace cv
|