Merge pull request #2566 from akashsharma02:master
[GSoC] Add new Hashtable based TSDF volume to RGBD module
* - Add HashTSDF class
- Implement Integrate function (untested)
* Integration seems to be working, raycasting does not
* Update integration code
* Integration and Raycasting fixes, (both work now)
* - Format code
- Clean up comments and few fixes
* Add Kinect Fusion backup file
* - Add interpolation for vertices and normals (slow and unreliable!)
- Format code
- Delete kinfu_back.cpp
* Bug fix for integration and noisy odometry
* - Create volume abstract class
- Address Review comments
* - Add getPoints and getNormals function
- Fix formatting according to comments
- Move volume abstract class to include/opencv2/rgbd/
- Write factory method for creating TSDFVolumes
- Small bug fixes
- Minor fixes according to comments
* - Add tests for hashTSDF
- Fix raycasting bug causing to loop forever
- Suppress warnings by explicit conversion
- Disable hashTsdf test until we figure out memory leak
- style changes
- Add missing license in a few files, correct precomp.hpp usage
pull/2595/head
Akash Sharma
5 years ago
committed by
GitHub
parent
52200a82e7
commit
468345511f
15 changed files with 1250 additions and 394 deletions
@ -0,0 +1,81 @@ |
||||
// 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
|
||||
|
||||
#ifndef __OPENCV_RGBD_INTRINSICS_HPP__ |
||||
#define __OPENCV_RGBD_INTRINSICS_HPP__ |
||||
|
||||
#include "opencv2/core/matx.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
|
||||
struct Intr |
||||
{ |
||||
/** @brief Camera intrinsics */ |
||||
/** Reprojects screen point to camera space given z coord. */ |
||||
struct Reprojector |
||||
{ |
||||
Reprojector() {} |
||||
inline Reprojector(Intr intr) |
||||
{ |
||||
fxinv = 1.f/intr.fx, fyinv = 1.f/intr.fy; |
||||
cx = intr.cx, cy = intr.cy; |
||||
} |
||||
template<typename T> |
||||
inline cv::Point3_<T> operator()(cv::Point3_<T> p) const |
||||
{ |
||||
T x = p.z * (p.x - cx) * fxinv; |
||||
T y = p.z * (p.y - cy) * fyinv; |
||||
return cv::Point3_<T>(x, y, p.z); |
||||
} |
||||
|
||||
float fxinv, fyinv, cx, cy; |
||||
}; |
||||
|
||||
/** Projects camera space vector onto screen */ |
||||
struct Projector |
||||
{ |
||||
inline Projector(Intr intr) : fx(intr.fx), fy(intr.fy), cx(intr.cx), cy(intr.cy) { } |
||||
template<typename T> |
||||
inline cv::Point_<T> operator()(cv::Point3_<T> p) const |
||||
{ |
||||
T invz = T(1)/p.z; |
||||
T x = fx*(p.x*invz) + cx; |
||||
T y = fy*(p.y*invz) + cy; |
||||
return cv::Point_<T>(x, y); |
||||
} |
||||
template<typename T> |
||||
inline cv::Point_<T> operator()(cv::Point3_<T> p, cv::Point3_<T>& pixVec) const |
||||
{ |
||||
T invz = T(1)/p.z; |
||||
pixVec = cv::Point3_<T>(p.x*invz, p.y*invz, 1); |
||||
T x = fx*pixVec.x + cx; |
||||
T y = fy*pixVec.y + cy; |
||||
return cv::Point_<T>(x, y); |
||||
} |
||||
float fx, fy, cx, cy; |
||||
}; |
||||
Intr() : fx(), fy(), cx(), cy() { } |
||||
Intr(float _fx, float _fy, float _cx, float _cy) : fx(_fx), fy(_fy), cx(_cx), cy(_cy) { } |
||||
Intr(cv::Matx33f m) : fx(m(0, 0)), fy(m(1, 1)), cx(m(0, 2)), cy(m(1, 2)) { } |
||||
// scale intrinsics to pyramid level
|
||||
inline Intr scale(int pyr) const |
||||
{ |
||||
float factor = (1.f /(1 << pyr)); |
||||
return Intr(fx*factor, fy*factor, cx*factor, cy*factor); |
||||
} |
||||
inline Reprojector makeReprojector() const { return Reprojector(*this); } |
||||
inline Projector makeProjector() const { return Projector(*this); } |
||||
|
||||
inline cv::Matx33f getMat() const { return Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1); } |
||||
|
||||
float fx, fy, cx, cy; |
||||
}; |
||||
|
||||
} // namespace rgbd
|
||||
} // namespace cv
|
||||
|
||||
#endif |
||||
@ -0,0 +1,58 @@ |
||||
// 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
|
||||
|
||||
// This code is also subject to the license terms in the LICENSE_KinectFusion.md file found in this
|
||||
// module's directory
|
||||
|
||||
#ifndef __OPENCV_RGBD_VOLUME_H__ |
||||
#define __OPENCV_RGBD_VOLUME_H__ |
||||
|
||||
#include "intrinsics.hpp" |
||||
#include "opencv2/core/affine.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
class Volume |
||||
{ |
||||
public: |
||||
Volume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor) |
||||
: voxelSize(_voxelSize), |
||||
voxelSizeInv(1.0f / voxelSize), |
||||
pose(_pose), |
||||
raycastStepFactor(_raycastStepFactor) |
||||
{ |
||||
} |
||||
|
||||
virtual ~Volume(){}; |
||||
|
||||
virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, |
||||
const cv::kinfu::Intr& intrinsics) = 0; |
||||
virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, |
||||
cv::Size frameSize, cv::OutputArray points, |
||||
cv::OutputArray normals) const = 0; |
||||
virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const = 0; |
||||
virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const = 0; |
||||
virtual void reset() = 0; |
||||
|
||||
public: |
||||
const float voxelSize; |
||||
const float voxelSizeInv; |
||||
const cv::Affine3f pose; |
||||
const float raycastStepFactor; |
||||
}; |
||||
|
||||
enum class VolumeType |
||||
{ |
||||
TSDF = 0, |
||||
HASHTSDF = 1 |
||||
}; |
||||
|
||||
cv::Ptr<Volume> makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, |
||||
float _raycastStepFactor, float _truncDist, int _maxWeight, |
||||
float _truncateThreshold, Point3i _resolution); |
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
#endif |
||||
@ -0,0 +1,578 @@ |
||||
// 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
|
||||
#include "precomp.hpp" |
||||
#include "hash_tsdf.hpp" |
||||
|
||||
#include <atomic> |
||||
#include <functional> |
||||
#include <iostream> |
||||
#include <limits> |
||||
#include <vector> |
||||
|
||||
#include "kinfu_frame.hpp" |
||||
#include "opencv2/core/cvstd.hpp" |
||||
#include "opencv2/core/utility.hpp" |
||||
#include "opencv2/core/utils/trace.hpp" |
||||
#include "utils.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
HashTSDFVolume::HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, |
||||
float _truncDist, int _maxWeight, float _truncateThreshold, |
||||
int _volumeUnitRes, bool _zFirstMemOrder) |
||||
: Volume(_voxelSize, _pose, _raycastStepFactor), |
||||
maxWeight(_maxWeight), |
||||
truncateThreshold(_truncateThreshold), |
||||
volumeUnitResolution(_volumeUnitRes), |
||||
volumeUnitSize(voxelSize * volumeUnitResolution), |
||||
zFirstMemOrder(_zFirstMemOrder) |
||||
{ |
||||
truncDist = std::max(_truncDist, 4.0f * voxelSize); |
||||
} |
||||
|
||||
HashTSDFVolumeCPU::HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, |
||||
float _truncDist, int _maxWeight, float _truncateThreshold, |
||||
int _volumeUnitRes, bool _zFirstMemOrder) |
||||
: HashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, |
||||
_truncateThreshold, _volumeUnitRes, _zFirstMemOrder) |
||||
{ |
||||
} |
||||
|
||||
// zero volume, leave rest params the same
|
||||
void HashTSDFVolumeCPU::reset() |
||||
{ |
||||
CV_TRACE_FUNCTION(); |
||||
volumeUnits.clear(); |
||||
} |
||||
|
||||
struct AllocateVolumeUnitsInvoker : ParallelLoopBody |
||||
{ |
||||
AllocateVolumeUnitsInvoker(HashTSDFVolumeCPU& _volume, const Depth& _depth, Intr intrinsics, |
||||
cv::Affine3f cameraPose, float _depthFactor, int _depthStride = 4) |
||||
: ParallelLoopBody(), |
||||
volume(_volume), |
||||
depth(_depth), |
||||
reproj(intrinsics.makeReprojector()), |
||||
cam2vol(_volume.pose.inv() * cameraPose), |
||||
depthFactor(1.0f / _depthFactor), |
||||
depthStride(_depthStride) |
||||
{ |
||||
} |
||||
|
||||
virtual void operator()(const Range& range) const override |
||||
{ |
||||
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] * depthFactor; |
||||
if (z <= 0) |
||||
continue; |
||||
|
||||
Point3f camPoint = reproj(Point3f((float)x, (float)y, z)); |
||||
Point3f volPoint = cam2vol * camPoint; |
||||
|
||||
//! Find accessed TSDF volume unit for valid 3D vertex
|
||||
cv::Vec3i lower_bound = volume.volumeToVolumeUnitIdx( |
||||
volPoint - cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); |
||||
cv::Vec3i upper_bound = volume.volumeToVolumeUnitIdx( |
||||
volPoint + cv::Point3f(volume.truncDist, volume.truncDist, volume.truncDist)); |
||||
VolumeUnitIndexSet localAccessVolUnits; |
||||
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 <= lower_bound[2]; k++) |
||||
{ |
||||
const cv::Vec3i tsdf_idx = cv::Vec3i(i, j, k); |
||||
if (!localAccessVolUnits.count(tsdf_idx)) |
||||
{ |
||||
localAccessVolUnits.emplace(tsdf_idx); |
||||
} |
||||
} |
||||
AutoLock al(mutex); |
||||
for (const auto& tsdf_idx : localAccessVolUnits) |
||||
{ |
||||
//! If the insert into the global set passes
|
||||
if (!volume.volumeUnits.count(tsdf_idx)) |
||||
{ |
||||
VolumeUnit volumeUnit; |
||||
cv::Point3i volumeDims(volume.volumeUnitResolution, |
||||
volume.volumeUnitResolution, |
||||
volume.volumeUnitResolution); |
||||
|
||||
cv::Affine3f subvolumePose = |
||||
volume.pose.translate(volume.volumeUnitIdxToVolume(tsdf_idx)); |
||||
volumeUnit.pVolume = cv::makePtr<TSDFVolumeCPU>( |
||||
volume.voxelSize, subvolumePose, volume.raycastStepFactor, |
||||
volume.truncDist, volume.maxWeight, volumeDims); |
||||
//! This volume unit will definitely be required for current integration
|
||||
volumeUnit.index = tsdf_idx; |
||||
volumeUnit.isActive = true; |
||||
volume.volumeUnits[tsdf_idx] = volumeUnit; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
HashTSDFVolumeCPU& volume; |
||||
const Depth& depth; |
||||
const Intr::Reprojector reproj; |
||||
const cv::Affine3f cam2vol; |
||||
const float depthFactor; |
||||
const int depthStride; |
||||
mutable Mutex mutex; |
||||
}; |
||||
|
||||
|
||||
void HashTSDFVolumeCPU::integrate(InputArray _depth, float depthFactor, |
||||
const cv::Affine3f& cameraPose, const Intr& intrinsics) |
||||
{ |
||||
CV_TRACE_FUNCTION(); |
||||
|
||||
CV_Assert(_depth.type() == DEPTH_TYPE); |
||||
Depth depth = _depth.getMat(); |
||||
|
||||
//! Compute volumes to be allocated
|
||||
AllocateVolumeUnitsInvoker allocate_i(*this, depth, intrinsics, cameraPose, depthFactor); |
||||
Range allocateRange(0, depth.rows); |
||||
parallel_for_(allocateRange, allocate_i); |
||||
|
||||
//! Get volumes that are in the current camera frame
|
||||
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(cameraPose.inv() * pose); |
||||
const Intr::Projector proj(intrinsics.makeProjector()); |
||||
|
||||
for (int i = range.start; i < range.end; ++i) |
||||
{ |
||||
cv::Vec3i tsdf_idx = totalVolUnits[i]; |
||||
VolumeUnitMap::iterator it = volumeUnits.find(tsdf_idx); |
||||
if (it == volumeUnits.end()) |
||||
return; |
||||
|
||||
Point3f volumeUnitPos = volumeUnitIdxToVolume(it->first); |
||||
Point3f volUnitInCamSpace = vol2cam * volumeUnitPos; |
||||
if (volUnitInCamSpace.z < 0 || volUnitInCamSpace.z > truncateThreshold) |
||||
{ |
||||
it->second.isActive = false; |
||||
return; |
||||
} |
||||
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.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++) |
||||
{ |
||||
cv::Vec3i tsdf_idx = totalVolUnits[i]; |
||||
VolumeUnitMap::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
|
||||
volumeUnit.pVolume->integrate(depth, depthFactor, cameraPose, intrinsics); |
||||
//! Ensure all active volumeUnits are set to inactive for next integration
|
||||
volumeUnit.isActive = false; |
||||
} |
||||
} |
||||
}); |
||||
} |
||||
|
||||
cv::Vec3i HashTSDFVolumeCPU::volumeToVolumeUnitIdx(cv::Point3f p) const |
||||
{ |
||||
return cv::Vec3i(cvFloor(p.x / volumeUnitSize), cvFloor(p.y / volumeUnitSize), |
||||
cvFloor(p.z / volumeUnitSize)); |
||||
} |
||||
|
||||
cv::Point3f HashTSDFVolumeCPU::volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const |
||||
{ |
||||
return cv::Point3f(volumeUnitIdx[0] * volumeUnitSize, volumeUnitIdx[1] * volumeUnitSize, |
||||
volumeUnitIdx[2] * volumeUnitSize); |
||||
} |
||||
|
||||
cv::Point3f HashTSDFVolumeCPU::voxelCoordToVolume(cv::Vec3i voxelIdx) const |
||||
{ |
||||
return cv::Point3f(voxelIdx[0] * voxelSize, voxelIdx[1] * voxelSize, voxelIdx[2] * voxelSize); |
||||
} |
||||
|
||||
cv::Vec3i HashTSDFVolumeCPU::volumeToVoxelCoord(cv::Point3f point) const |
||||
{ |
||||
return cv::Vec3i(cvFloor(point.x * voxelSizeInv), cvFloor(point.y * voxelSizeInv), |
||||
cvFloor(point.z * voxelSizeInv)); |
||||
} |
||||
|
||||
inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Vec3i& volumeIdx) const |
||||
{ |
||||
cv::Vec3i volumeUnitIdx = cv::Vec3i(cvFloor(volumeIdx[0] / volumeUnitResolution), |
||||
cvFloor(volumeIdx[1] / volumeUnitResolution), |
||||
cvFloor(volumeIdx[2] / volumeUnitResolution)); |
||||
|
||||
VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); |
||||
if (it == volumeUnits.end()) |
||||
{ |
||||
TsdfVoxel dummy; |
||||
dummy.tsdf = 1.f; |
||||
dummy.weight = 0; |
||||
return dummy; |
||||
} |
||||
cv::Ptr<TSDFVolumeCPU> volumeUnit = |
||||
std::dynamic_pointer_cast<TSDFVolumeCPU>(it->second.pVolume); |
||||
|
||||
cv::Vec3i volUnitLocalIdx = volumeIdx - cv::Vec3i(volumeUnitIdx[0] * volumeUnitResolution, |
||||
volumeUnitIdx[1] * volumeUnitResolution, |
||||
volumeUnitIdx[2] * volumeUnitResolution); |
||||
|
||||
volUnitLocalIdx = |
||||
cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); |
||||
return volumeUnit->at(volUnitLocalIdx); |
||||
} |
||||
|
||||
inline TsdfVoxel HashTSDFVolumeCPU::at(const cv::Point3f& point) const |
||||
{ |
||||
cv::Vec3i volumeUnitIdx = volumeToVolumeUnitIdx(point); |
||||
VolumeUnitMap::const_iterator it = volumeUnits.find(volumeUnitIdx); |
||||
if (it == volumeUnits.end()) |
||||
{ |
||||
TsdfVoxel dummy; |
||||
dummy.tsdf = 1.f; |
||||
dummy.weight = 0; |
||||
return dummy; |
||||
} |
||||
cv::Ptr<TSDFVolumeCPU> volumeUnit = |
||||
std::dynamic_pointer_cast<TSDFVolumeCPU>(it->second.pVolume); |
||||
|
||||
cv::Point3f volumeUnitPos = volumeUnitIdxToVolume(volumeUnitIdx); |
||||
cv::Vec3i volUnitLocalIdx = volumeToVoxelCoord(point - volumeUnitPos); |
||||
volUnitLocalIdx = |
||||
cv::Vec3i(abs(volUnitLocalIdx[0]), abs(volUnitLocalIdx[1]), abs(volUnitLocalIdx[2])); |
||||
return volumeUnit->at(volUnitLocalIdx); |
||||
} |
||||
|
||||
inline Point3f HashTSDFVolumeCPU::getNormalVoxel(Point3f point) const |
||||
{ |
||||
Vec3f pointVec(point); |
||||
Vec3f normal = Vec3f(0, 0, 0); |
||||
|
||||
Vec3f pointPrev = point; |
||||
Vec3f pointNext = point; |
||||
|
||||
for (int c = 0; c < 3; c++) |
||||
{ |
||||
pointPrev[c] -= voxelSize * 0.5f; |
||||
pointNext[c] += voxelSize * 0.5f; |
||||
|
||||
normal[c] = at(Point3f(pointNext)).tsdf - at(Point3f(pointPrev)).tsdf; |
||||
normal[c] *= 0.5f; |
||||
|
||||
pointPrev[c] = pointVec[c]; |
||||
pointNext[c] = pointVec[c]; |
||||
} |
||||
return normalize(normal); |
||||
} |
||||
|
||||
struct HashRaycastInvoker : ParallelLoopBody |
||||
{ |
||||
HashRaycastInvoker(Points& _points, Normals& _normals, const Affine3f& cameraPose, |
||||
const Intr& intrinsics, const HashTSDFVolumeCPU& _volume) |
||||
: ParallelLoopBody(), |
||||
points(_points), |
||||
normals(_normals), |
||||
volume(_volume), |
||||
tstep(_volume.truncDist * _volume.raycastStepFactor), |
||||
cam2vol(volume.pose.inv() * cameraPose), |
||||
vol2cam(cameraPose.inv() * volume.pose), |
||||
reproj(intrinsics.makeReprojector()) |
||||
{ |
||||
} |
||||
|
||||
virtual void operator()(const Range& range) const override |
||||
{ |
||||
const Point3f cam2volTrans = cam2vol.translation(); |
||||
const Matx33f cam2volRot = cam2vol.rotation(); |
||||
const Matx33f vol2camRot = vol2cam.rotation(); |
||||
|
||||
const float blockSize = volume.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 = volume.truncateThreshold; |
||||
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; |
||||
TsdfType prevTsdf = volume.truncDist; |
||||
cv::Ptr<TSDFVolumeCPU> currVolumeUnit; |
||||
while (tcurr < tmax) |
||||
{ |
||||
Point3f currRayPos = orig + tcurr * rayDirV; |
||||
cv::Vec3i currVolumeUnitIdx = volume.volumeToVolumeUnitIdx(currRayPos); |
||||
|
||||
VolumeUnitMap::const_iterator it = volume.volumeUnits.find(currVolumeUnitIdx); |
||||
|
||||
TsdfType currTsdf = prevTsdf; |
||||
int currWeight = 0; |
||||
float stepSize = 0.5f * blockSize; |
||||
cv::Vec3i volUnitLocalIdx; |
||||
|
||||
//! Does the subvolume exist in hashtable
|
||||
if (it != volume.volumeUnits.end()) |
||||
{ |
||||
currVolumeUnit = |
||||
std::dynamic_pointer_cast<TSDFVolumeCPU>(it->second.pVolume); |
||||
cv::Point3f currVolUnitPos = |
||||
volume.volumeUnitIdxToVolume(currVolumeUnitIdx); |
||||
volUnitLocalIdx = volume.volumeToVoxelCoord(currRayPos - currVolUnitPos); |
||||
|
||||
//! TODO: Figure out voxel interpolation
|
||||
TsdfVoxel currVoxel = currVolumeUnit->at(volUnitLocalIdx); |
||||
currTsdf = currVoxel.tsdf; |
||||
currWeight = currVoxel.weight; |
||||
stepSize = tstep; |
||||
} |
||||
//! Surface crossing
|
||||
if (prevTsdf > 0.f && currTsdf <= 0.f && currWeight > 0) |
||||
{ |
||||
float tInterp = |
||||
(tcurr * prevTsdf - tprev * currTsdf) / (prevTsdf - currTsdf); |
||||
if (!cvIsNaN(tInterp) && !cvIsInf(tInterp)) |
||||
{ |
||||
Point3f pv = orig + tInterp * rayDirV; |
||||
Point3f nv = volume.getNormalVoxel(pv); |
||||
|
||||
if (!isNaN(nv)) |
||||
{ |
||||
normal = vol2camRot * nv; |
||||
point = vol2cam * pv; |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
prevVolumeUnitIdx = currVolumeUnitIdx; |
||||
prevTsdf = currTsdf; |
||||
tprev = tcurr; |
||||
tcurr += stepSize; |
||||
} |
||||
ptsRow[x] = toPtype(point); |
||||
nrmRow[x] = toPtype(normal); |
||||
} |
||||
} |
||||
} |
||||
|
||||
Points& points; |
||||
Normals& normals; |
||||
const HashTSDFVolumeCPU& volume; |
||||
const float tstep; |
||||
const Affine3f cam2vol; |
||||
const Affine3f vol2cam; |
||||
const Intr::Reprojector reproj; |
||||
}; |
||||
|
||||
void HashTSDFVolumeCPU::raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, |
||||
cv::Size frameSize, cv::OutputArray _points, |
||||
cv::OutputArray _normals) const |
||||
{ |
||||
CV_TRACE_FUNCTION(); |
||||
CV_Assert(frameSize.area() > 0); |
||||
|
||||
_points.create(frameSize, POINT_TYPE); |
||||
_normals.create(frameSize, POINT_TYPE); |
||||
|
||||
Points points = _points.getMat(); |
||||
Normals normals = _normals.getMat(); |
||||
|
||||
HashRaycastInvoker ri(points, normals, cameraPose, intrinsics, *this); |
||||
|
||||
const int nstripes = -1; |
||||
parallel_for_(Range(0, points.rows), ri, nstripes); |
||||
} |
||||
|
||||
struct FetchPointsNormalsInvoker : ParallelLoopBody |
||||
{ |
||||
FetchPointsNormalsInvoker(const HashTSDFVolumeCPU& _volume, |
||||
const std::vector<Vec3i>& _totalVolUnits, |
||||
std::vector<std::vector<ptype>>& _pVecs, |
||||
std::vector<std::vector<ptype>>& _nVecs, bool _needNormals) |
||||
: ParallelLoopBody(), |
||||
volume(_volume), |
||||
totalVolUnits(_totalVolUnits), |
||||
pVecs(_pVecs), |
||||
nVecs(_nVecs), |
||||
needNormals(_needNormals) |
||||
{ |
||||
} |
||||
|
||||
virtual void operator()(const Range& range) const override |
||||
{ |
||||
std::vector<ptype> points, normals; |
||||
for (int i = range.start; i < range.end; i++) |
||||
{ |
||||
cv::Vec3i tsdf_idx = totalVolUnits[i]; |
||||
|
||||
VolumeUnitMap::const_iterator it = volume.volumeUnits.find(tsdf_idx); |
||||
Point3f base_point = volume.volumeUnitIdxToVolume(tsdf_idx); |
||||
if (it != volume.volumeUnits.end()) |
||||
{ |
||||
cv::Ptr<TSDFVolumeCPU> volumeUnit = |
||||
std::dynamic_pointer_cast<TSDFVolumeCPU>(it->second.pVolume); |
||||
std::vector<ptype> localPoints; |
||||
std::vector<ptype> localNormals; |
||||
for (int x = 0; x < volume.volumeUnitResolution; x++) |
||||
for (int y = 0; y < volume.volumeUnitResolution; y++) |
||||
for (int z = 0; z < volume.volumeUnitResolution; z++) |
||||
{ |
||||
cv::Vec3i voxelIdx(x, y, z); |
||||
TsdfVoxel voxel = volumeUnit->at(voxelIdx); |
||||
|
||||
if (voxel.tsdf != 1.f && voxel.weight != 0) |
||||
{ |
||||
Point3f point = base_point + volume.voxelCoordToVolume(voxelIdx); |
||||
localPoints.push_back(toPtype(point)); |
||||
if (needNormals) |
||||
{ |
||||
Point3f normal = volume.getNormalVoxel(point); |
||||
localNormals.push_back(toPtype(normal)); |
||||
} |
||||
} |
||||
} |
||||
|
||||
AutoLock al(mutex); |
||||
pVecs.push_back(localPoints); |
||||
nVecs.push_back(localNormals); |
||||
} |
||||
} |
||||
} |
||||
|
||||
const HashTSDFVolumeCPU& volume; |
||||
std::vector<cv::Vec3i> totalVolUnits; |
||||
std::vector<std::vector<ptype>>& pVecs; |
||||
std::vector<std::vector<ptype>>& nVecs; |
||||
const TsdfVoxel* volDataStart; |
||||
bool needNormals; |
||||
mutable Mutex mutex; |
||||
}; |
||||
|
||||
void HashTSDFVolumeCPU::fetchPointsNormals(OutputArray _points, OutputArray _normals) const |
||||
{ |
||||
CV_TRACE_FUNCTION(); |
||||
|
||||
if (_points.needed()) |
||||
{ |
||||
std::vector<std::vector<ptype>> pVecs, nVecs; |
||||
|
||||
std::vector<Vec3i> totalVolUnits; |
||||
for (const auto& keyvalue : volumeUnits) |
||||
{ |
||||
totalVolUnits.push_back(keyvalue.first); |
||||
} |
||||
FetchPointsNormalsInvoker fi(*this, totalVolUnits, pVecs, nVecs, _normals.needed()); |
||||
Range range(0, (int)totalVolUnits.size()); |
||||
const int nstripes = -1; |
||||
parallel_for_(range, fi, 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()); |
||||
} |
||||
} |
||||
} |
||||
|
||||
struct PushNormals |
||||
{ |
||||
PushNormals(const HashTSDFVolumeCPU& _volume, Normals& _normals) |
||||
: volume(_volume), normals(_normals), invPose(volume.pose.inv()) |
||||
{ |
||||
} |
||||
|
||||
void operator()(const ptype& point, const int* position) const |
||||
{ |
||||
Point3f p = fromPtype(point); |
||||
Point3f n = nan3; |
||||
if (!isNaN(p)) |
||||
{ |
||||
Point3f voxelPoint = invPose * p; |
||||
n = volume.pose.rotation() * volume.getNormalVoxel(voxelPoint); |
||||
} |
||||
normals(position[0], position[1]) = toPtype(n); |
||||
} |
||||
const HashTSDFVolumeCPU& volume; |
||||
Normals& normals; |
||||
Affine3f invPose; |
||||
}; |
||||
|
||||
void HashTSDFVolumeCPU::fetchNormals(cv::InputArray _points, cv::OutputArray _normals) const |
||||
{ |
||||
CV_TRACE_FUNCTION(); |
||||
|
||||
if (_normals.needed()) |
||||
{ |
||||
Points points = _points.getMat(); |
||||
CV_Assert(points.type() == POINT_TYPE); |
||||
|
||||
_normals.createSameSize(_points, _points.type()); |
||||
Normals normals = _normals.getMat(); |
||||
|
||||
points.forEach(PushNormals(*this, normals)); |
||||
} |
||||
} |
||||
|
||||
cv::Ptr<HashTSDFVolume> makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, |
||||
float _raycastStepFactor, float _truncDist, |
||||
int _maxWeight, float _truncateThreshold, |
||||
int _volumeUnitResolution) |
||||
{ |
||||
return cv::makePtr<HashTSDFVolumeCPU>(_voxelSize, _pose, _raycastStepFactor, _truncDist, |
||||
_maxWeight, _truncateThreshold, _volumeUnitResolution); |
||||
} |
||||
|
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
@ -0,0 +1,112 @@ |
||||
// 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
|
||||
|
||||
#ifndef __OPENCV_HASH_TSDF_H__ |
||||
#define __OPENCV_HASH_TSDF_H__ |
||||
|
||||
#include <opencv2/rgbd/volume.hpp> |
||||
#include <unordered_map> |
||||
#include <unordered_set> |
||||
|
||||
#include "tsdf.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
class HashTSDFVolume : public Volume |
||||
{ |
||||
public: |
||||
// dimension in voxels, size in meters
|
||||
//! Use fixed volume cuboid
|
||||
HashTSDFVolume(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, float _truncDist, |
||||
int _maxWeight, float _truncateThreshold, int _volumeUnitRes, |
||||
bool zFirstMemOrder = true); |
||||
|
||||
virtual ~HashTSDFVolume() = default; |
||||
|
||||
public: |
||||
int maxWeight; |
||||
float truncDist; |
||||
float truncateThreshold; |
||||
int volumeUnitResolution; |
||||
float volumeUnitSize; |
||||
bool zFirstMemOrder; |
||||
}; |
||||
|
||||
struct VolumeUnit |
||||
{ |
||||
VolumeUnit() : pVolume(nullptr){}; |
||||
~VolumeUnit() = default; |
||||
|
||||
cv::Ptr<TSDFVolume> pVolume; |
||||
cv::Vec3i index; |
||||
bool isActive; |
||||
}; |
||||
|
||||
//! Spatial hashing
|
||||
struct tsdf_hash |
||||
{ |
||||
size_t operator()(const cv::Vec3i& x) const noexcept |
||||
{ |
||||
size_t seed = 0; |
||||
constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9; |
||||
for (uint16_t i = 0; i < 3; i++) |
||||
{ |
||||
seed ^= std::hash<int>()(x[i]) + GOLDEN_RATIO + (seed << 6) + (seed >> 2); |
||||
} |
||||
return seed; |
||||
} |
||||
}; |
||||
|
||||
typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet; |
||||
typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitMap; |
||||
|
||||
class HashTSDFVolumeCPU : public HashTSDFVolume |
||||
{ |
||||
public: |
||||
// dimension in voxels, size in meters
|
||||
HashTSDFVolumeCPU(float _voxelSize, cv::Affine3f _pose, float _raycastStepFactor, |
||||
float _truncDist, int _maxWeight, float _truncateThreshold, |
||||
int _volumeUnitRes, bool zFirstMemOrder = true); |
||||
|
||||
virtual void integrate(InputArray _depth, float depthFactor, const cv::Affine3f& cameraPose, |
||||
const cv::kinfu::Intr& intrinsics) override; |
||||
virtual void raycast(const cv::Affine3f& cameraPose, const cv::kinfu::Intr& intrinsics, |
||||
cv::Size frameSize, cv::OutputArray points, |
||||
cv::OutputArray normals) const override; |
||||
|
||||
virtual void fetchNormals(cv::InputArray points, cv::OutputArray _normals) const override; |
||||
virtual void fetchPointsNormals(cv::OutputArray points, cv::OutputArray normals) const override; |
||||
|
||||
virtual void reset() override; |
||||
|
||||
//! Return the voxel given the voxel index in the universal volume (1 unit = 1 voxel_length)
|
||||
virtual TsdfVoxel at(const cv::Vec3i& volumeIdx) const; |
||||
|
||||
//! Return the voxel given the point in volume coordinate system i.e., (metric scale 1 unit =
|
||||
//! 1m)
|
||||
virtual TsdfVoxel at(const cv::Point3f& point) const; |
||||
|
||||
inline TsdfType interpolateVoxel(const cv::Point3f& point) const; |
||||
Point3f getNormalVoxel(cv::Point3f p) const; |
||||
|
||||
//! Utility functions for coordinate transformations
|
||||
cv::Vec3i volumeToVolumeUnitIdx(cv::Point3f point) const; |
||||
cv::Point3f volumeUnitIdxToVolume(cv::Vec3i volumeUnitIdx) const; |
||||
|
||||
cv::Point3f voxelCoordToVolume(cv::Vec3i voxelIdx) const; |
||||
cv::Vec3i volumeToVoxelCoord(cv::Point3f point) const; |
||||
|
||||
public: |
||||
//! Hashtable of individual smaller volume units
|
||||
VolumeUnitMap volumeUnits; |
||||
}; |
||||
cv::Ptr<HashTSDFVolume> makeHashTSDFVolume(float _voxelSize, cv::Affine3f _pose, |
||||
float _raycastStepFactor, float _truncDist, |
||||
int _maxWeight, float truncateThreshold, |
||||
int volumeUnitResolution = 16); |
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
#endif |
||||
@ -0,0 +1,34 @@ |
||||
// 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
|
||||
|
||||
#include "precomp.hpp" |
||||
#include <opencv2/rgbd/volume.hpp> |
||||
|
||||
#include "tsdf.hpp" |
||||
#include "hash_tsdf.hpp" |
||||
|
||||
namespace cv |
||||
{ |
||||
namespace kinfu |
||||
{ |
||||
cv::Ptr<Volume> makeVolume(VolumeType _volumeType, float _voxelSize, cv::Affine3f _pose, |
||||
float _raycastStepFactor, float _truncDist, int _maxWeight, |
||||
float _truncateThreshold, Point3i _resolution) |
||||
{ |
||||
if (_volumeType == VolumeType::TSDF) |
||||
{ |
||||
return makeTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, |
||||
_resolution); |
||||
} |
||||
else if (_volumeType == VolumeType::HASHTSDF) |
||||
{ |
||||
return makeHashTSDFVolume(_voxelSize, _pose, _raycastStepFactor, _truncDist, _maxWeight, |
||||
_truncateThreshold); |
||||
} |
||||
else |
||||
return nullptr; |
||||
} |
||||
|
||||
} // namespace kinfu
|
||||
} // namespace cv
|
||||
Loading…
Reference in new issue