Files
opencv-MIRROR/modules/ptcloud/src/hash_tsdf_functions.cpp
Arman Rostami 279bc4a279 Merge pull request #27823 from armanrasta:5.x
Add ColorHashTSDFVolume implementation #27823

# Add ColorHashTSDFVolume implementation  [[#25155](https://github.com/opencv/opencv/issues/25155)]
## Description
Added a new ColorHashTSDFVolume implementation that combines the benefits of HashTSDFVolume's efficient spatial hashing with color support. This provides memory-efficient RGB-D fusion with better performance compared to regular ColorTSDFVolume.

### Key Features
- Hash-based spatial data structure for efficient storage
- Color integration during volume updates
- Raycast with color interpolation
- Compatible with existing TSDF interfaces
- CPU implementation with parallel processing support

### Implementation Details
- Added new ColorHashTSDFVolume class with create() factory method
- ColorVoxel structure combining TSDF and RGB data
- Spatial hashing for efficient voxel lookup
- Weighted running average for color updates
- Trilinear interpolation during raycasting
- Unit tests for basic operations and edge cases

### Files Modified/Added
- modules/3d/src/rgbd/color_hash_volume.hpp - New header defining ColorHashTSDFVolume interface
- modules/3d/src/rgbd/color_hash_volume.cpp - Implementation of ColorHashTSDFVolume
- modules/3d/test/test_color_hash_volume.cpp - Unit tests

### Performance
The implementation uses spatial hashing to only store voxels near surfaces, significantly reducing memory usage compared to regular ColorTSDFVolume while maintaining similar processing speed.

### Testing
Added unit tests that verify:
- Basic integration and raycasting operations
- Empty volume handling
- Memory usage patterns

### Future Work
- GPU/OpenCL implementation
- Additional color interpolation methods
- Extended comparison tests with other volume types
2026-07-20 10:40:19 +03:00

1553 lines
55 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 "hash_tsdf_functions.hpp"
#include "opencl_kernels_ptcloud.hpp"
namespace cv {
static Vec3i volumeToVolumeUnitIdx(const Point3f& point, const float volumeUnitSize)
{
return cv::Vec3i(
cvFloor(point.x / volumeUnitSize),
cvFloor(point.y / volumeUnitSize),
cvFloor(point.z / volumeUnitSize));
}
static cv::Point3f volumeUnitIdxToVolume(const cv::Vec3i& volumeUnitIdx, const float volumeUnitSize)
{
return cv::Point3f(
volumeUnitIdx[0] * volumeUnitSize,
volumeUnitIdx[1] * volumeUnitSize,
volumeUnitIdx[2] * volumeUnitSize);
}
static cv::Point3f voxelCoordToVolume(const cv::Vec3i& voxelIdx, const float voxelSize)
{
return cv::Point3f(
voxelIdx[0] * voxelSize,
voxelIdx[1] * voxelSize,
voxelIdx[2] * voxelSize);
}
static cv::Vec3i volumeToVoxelCoord(const cv::Point3f& point, const float voxelSizeInv)
{
return cv::Vec3i(
cvFloor(point.x * voxelSizeInv),
cvFloor(point.y * voxelSizeInv),
cvFloor(point.z * voxelSizeInv));
}
int calcVolumeUnitDegree(Point3i volumeResolution)
{
if (!(volumeResolution.x & (volumeResolution.x - 1)))
{
// vuRes is a power of 2, let's get this power
return trailingZeros32(volumeResolution.x);
}
else
{
CV_Error(Error::StsBadArg, "Volume unit resolution should be a power of 2");
}
}
void allocateVolumeUnits(
const UMat& _depth, float depthFactor, const Affine3f volumePose, const Matx44f& cameraPose, const Intr& intrinsics,
CustomHashSet& hashTable, const int volumeUnitDegree, const float truncDist, const float maxDepth, const float volumeUnitSize);
TsdfVoxel atVolumeUnit(
const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits,
const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it,
const int volumeUnitDegree, const Vec4i volStrides);
TsdfVoxel new_atVolumeUnit(
const Mat& volUnitsDataCopy,
const Vec3i& point, const Vec3i& volumeUnitIdx, const int indx,
const int volumeUnitDegree, const Vec4i volStrides);
Point3f getNormalVoxel(
const Point3f& point, const float voxelSizeInv,
const int volumeUnitDegree, const Vec4i volStrides,
const Mat& volUnitsDataCopy, const VolumeUnitIndexes& volumeUnits);
#ifdef HAVE_OPENCL
void markActive(
const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId,
const Affine3f volumePose, CustomHashSet& hashTable, UMat& isActiveFlags, UMat& lastVisibleIndices,
const float maxDepth, const float volumeUnitSize);
Point3f ocl_getNormalVoxel(
const Point3f& point, const float voxelSizeInv,
const int volumeUnitDegree, const Vec4i volStrides,
const Mat& volUnitsData, const CustomHashSet& hashTable);
#endif
void integrateHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
InputArray _depth, InputArray _pixNorms, InputOutputArray _volUnitsData, VolumeUnitIndexes& volumeUnits)
{
CV_TRACE_FUNCTION();
CV_Assert(_depth.type() == DEPTH_TYPE);
Depth depth = _depth.getMat();
Mat& volUnitsData = _volUnitsData.getMatRef();
Mat pixNorms = _pixNorms.getMat();
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
Matx33f intr;
settings.getCameraIntegrateIntrinsics(intr);
const Intr intrinsics(intr);
const Intr::Reprojector reproj(intrinsics.makeReprojector());
const float maxDepth = settings.getMaxDepth();
const float voxelSize = settings.getVoxelSize();
Vec3i resolution;
settings.getVolumeResolution(resolution);
const float volumeUnitSize = voxelSize * resolution[0];
if (enableGrowth)
{
//! Compute volumes to be allocated
const int depthStride = volumeUnitDegree;
const float invDepthFactor = 1.f / settings.getDepthFactor();
const float truncDist = settings.getTsdfTruncateDistance();
const Point3f truncPt(truncDist, truncDist, truncDist);
std::unordered_set<cv::Vec3i, tsdf_hash> newIndices;
Mutex mutex;
Range allocateRange(0, depth.rows);
auto AllocateVolumeUnitsInvoker = [&](const Range& range)
{
std::unordered_set<cv::Vec3i, tsdf_hash> localAccessVolUnits;
for (int y = range.start; y < range.end; y += depthStride)
{
const depthType* depthRow = depth[y];
for (int x = 0; x < depth.cols; x += depthStride)
{
depthType z = depthRow[x] * invDepthFactor;
if (z <= 0 || z > maxDepth)
continue;
Point3f camPoint = reproj(Point3f((float)x, (float)y, z));
Point3f volPoint = cam2vol * camPoint;
//! Find accessed TSDF volume unit for valid 3D vertex
Vec3i lower_bound = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize);
Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize);
for (int i = lower_bound[0]; i <= upper_bound[0]; i++)
for (int j = lower_bound[1]; j <= upper_bound[1]; j++)
for (int k = lower_bound[2]; k <= upper_bound[2]; k++)
{
const Vec3i tsdf_idx = Vec3i(i, j, k);
if (localAccessVolUnits.count(tsdf_idx) <= 0 && volumeUnits.count(tsdf_idx) <= 0)
{
//! This volume unit will definitely be required for current integration
localAccessVolUnits.emplace(tsdf_idx);
}
}
}
}
mutex.lock();
for (const auto& tsdf_idx : localAccessVolUnits)
{
//! If the insert into the global set passes
if (!newIndices.count(tsdf_idx))
{
// Volume allocation can be performed outside of the lock
newIndices.emplace(tsdf_idx);
}
}
mutex.unlock();
};
parallel_for_(allocateRange, AllocateVolumeUnitsInvoker);
//! Perform the allocation
for (auto idx : newIndices)
{
VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second;
Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
vu.pose = subvolumePose;
vu.index = lastVolIndex;
if (lastVolIndex >= int(volUnitsData.size().height))
{
volUnitsData.resize(lastVolIndex * 2);
CV_LOG_DEBUG(NULL, "HashTSDF storage extended from " << lastVolIndex << " to " << lastVolIndex * 2 << " volume units");
}
lastVolIndex++;
volUnitsData.row(vu.index).forEach<VecTsdfVoxel>([](VecTsdfVoxel &vv, const int * /* position */)
{
TsdfVoxel& v = reinterpret_cast<TsdfVoxel&>(vv);
v.tsdf = floatToTsdf(0.0f); v.weight = 0;
});
//! This volume unit will definitely be required for current integration
vu.lastVisibleIndex = frameId;
vu.isActive = true;
}
}
//! Get keys for all the allocated volume Units
std::vector<Vec3i> totalVolUnits;
for (const auto& keyvalue : volumeUnits)
{
totalVolUnits.push_back(keyvalue.first);
}
//! Mark volumes in the camera frustum as active
Range inFrustumRange(0, (int)volumeUnits.size());
parallel_for_(inFrustumRange, [&](const Range& range)
{
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
const Intr::Projector proj(intrinsics.makeProjector());
for (int i = range.start; i < range.end; ++i)
{
Vec3i tsdf_idx = totalVolUnits[i];
VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx);
if (it == volumeUnits.end())
continue;
Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first, volumeUnitSize);
Point3f volUnitInCamSpace = vol2cam * volumeUnitPos;
if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > maxDepth)
{
it->second.isActive = false;
continue;
}
Point2f cameraPoint = proj(volUnitInCamSpace);
if (cameraPoint.x >= 0 && cameraPoint.y >= 0 && cameraPoint.x < depth.cols && cameraPoint.y < depth.rows)
{
assert(it != volumeUnits.end());
it->second.lastVisibleIndex = frameId;
it->second.isActive = true;
}
}
});
//! Integrate the correct volumeUnits
parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range)
{
for (int i = range.start; i < range.end; i++)
{
Vec3i tsdf_idx = totalVolUnits[i];
VolumeUnitIndexes::iterator it = volumeUnits.find(tsdf_idx);
if (it == volumeUnits.end())
return;
VolumeUnit& volumeUnit = it->second;
if (volumeUnit.isActive)
{
//! The volume unit should already be added into the Volume from the allocator
integrateTsdfVolumeUnit(settings, volumeUnit.pose, cameraPose, depth, pixNorms, volUnitsData.row(volumeUnit.index));
//! Ensure all active volumeUnits are set to inactive for next integration
volumeUnit.isActive = false;
}
}
});
}
void allocateVolumeUnits(
const UMat& _depth, float depthFactor, const Affine3f volumePose, const Matx44f& cameraPose, const Intr& intrinsics,
CustomHashSet& hashTable, const int volumeUnitDegree, const float truncDist, const float maxDepth, const float volumeUnitSize)
{
constexpr int pixCapacity = 16;
typedef std::array<Vec3i, pixCapacity> LocalVolUnits;
Depth depth = _depth.getMat(ACCESS_READ);
//! Compute volumes to be allocated
const int depthStride = volumeUnitDegree;
const float invDepthFactor = 1.f / depthFactor;
const Intr::Reprojector reproj(intrinsics.makeReprojector());
const Affine3f cam2vol(volumePose.inv() * Affine3f(cameraPose));
const Point3f truncPt(truncDist, truncDist, truncDist);
Mutex mutex;
// for new indices
CustomHashSet thm;
auto fillLocalAcessVolUnits = [&](const Range& xrange, const Range& yrange, CustomHashSet& ghm)
{
for (int y = yrange.start; y < yrange.end; y += depthStride)
{
const depthType* depthRow = depth[y];
for (int x = xrange.start; x < xrange.end; x += depthStride)
{
depthType z = depthRow[x] * invDepthFactor;
if (z <= 0 || z > maxDepth)
continue;
Point3f camPoint = reproj(Point3f((float)x, (float)y, z));
Point3f volPoint = cam2vol * camPoint;
//! Find accessed TSDF volume unit for valid 3D vertex
Vec3i lower_bound = volumeToVolumeUnitIdx(volPoint - truncPt, volumeUnitSize);
Vec3i upper_bound = volumeToVolumeUnitIdx(volPoint + truncPt, volumeUnitSize);
int pixLocalCounter = 0;
LocalVolUnits pixLocalVolUnits;
for (int i = lower_bound[0]; i <= upper_bound[0]; i++)
for (int j = lower_bound[1]; j <= upper_bound[1]; j++)
for (int k = lower_bound[2]; k <= upper_bound[2]; k++)
{
const Vec3i tsdf_idx = Vec3i(i, j, k);
if (hashTable.find(tsdf_idx) < 0)
{
bool found = false;
for (int c = 0; c < pixLocalCounter; c++)
{
if (pixLocalVolUnits[c] == tsdf_idx)
{
found = true; break;
}
}
if (!found)
{
pixLocalVolUnits[pixLocalCounter++] = tsdf_idx;
if (pixLocalCounter >= pixCapacity)
{
return;
}
}
}
}
// lock localAccessVolUnits somehow
for (int i = 0; i < pixLocalCounter; i++)
{
Vec3i idx = pixLocalVolUnits[i];
if (!ghm.insert(idx))
{
//return;
}
}
// unlock
}
}
};
Rect dim(0, 0, depth.cols, depth.rows);
Size gsz(32, 32);
Size gg(divUp(dim.width, gsz.width), divUp(dim.height, gsz.height));
bool needReallocation = false;
auto allocateLambda = [&](const Range& r)
{
for (int yg = r.start; yg < r.end; yg++)
{
for (int xg = 0; xg < gg.width; xg++)
{
Rect gr(xg * gsz.width, yg * gsz.height, (xg + 1) * gsz.width, (yg + 1) * gsz.height);
gr = gr & dim;
Range xr(gr.tl().x, gr.br().x), yr(gr.tl().y, gr.br().y);
CustomHashSet ghm;
fillLocalAcessVolUnits(xr, yr, ghm);
if (ghm.last)
{
cv::AutoLock al(mutex);
for (int i = 0; i < ghm.last; i++)
{
Vec4i node = ghm.data[i];
Vec3i idx(node[0], node[1], node[2]);
//TODO: 1. add to separate hash map instead, then merge on GPU side
int result = thm.insert(idx);
if (!result)
{
needReallocation = true;
return;
}
}
}
}
}
};
do
{
if (needReallocation)
{
thm.capacity *= 2;
thm.data.resize(thm.capacity);
needReallocation = false;
}
parallel_for_(Range(0, gg.height), allocateLambda);
} while (needReallocation);
auto pushToGlobal = [](const CustomHashSet _thm, CustomHashSet& _globalHashMap,
bool& _needReallocation, Mutex& _mutex)
{
for (int i = 0; i < _thm.last; i++)
{
Vec4i node = _thm.data[i];
Vec3i idx(node[0], node[1], node[2]);
std::lock_guard<Mutex> al(_mutex);
int result = _globalHashMap.insert(idx);
if (result == 0)
{
_needReallocation = true;
return;
}
}
};
needReallocation = false;
do
{
if (needReallocation)
{
hashTable.capacity *= 2;
hashTable.data.resize(hashTable.capacity);
needReallocation = false;
}
pushToGlobal(thm, hashTable, needReallocation, mutex);
} while (needReallocation);
}
#ifdef HAVE_OPENCL
void markActive(
const Matx44f& cameraPose, const Intr& intrinsics, const Size frameSz, const int frameId,
const Affine3f volumePose, CustomHashSet& hashTable, UMat& isActiveFlags, UMat& lastVisibleIndices,
const float maxDepth, const float volumeUnitSize)
{
//! Mark volumes in the camera frustum as active
String errorStr;
String name = "markActive";
ocl::ProgramSource source = ocl::ptcloud::hash_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);
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * volumePose);
const Intr::Projector proj(intrinsics.makeProjector());
Vec2f fxy(proj.fx, proj.fy), cxy(proj.cx, proj.cy);
UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ);
k.args(
ocl::KernelArg::PtrReadOnly(hashDataGpu),
ocl::KernelArg::WriteOnly(isActiveFlags),
ocl::KernelArg::WriteOnly(lastVisibleIndices),
vol2cam.matrix,
fxy,
cxy,
frameSz,
volumeUnitSize,
hashTable.last,
maxDepth,
frameId
);
size_t globalSize[1] = { (size_t)hashTable.last };
if (!k.run(1, globalSize, nullptr, true))
throw std::runtime_error("Failed to run kernel");
}
void ocl_integrateHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth,
InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputOutputArray _volUnitsDataCopy, InputOutputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags)
{
CV_TRACE_FUNCTION();
UMat depth = _depth.getUMat();
CV_Assert(!depth.empty());
CV_Assert(lastVolIndex >= 0);
UMat pixNorms = _pixNorms.getUMat();
UMat& volUnitsData = _volUnitsData.getUMatRef();
Mat& volUnitsDataCopy = _volUnitsDataCopy.getMatRef();
UMat isActiveFlags = _isActiveFlags.getUMat();
UMat lastVisibleIndices = _lastVisibleIndices.getUMat();
Matx33f intr;
settings.getCameraIntegrateIntrinsics(intr);
const Intr intrinsics(intr);
Vec4i volStrides;
settings.getVolumeStrides(volStrides);
Vec3i resolution;
settings.getVolumeResolution(resolution);
const int volumeUnitResolution = resolution[0];
const int maxWeight = settings.getMaxWeight();
const float truncDist = settings.getTsdfTruncateDistance();
const float maxDepth = settings.getMaxDepth();
const float voxelSize = settings.getVoxelSize();
const float depthFactor = settings.getDepthFactor();
const float dfac = 1.f / depthFactor;
const float volumeUnitSize = voxelSize * resolution[0];
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix;
if (enableGrowth)
{
// Save length to fill new data in ranges
int sizeBefore = hashTable.last;
allocateVolumeUnits(depth, depthFactor, pose, cameraPose, intrinsics, hashTable, volumeUnitDegree, truncDist, maxDepth, volumeUnitSize);
int sizeAfter = hashTable.last;
//! Perform the allocation
// Grow buffers
int buff_lvl = (int)(1 << bufferSizeDegree);
if (sizeAfter >= buff_lvl)
{
bufferSizeDegree = (int)(log2(sizeAfter) + 1); // clz() would be better
int oldBuffSize = buff_lvl;
buff_lvl = (int)pow(2, bufferSizeDegree);
volUnitsDataCopy.resize(buff_lvl);
Range oldr(0, oldBuffSize);
int volCubed = volumeUnitResolution * volumeUnitResolution * volumeUnitResolution;
UMat newData(buff_lvl, volCubed, CV_8UC2);
volUnitsData.copyTo(newData.rowRange(oldr));
volUnitsData = newData;
UMat newLastVisibleIndices(buff_lvl, 1, CV_32S);
lastVisibleIndices.copyTo(newLastVisibleIndices.rowRange(oldr));
lastVisibleIndices = newLastVisibleIndices;
UMat newIsActiveFlags(buff_lvl, 1, CV_8U);
isActiveFlags.copyTo(newIsActiveFlags.rowRange(oldr));
isActiveFlags = newIsActiveFlags;
}
// Fill data for new volume units
Range r(sizeBefore, sizeAfter);
if (r.start < r.end)
{
lastVisibleIndices.rowRange(r) = frameId;
isActiveFlags.rowRange(r) = 1;
TsdfVoxel emptyVoxel(floatToTsdf(0.0f), 0);
volUnitsData.rowRange(r) = Vec2b((uchar)(emptyVoxel.tsdf), (uchar)(emptyVoxel.weight));
}
}
//! Mark volumes in the camera frustum as active
markActive(cameraPose, intrinsics, depth.size(), frameId, pose, hashTable, isActiveFlags, lastVisibleIndices, maxDepth, volumeUnitSize);
//! Integrate the correct volumeUnits
String errorStr;
String name = "integrateAllVolumeUnits";
ocl::ProgramSource source = ocl::ptcloud::hash_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);
Vec2f fxy(intrinsics.fx, intrinsics.fy), cxy(intrinsics.cx, intrinsics.cy);
UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ);
UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ);
k.args(ocl::KernelArg::ReadOnly(depth),
ocl::KernelArg::PtrReadOnly(hashesGpu),
ocl::KernelArg::PtrReadOnly(hashDataGpu),
ocl::KernelArg::ReadWrite(volUnitsData),
ocl::KernelArg::ReadOnly(pixNorms),
ocl::KernelArg::ReadOnly(isActiveFlags),
vol2camMatrix,
voxelSize,
volumeUnitResolution,
volStrides.val,
fxy.val,
cxy.val,
dfac,
truncDist,
int(maxWeight)
);
int resol = volumeUnitResolution;
size_t globalSize[3];
globalSize[0] = (size_t)resol;
globalSize[1] = (size_t)resol;
globalSize[2] = (size_t)hashTable.last; // num of volume units
if (!k.run(3, globalSize, NULL, true))
throw std::runtime_error("Failed to run kernel");
}
#endif
inline TsdfVoxel _at(Mat& volUnitsData, const cv::Vec3i& volumeIdx, int indx,
const int volumeUnitResolution, const Vec4i volStrides)
{
//! Out of bounds
if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) ||
(volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) ||
(volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0))
{
return TsdfVoxel(floatToTsdf(1.f), 0);
}
const TsdfVoxel* volData = volUnitsData.ptr<TsdfVoxel>(indx);
int coordBase =
volumeIdx[0] * volStrides[0] + volumeIdx[1] * volStrides[1] + volumeIdx[2] * volStrides[2];
return volData[coordBase];
}
#if USE_INTRINSICS
inline float interpolate(float tx, float ty, float tz, float vx[8])
{
v_float32x4 v0246, v1357;
v_load_deinterleave(vx, v0246, v1357);
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);
}
#else
inline float interpolate(float tx, float ty, float tz, float vx[8])
{
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
TsdfVoxel atVolumeUnit(
const Mat& volUnitsData, const VolumeUnitIndexes& volumeUnits,
const Vec3i& point, const Vec3i& volumeUnitIdx, VolumeUnitIndexes::const_iterator it,
const int volumeUnitDegree, const Vec4i volStrides)
{
if (it == volumeUnits.end())
{
return TsdfVoxel(floatToTsdf(1.f), 0);
}
Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree,
volumeUnitIdx[1] << volumeUnitDegree,
volumeUnitIdx[2] << volumeUnitDegree);
// expanding at(), removing bounds check
const TsdfVoxel* volData = volUnitsData.ptr<TsdfVoxel>(it->second.index);
int coordBase = volUnitLocalIdx[0] * volStrides[0] + volUnitLocalIdx[1] * volStrides[1] + volUnitLocalIdx[2] * volStrides[2];
return volData[coordBase];
}
TsdfVoxel new_atVolumeUnit(
const Mat& volUnitsDataCopy,
const Vec3i& point, const Vec3i& volumeUnitIdx, const int indx,
const int volumeUnitDegree, const Vec4i volStrides)
{
if (indx < 0)
{
return TsdfVoxel(floatToTsdf(1.f), 0);
}
Vec3i volUnitLocalIdx = point - Vec3i(volumeUnitIdx[0] << volumeUnitDegree,
volumeUnitIdx[1] << volumeUnitDegree,
volumeUnitIdx[2] << volumeUnitDegree);
// expanding at(), removing bounds check
const TsdfVoxel* volData = volUnitsDataCopy.ptr<TsdfVoxel>(indx);
int coordBase = volUnitLocalIdx[0] * volStrides[0] +
volUnitLocalIdx[1] * volStrides[1] +
volUnitLocalIdx[2] * volStrides[2];
return volData[coordBase];
}
Point3f getNormalVoxel(
const Point3f& point, const float voxelSizeInv,
const int volumeUnitDegree, const Vec4i volStrides,
const Mat& volUnitsDataCopy, const VolumeUnitIndexes& volumeUnits)
{
Vec3f normal = Vec3f(0, 0, 0);
Point3f ptVox = point * voxelSizeInv;
Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z));
// A small hash table to reduce a number of find() calls
bool queried[8];
VolumeUnitIndexes::const_iterator iterMap[8];
for (int i = 0; i < 8; i++)
{
iterMap[i] = volumeUnits.end();
queried[i] = false;
}
#if !USE_INTERPOLATION_IN_GETNORMAL
const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3
{ 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7
};
const int nVals = 6;
#else
const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3
{ 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7
{-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11
{ 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15
{ 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19
{ 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23
{ 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27
{ 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31
};
const int nVals = 32;
#endif
float vals[nVals];
for (int i = 0; i < nVals; i++)
{
Vec3i pt = iptVox + offsets[i];
Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree);
int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4;
auto it = iterMap[dictIdx];
if (!queried[dictIdx])
{
it = volumeUnits.find(volumeUnitIdx);
iterMap[dictIdx] = it;
queried[dictIdx] = true;
}
vals[i] = tsdfToFloat(atVolumeUnit(volUnitsDataCopy, volumeUnits, pt, volumeUnitIdx, it, volumeUnitDegree, volStrides).tsdf);
}
#if !USE_INTERPOLATION_IN_GETNORMAL
for (int c = 0; c < 3; c++)
{
normal[c] = vals[c * 2] - vals[c * 2 + 1];
}
#else
float cxv[8], cyv[8], czv[8];
// How these numbers were obtained:
// 1. Take the basic interpolation sequence:
// 000, 001, 010, 011, 100, 101, 110, 111
// where each digit corresponds to shift by x, y, z axis respectively.
// 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis
// 3. Search corresponding values in offsets
const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 };
const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 };
const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 };
const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 };
const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 };
const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 };
#if !USE_INTRINSICS
for (int i = 0; i < 8; i++)
{
cxv[i] = vals[idxxn[i]] - vals[idxxp[i]];
cyv[i] = vals[idxyn[i]] - vals[idxyp[i]];
czv[i] = vals[idxzn[i]] - vals[idxzp[i]];
}
#else
# if CV_SIMD_WIDTH >= 32
v_float32x8 cxp = v_lut(vals, v256_load(idxxp));
v_float32x8 cxn = v_lut(vals, v256_load(idxxn));
v_float32x8 cyp = v_lut(vals, v256_load(idxyp));
v_float32x8 cyn = v_lut(vals, v256_load(idxyn));
v_float32x8 czp = v_lut(vals, v256_load(idxzp));
v_float32x8 czn = v_lut(vals, v256_load(idxzn));
v_float32x8 vcxv = v_sub(cxn, cxp);
v_float32x8 vcyv = v_sub(cyn, cyp);
v_float32x8 vczv = v_sub(czn, czp);
v_store(cxv, vcxv);
v_store(cyv, vcyv);
v_store(czv, vczv);
# else
v_float32x4 cxp0 = v_lut(vals, idxxp + 0); v_float32x4 cxp1 = v_lut(vals, idxxp + 4);
v_float32x4 cxn0 = v_lut(vals, idxxn + 0); v_float32x4 cxn1 = v_lut(vals, idxxn + 4);
v_float32x4 cyp0 = v_lut(vals, idxyp + 0); v_float32x4 cyp1 = v_lut(vals, idxyp + 4);
v_float32x4 cyn0 = v_lut(vals, idxyn + 0); v_float32x4 cyn1 = v_lut(vals, idxyn + 4);
v_float32x4 czp0 = v_lut(vals, idxzp + 0); v_float32x4 czp1 = v_lut(vals, idxzp + 4);
v_float32x4 czn0 = v_lut(vals, idxzn + 0); v_float32x4 czn1 = v_lut(vals, idxzn + 4);
v_float32x4 cxv0 = v_sub(cxn0, cxp0); v_float32x4 cxv1 = v_sub(cxn1, cxp1);
v_float32x4 cyv0 = v_sub(cyn0, cyp0); v_float32x4 cyv1 = v_sub(cyn1, cyp1);
v_float32x4 czv0 = v_sub(czn0, czp0); v_float32x4 czv1 = v_sub(czn1, czp1);
v_store(cxv + 0, cxv0); v_store(cxv + 4, cxv1);
v_store(cyv + 0, cyv0); v_store(cyv + 4, cyv1);
v_store(czv + 0, czv0); v_store(czv + 4, czv1);
#endif
#endif
float tx = ptVox.x - iptVox[0];
float ty = ptVox.y - iptVox[1];
float tz = ptVox.z - iptVox[2];
normal[0] = interpolate(tx, ty, tz, cxv);
normal[1] = interpolate(tx, ty, tz, cyv);
normal[2] = interpolate(tx, ty, tz, czv);
#endif
float nv = sqrt(normal[0] * normal[0] +
normal[1] * normal[1] +
normal[2] * normal[2]);
return nv < 0.0001f ? nan3 : normal / nv;
}
#ifdef HAVE_OPENCL
Point3f ocl_getNormalVoxel(
const Point3f& point, const float voxelSizeInv,
const int volumeUnitDegree, const Vec4i volStrides,
const Mat& volUnitsData, const CustomHashSet& hashTable)
{
Vec3f normal = Vec3f(0, 0, 0);
Point3f ptVox = point * voxelSizeInv;
Vec3i iptVox(cvFloor(ptVox.x), cvFloor(ptVox.y), cvFloor(ptVox.z));
// A small hash table to reduce a number of find() calls
// -2 and lower means not queried yet
// -1 means not found
// 0+ means found
int iterMap[8];
for (int i = 0; i < 8; i++)
{
iterMap[i] = -2;
}
#if !USE_INTERPOLATION_IN_GETNORMAL
const Vec3i offsets[] = { { 1, 0, 0}, {-1, 0, 0}, { 0, 1, 0}, // 0-3
{ 0, -1, 0}, { 0, 0, 1}, { 0, 0, -1} // 4-7
};
const int nVals = 6;
#else
const Vec3i offsets[] = { { 0, 0, 0}, { 0, 0, 1}, { 0, 1, 0}, { 0, 1, 1}, // 0-3
{ 1, 0, 0}, { 1, 0, 1}, { 1, 1, 0}, { 1, 1, 1}, // 4-7
{-1, 0, 0}, {-1, 0, 1}, {-1, 1, 0}, {-1, 1, 1}, // 8-11
{ 2, 0, 0}, { 2, 0, 1}, { 2, 1, 0}, { 2, 1, 1}, // 12-15
{ 0, -1, 0}, { 0, -1, 1}, { 1, -1, 0}, { 1, -1, 1}, // 16-19
{ 0, 2, 0}, { 0, 2, 1}, { 1, 2, 0}, { 1, 2, 1}, // 20-23
{ 0, 0, -1}, { 0, 1, -1}, { 1, 0, -1}, { 1, 1, -1}, // 24-27
{ 0, 0, 2}, { 0, 1, 2}, { 1, 0, 2}, { 1, 1, 2}, // 28-31
};
const int nVals = 32;
#endif
float vals[nVals];
for (int i = 0; i < nVals; i++)
{
Vec3i pt = iptVox + offsets[i];
Vec3i volumeUnitIdx = Vec3i(pt[0] >> volumeUnitDegree, pt[1] >> volumeUnitDegree, pt[2] >> volumeUnitDegree);
int dictIdx = (volumeUnitIdx[0] & 1) + (volumeUnitIdx[1] & 1) * 2 + (volumeUnitIdx[2] & 1) * 4;
auto it = iterMap[dictIdx];
if (it < -1)
{
it = hashTable.find(volumeUnitIdx);
iterMap[dictIdx] = it;
}
vals[i] = tsdfToFloat(new_atVolumeUnit(volUnitsData, pt, volumeUnitIdx, it, volumeUnitDegree, volStrides).tsdf);
}
#if !USE_INTERPOLATION_IN_GETNORMAL
for (int c = 0; c < 3; c++)
{
normal[c] = vals[c * 2] - vals[c * 2 + 1];
}
#else
float cxv[8], cyv[8], czv[8];
// How these numbers were obtained:
// 1. Take the basic interpolation sequence:
// 000, 001, 010, 011, 100, 101, 110, 111
// where each digit corresponds to shift by x, y, z axis respectively.
// 2. Add +1 for next or -1 for prev to each coordinate to corresponding axis
// 3. Search corresponding values in offsets
const int idxxp[8] = { 8, 9, 10, 11, 0, 1, 2, 3 };
const int idxxn[8] = { 4, 5, 6, 7, 12, 13, 14, 15 };
const int idxyp[8] = { 16, 17, 0, 1, 18, 19, 4, 5 };
const int idxyn[8] = { 2, 3, 20, 21, 6, 7, 22, 23 };
const int idxzp[8] = { 24, 0, 25, 2, 26, 4, 27, 6 };
const int idxzn[8] = { 1, 28, 3, 29, 5, 30, 7, 31 };
#if !USE_INTRINSICS
for (int i = 0; i < 8; i++)
{
cxv[i] = vals[idxxn[i]] - vals[idxxp[i]];
cyv[i] = vals[idxyn[i]] - vals[idxyp[i]];
czv[i] = vals[idxzn[i]] - vals[idxzp[i]];
}
#else
# if CV_SIMD_WIDTH >= 32
v_float32x8 cxp = v_lut(vals, v256_load(idxxp));
v_float32x8 cxn = v_lut(vals, v256_load(idxxn));
v_float32x8 cyp = v_lut(vals, v256_load(idxyp));
v_float32x8 cyn = v_lut(vals, v256_load(idxyn));
v_float32x8 czp = v_lut(vals, v256_load(idxzp));
v_float32x8 czn = v_lut(vals, v256_load(idxzn));
v_float32x8 vcxv = v_sub(cxn, cxp);
v_float32x8 vcyv = v_sub(cyn, cyp);
v_float32x8 vczv = v_sub(czn, czp);
v_store(cxv, vcxv);
v_store(cyv, vcyv);
v_store(czv, vczv);
# else
v_float32x4 cxp0 = v_lut(vals, idxxp + 0); v_float32x4 cxp1 = v_lut(vals, idxxp + 4);
v_float32x4 cxn0 = v_lut(vals, idxxn + 0); v_float32x4 cxn1 = v_lut(vals, idxxn + 4);
v_float32x4 cyp0 = v_lut(vals, idxyp + 0); v_float32x4 cyp1 = v_lut(vals, idxyp + 4);
v_float32x4 cyn0 = v_lut(vals, idxyn + 0); v_float32x4 cyn1 = v_lut(vals, idxyn + 4);
v_float32x4 czp0 = v_lut(vals, idxzp + 0); v_float32x4 czp1 = v_lut(vals, idxzp + 4);
v_float32x4 czn0 = v_lut(vals, idxzn + 0); v_float32x4 czn1 = v_lut(vals, idxzn + 4);
v_float32x4 cxv0 = v_sub(cxn0, cxp0); v_float32x4 cxv1 = v_sub(cxn1, cxp1);
v_float32x4 cyv0 = v_sub(cyn0, cyp0); v_float32x4 cyv1 = v_sub(cyn1, cyp1);
v_float32x4 czv0 = v_sub(czn0, czp0); v_float32x4 czv1 = v_sub(czn1, czp1);
v_store(cxv + 0, cxv0); v_store(cxv + 4, cxv1);
v_store(cyv + 0, cyv0); v_store(cyv + 4, cyv1);
v_store(czv + 0, czv0); v_store(czv + 4, czv1);
#endif
#endif
float tx = ptVox.x - iptVox[0];
float ty = ptVox.y - iptVox[1];
float tz = ptVox.z - iptVox[2];
normal[0] = interpolate(tx, ty, tz, cxv);
normal[1] = interpolate(tx, ty, tz, cyv);
normal[2] = interpolate(tx, ty, tz, czv);
#endif
float nv = sqrt(normal[0] * normal[0] +
normal[1] * normal[1] +
normal[2] * normal[2]);
return nv < 0.0001f ? nan3 : normal / nv;
}
#endif
void raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits, OutputArray _points, OutputArray _normals)
{
CV_TRACE_FUNCTION();
Size frameSize(width, height);
CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
Mat volUnitsData = _volUnitsData.getMat();
_points.create(frameSize, POINT_TYPE);
_normals.create(frameSize, POINT_TYPE);
Points points1 = _points.getMat();
Normals normals1 = _normals.getMat();
Points& points(points1);
Normals& normals(normals1);
const float truncDist = settings.getTsdfTruncateDistance();
const float raycastStepFactor = settings.getRaycastStepFactor();
const float tstep = truncDist * raycastStepFactor;
const float maxDepth = settings.getMaxDepth();
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims;
settings.getVolumeStrides(volDims);
Vec3i resolution;
settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution);
const float volumeUnitSize = voxelSize * resolution[0];
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
const Intr intrinsics(mintr);
const Intr::Reprojector reproj(intrinsics.makeReprojector());
const int nstripes = -1;
auto _HashRaycastInvoker = [&](const Range& range)
{
const Point3f cam2volTrans = cam2vol.translation();
const Matx33f cam2volRot = cam2vol.rotation();
const Matx33f vol2camRot = vol2cam.rotation();
const float blockSize = volumeUnitSize;
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++)
{
//! Initialize default value
Point3f point = nan3, normal = nan3;
//! Ray origin and direction in the volume coordinate frame
Point3f orig = cam2volTrans;
Point3f rayDirV = normalize(Vec3f(cam2volRot * reproj(Point3f(float(x), float(y), 1.f))));
float tmin = 0;
float tmax = maxDepth;
float tcurr = tmin;
cv::Vec3i prevVolumeUnitIdx =
cv::Vec3i(std::numeric_limits<int>::min(), std::numeric_limits<int>::min(),
std::numeric_limits<int>::min());
float tprev = tcurr;
float prevTsdf = truncDist;
float currTsdf = truncDist;
while (tcurr < tmax)
{
Point3f currRayPos = orig + tcurr * rayDirV;
cv::Vec3i currVolumeUnitIdx = volumeToVolumeUnitIdx(currRayPos, volumeUnitSize);
VolumeUnitIndexes::const_iterator it = volumeUnits.find(currVolumeUnitIdx);
currTsdf = prevTsdf;
float stepSize = 0.5f * blockSize;
cv::Vec3i volUnitLocalIdx;
//! The subvolume exists in hashtable
if (it != volumeUnits.end())
{
cv::Point3f currVolUnitPos = volumeUnitIdxToVolume(currVolumeUnitIdx, volumeUnitSize);
volUnitLocalIdx = volumeToVoxelCoord(currRayPos - currVolUnitPos, voxelSizeInv);
//! TODO: Figure out voxel interpolation
TsdfVoxel currVoxel = _at(volUnitsData, volUnitLocalIdx, it->second.index, volResolution.x, volDims);
currTsdf = tsdfToFloat(currVoxel.tsdf);
if (currTsdf != prevTsdf)
{
// from pos to zero or negative
// or from neg to zero or positive
bool posCurr = currTsdf > 0.f, posPrev = prevTsdf > 0.f;
bool negCurr = currTsdf < 0.f, negPrev = prevTsdf < 0.f;
if (posCurr != posPrev || negCurr != negPrev)
break;
}
stepSize = tstep;
}
prevVolumeUnitIdx = currVolumeUnitIdx;
prevTsdf = currTsdf;
tprev = tcurr;
tcurr += stepSize;
}
//! Surface crossing
if (prevTsdf >= 0.f && currTsdf <= 0.f && prevTsdf > currTsdf)
{
float tInterp = (tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf);
if (!cvIsNaN(tInterp) && !cvIsInf(tInterp))
{
Point3f pv = orig + tInterp * rayDirV;
Point3f nv = getNormalVoxel(pv, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits);
if (!isNaN(nv))
{
normal = vol2camRot * nv;
point = vol2cam * pv;
}
}
}
ptsRow[x] = toPtype(point);
nrmRow[x] = toPtype(normal);
}
}
};
parallel_for_(Range(0, points.rows), _HashRaycastInvoker, nstripes);
}
#ifdef HAVE_OPENCL
void ocl_raycastHashTsdfVolumeUnit(
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, const int volumeUnitDegree,
const CustomHashSet& hashTable, InputArray _volUnitsData, OutputArray _points, OutputArray _normals)
{
CV_TRACE_FUNCTION();
Size frameSize(width, height);
CV_Assert(frameSize.area() > 0);
Matx33f mintr(intr.getMat());
UMat volUnitsData = _volUnitsData.getUMat();
String errorStr;
String name = "raycast";
ocl::ProgramSource source = ocl::ptcloud::hash_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();
Intr intrinsics(mintr);
Intr::Reprojector r = intrinsics.makeReprojector();
Vec2f finv(r.fxinv, r.fyinv), cxy(r.cx, r.cy);
const float truncDist = settings.getTsdfTruncateDistance();
const float raycastStepFactor = settings.getRaycastStepFactor();
const float tstep = truncDist * raycastStepFactor;
const float maxDepth = settings.getMaxDepth();
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volStrides;
settings.getVolumeStrides(volStrides);
Vec3i resolution;
settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution);
const float volumeUnitSize = voxelSize * volResolution.x;
Vec4f boxMin, boxMax(volumeUnitSize - voxelSize,
volumeUnitSize - voxelSize,
volumeUnitSize - voxelSize);
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
Matx44f cam2volRotGPU = cam2vol.matrix;
Matx44f vol2camRotGPU = vol2cam.matrix;
UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ);
UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ);
k.args(
ocl::KernelArg::PtrReadOnly(hashesGpu),
ocl::KernelArg::PtrReadOnly(hashDataGpu),
ocl::KernelArg::WriteOnlyNoSize(points),
ocl::KernelArg::WriteOnlyNoSize(normals),
frameSize,
ocl::KernelArg::ReadOnly(volUnitsData),
cam2volRotGPU,
vol2camRotGPU,
float(maxDepth),
finv.val, cxy.val,
boxMin.val, boxMax.val,
tstep,
voxelSize,
voxelSizeInv,
volumeUnitSize,
truncDist,
volumeUnitDegree,
volStrides
);
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
void fetchNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
const int volumeUnitDegree, InputArray _points, OutputArray _normals)
{
CV_TRACE_FUNCTION();
if (!_normals.needed())
return;
Points points = _points.getMat();
CV_Assert(points.type() == POINT_TYPE);
_normals.createSameSize(_points, _points.type());
Normals normals = _normals.getMat();
Mat volUnitsData = _volUnitsData.getMat();
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims;
settings.getVolumeStrides(volDims);
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose(_pose);
auto HashPushNormals = [&](const ptype& point, const int* position) {
Affine3f invPose(pose.inv());
Point3f p = fromPtype(point);
Point3f n = nan3;
if (!isNaN(p))
{
Point3f voxelPoint = invPose * p;
n = pose.rotation() * getNormalVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits);
}
normals(position[0], position[1]) = toPtype(n);
};
points.forEach(HashPushNormals);
}
#ifdef HAVE_OPENCL
void ocl_fetchNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
const CustomHashSet& hashTable, InputArray _points, OutputArray _normals)
{
CV_TRACE_FUNCTION();
UMat volUnitsData = _volUnitsData.getUMat();
Mat volUnitsDataCopy = _volUnitsDataCopy.getMat();
if (!_normals.needed())
return;
//TODO: remove it when it works w/o CPU code
volUnitsData.copyTo(volUnitsDataCopy);
Points points = _points.getMat();
CV_Assert(points.type() == POINT_TYPE);
_normals.createSameSize(_points, _points.type());
Normals normals = _normals.getMat();
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
const Vec4i volDims;
settings.getVolumeStrides(volDims);
Matx44f _pose;
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
auto HashPushNormals = [&](const ptype& point, const int* position) {
Affine3f invPose(pose.inv());
Point3f p = fromPtype(point);
Point3f n = nan3;
if (!isNaN(p))
{
Point3f voxelPoint = invPose * p;
n = pose.rotation() * ocl_getNormalVoxel(voxelPoint, voxelSizeInv, volumeUnitDegree, volDims, volUnitsDataCopy, hashTable);
}
normals(position[0], position[1]) = toPtype(n);
};
points.forEach(HashPushNormals);
}
#endif
void fetchPointsNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, InputArray _volUnitsData, const VolumeUnitIndexes& volumeUnits,
const int volumeUnitDegree, OutputArray _points, OutputArray _normals)
{
//std::cout << "fetchNormalsFromHashTsdfVolumeUnit()" << std::endl;
CV_TRACE_FUNCTION();
if (!_points.needed())
return;
std::vector<std::vector<ptype>> pVecs, nVecs;
Mat volUnitsData = _volUnitsData.getMat();
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
Vec3i resolution;
settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution);
const int volumeUnitResolution = volResolution.x;
const float volumeUnitSize = voxelSize * resolution[0];
const Vec4i volDims;
settings.getVolumeStrides(volDims);
Matx44f mpose;
settings.getVolumePose(mpose);
const Affine3f pose(mpose);
std::vector<Vec3i> totalVolUnits;
for (const auto& keyvalue : volumeUnits)
{
totalVolUnits.push_back(keyvalue.first);
}
Range fetchRange(0, (int)totalVolUnits.size());
const int nstripes = -1;
bool needNormals(_normals.needed());
Mutex mutex;
//TODO: this is incorrect; a 0-surface should be captured instead of all non-zero voxels
auto HashFetchPointsNormalsInvoker = [&](const Range& range)
{
std::vector<ptype> points, normals;
for (int i = range.start; i < range.end; i++)
{
cv::Vec3i tsdf_idx = totalVolUnits[i];
VolumeUnitIndexes::const_iterator it = volumeUnits.find(tsdf_idx);
Point3f base_point = volumeUnitIdxToVolume(tsdf_idx, volumeUnitSize);
if (it != volumeUnits.end())
{
std::vector<ptype> localPoints;
std::vector<ptype> localNormals;
for (int x = 0; x < volumeUnitResolution; x++)
for (int y = 0; y < volumeUnitResolution; y++)
for (int z = 0; z < volumeUnitResolution; z++)
{
cv::Vec3i voxelIdx(x, y, z);
TsdfVoxel voxel = _at(volUnitsData, voxelIdx, it->second.index, volResolution.x, volDims);
// floatToTsdf(1.0) == -128
if (voxel.tsdf != -128 && voxel.weight != 0)
{
Point3f point = base_point + voxelCoordToVolume(voxelIdx, voxelSize);
localPoints.push_back(toPtype(pose * point));
if (needNormals)
{
Point3f normal = getNormalVoxel(point, voxelSizeInv, volumeUnitDegree, volDims, volUnitsData, volumeUnits);
localNormals.push_back(toPtype(pose.rotation() * normal));
}
}
}
AutoLock al(mutex);
pVecs.push_back(localPoints);
nVecs.push_back(localNormals);
}
}
};
parallel_for_(fetchRange, HashFetchPointsNormalsInvoker, 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
inline TsdfVoxel new_at(
Mat& volUnitsDataCopy, const cv::Vec3i& volumeIdx, int indx,
const int volumeUnitResolution, const Vec4i volStrides)
{
//! Out of bounds
if ((volumeIdx[0] >= volumeUnitResolution || volumeIdx[0] < 0) ||
(volumeIdx[1] >= volumeUnitResolution || volumeIdx[1] < 0) ||
(volumeIdx[2] >= volumeUnitResolution || volumeIdx[2] < 0))
{
return TsdfVoxel(floatToTsdf(1.0f), 0);
}
const TsdfVoxel* volData = volUnitsDataCopy.ptr<TsdfVoxel>(indx);
int coordBase =
volumeIdx[0] * volStrides[0] +
volumeIdx[1] * volStrides[1] +
volumeIdx[2] * volStrides[2];
return volData[coordBase];
}
void ocl_fetchPointsNormalsFromHashTsdfVolumeUnit(
const VolumeSettings& settings, const int volumeUnitDegree, InputArray _volUnitsData, InputArray _volUnitsDataCopy,
const CustomHashSet& hashTable, OutputArray _points, OutputArray _normals)
{
CV_TRACE_FUNCTION();
if (!_points.needed())
return;
UMat volUnitsData = _volUnitsData.getUMat();
Mat volUnitsDataCopy = _volUnitsDataCopy.getMat();
//TODO: remove it when it works w/o CPU code
volUnitsData.copyTo(volUnitsDataCopy);
//TODO: remove it when it works w/o CPU code
//TODO: enable it when it's on GPU
//UMat hashDataGpu(hashMap.capacity, 1, CV_32SC4);
//Mat(hashMap.data, false).copyTo(hashDataGpu);
std::vector<std::vector<ptype>> pVecs, nVecs;
const float voxelSize = settings.getVoxelSize();
const float voxelSizeInv = 1.f / voxelSize;
Vec3i resolution;
settings.getVolumeResolution(resolution);
const Point3i volResolution = Point3i(resolution);
const int volumeUnitResolution = volResolution.x;
const float volumeUnitSize = voxelSize * resolution[0];
const Vec4i volDims;
settings.getVolumeStrides(volDims);
Matx44f mpose;
settings.getVolumePose(mpose);
const Affine3f pose(mpose);
Range _fetchRange(0, hashTable.last);
const int nstripes = -1;
bool needNormals(_normals.needed());
Mutex mutex;
//TODO: this is incorrect; a 0-surface should be captured instead of all non-zero voxels
auto _HashFetchPointsNormalsInvoker = [&](const Range& range)
{
std::vector<ptype> points, normals;
for (int row = range.start; row < range.end; row++)
{
cv::Vec4i idx4 = hashTable.data[row];
cv::Vec3i idx(idx4[0], idx4[1], idx4[2]);
Point3f base_point = volumeUnitIdxToVolume(idx, volumeUnitSize);
std::vector<ptype> localPoints;
std::vector<ptype> localNormals;
for (int x = 0; x < volumeUnitResolution; x++)
for (int y = 0; y < volumeUnitResolution; y++)
for (int z = 0; z < volumeUnitResolution; z++)
{
cv::Vec3i voxelIdx(x, y, z);
TsdfVoxel voxel = new_at(volUnitsDataCopy, voxelIdx, row, volumeUnitResolution, volDims);
// floatToTsdf(1.0) == -128
if (voxel.tsdf != -128 && voxel.weight != 0)
{
Point3f point = base_point + voxelCoordToVolume(voxelIdx, voxelSize);
localPoints.push_back(toPtype(pose * point));
if (needNormals)
{
Point3f normal = ocl_getNormalVoxel(point, voxelSizeInv, volumeUnitDegree, volDims, volUnitsDataCopy, hashTable);
localNormals.push_back(toPtype(pose.rotation() * normal));
}
}
}
AutoLock al(mutex);
pVecs.push_back(localPoints);
nVecs.push_back(localNormals);
}
};
parallel_for_(_fetchRange, _HashFetchPointsNormalsInvoker, 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());
}
}
#endif
} // namespace cv