mirror of
https://github.com/opencv/opencv.git
synced 2026-11-16 06:17:53 -06:00
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
1553 lines
55 KiB
C++
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
|