mirror of
https://github.com/opencv/opencv.git
synced 2025-06-07 17:44:04 +08:00
Merge pull request #22178 from savuor:hash_tsdf_fixes
This PR contains:
- a new property enableGrowth which controls should the HashTSDF be extended during integration by adding new volume units or not
- a new method getBoundingBox which calculates the size of currently occupied data
- a set of tests to check that new functionality
- a fix for TSDF GPU reset (data is correctly zeroed now using floatToTsdf() function)
minor changes
This commit is contained in:
Rostislav Vasilikhin
GitHub
parent
a9d98bfb34
commit
2407ab4e61
@ -23,7 +23,7 @@ public:
|
||||
* @param settings the custom settings for volume.
|
||||
*/
|
||||
Volume(VolumeType vtype, const VolumeSettings& settings);
|
||||
~Volume();
|
||||
virtual ~Volume();
|
||||
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
@ -102,14 +102,38 @@ public:
|
||||
*/
|
||||
void reset();
|
||||
|
||||
//TODO: remove this
|
||||
/** @brief return visible blocks in volume.
|
||||
*/
|
||||
int getVisibleBlocks() const;
|
||||
|
||||
/** @brief return number of vulmeunits in volume.
|
||||
/** @brief return number of volume units in volume.
|
||||
*/
|
||||
size_t getTotalVolumeUnits() const;
|
||||
|
||||
enum BoundingBoxPrecision
|
||||
{
|
||||
VOLUME_UNIT = 0,
|
||||
VOXEL = 1
|
||||
};
|
||||
/** @brief Gets bounding box in volume coordinates with given precision:
|
||||
* VOLUME_UNIT - up to volume unit
|
||||
* VOXEL - up to voxel (currently not supported)
|
||||
* @return (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
|
||||
*/
|
||||
virtual Vec6f getBoundingBox(int precision) const;
|
||||
|
||||
/**
|
||||
* @brief Enables or disables new volume unit allocation during integration.
|
||||
* Makes sense for HashTSDF only.
|
||||
*/
|
||||
virtual void setEnableGrowth(bool v);
|
||||
/**
|
||||
* @brief Returns if new volume units are allocated during integration or not.
|
||||
* Makes sense for HashTSDF only.
|
||||
*/
|
||||
virtual bool getEnableGrowth() const;
|
||||
|
||||
class Impl;
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
|
||||
@ -89,11 +89,9 @@ Point3f ocl_getNormalVoxel(
|
||||
#endif
|
||||
|
||||
void integrateHashTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree,
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits)
|
||||
{
|
||||
//std::cout << "integrateHashTsdfVolumeUnit()" << std::endl;
|
||||
|
||||
CV_TRACE_FUNCTION();
|
||||
|
||||
CV_Assert(_depth.type() == DEPTH_TYPE);
|
||||
@ -101,10 +99,16 @@ void integrateHashTsdfVolumeUnit(
|
||||
Mat volUnitsData = _volUnitsData.getMat();
|
||||
Mat pixNorms = _pixNorms.getMat();
|
||||
|
||||
//! Compute volumes to be allocated
|
||||
const int depthStride = volumeUnitDegree;
|
||||
const float invDepthFactor = 1.f / settings.getDepthFactor();
|
||||
const float truncDist = settings.getTsdfTruncateDistance();
|
||||
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();
|
||||
|
||||
@ -112,23 +116,21 @@ void integrateHashTsdfVolumeUnit(
|
||||
settings.getVolumeResolution(resolution);
|
||||
const float volumeUnitSize = voxelSize * resolution[0];
|
||||
|
||||
Matx33f intr;
|
||||
settings.getCameraIntegrateIntrinsics(intr);
|
||||
const Intr intrinsics(intr);
|
||||
const Intr::Reprojector reproj(intrinsics.makeReprojector());
|
||||
|
||||
Matx44f _pose;
|
||||
settings.getVolumePose(_pose);
|
||||
const Affine3f pose = Affine3f(_pose);
|
||||
const Affine3f cam2vol(pose.inv() * Affine3f(cameraPose));
|
||||
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);
|
||||
VolumeUnitIndexSet newIndices;
|
||||
std::unordered_set<cv::Vec3i, tsdf_hash> newIndices;
|
||||
Mutex mutex;
|
||||
Range allocateRange(0, depth.rows);
|
||||
|
||||
auto AllocateVolumeUnitsInvoker = [&](const Range& range) {
|
||||
VolumeUnitIndexSet localAccessVolUnits;
|
||||
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];
|
||||
@ -172,7 +174,6 @@ void integrateHashTsdfVolumeUnit(
|
||||
mutex.unlock();
|
||||
};
|
||||
parallel_for_(allocateRange, AllocateVolumeUnitsInvoker);
|
||||
//AllocateVolumeUnitsInvoker(allocateRange);
|
||||
|
||||
//! Perform the allocation
|
||||
for (auto idx : newIndices)
|
||||
@ -182,7 +183,8 @@ void integrateHashTsdfVolumeUnit(
|
||||
Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
|
||||
|
||||
vu.pose = subvolumePose;
|
||||
vu.index = lastVolIndex; lastVolIndex++;
|
||||
vu.index = lastVolIndex;
|
||||
lastVolIndex++;
|
||||
if (lastVolIndex > int(volUnitsData.size().height))
|
||||
{
|
||||
volUnitsData.resize((lastVolIndex - 1) * 2);
|
||||
@ -196,6 +198,7 @@ void integrateHashTsdfVolumeUnit(
|
||||
vu.lastVisibleIndex = frameId;
|
||||
vu.isActive = true;
|
||||
}
|
||||
}
|
||||
|
||||
//! Get keys for all the allocated volume Units
|
||||
std::vector<Vec3i> totalVolUnits;
|
||||
@ -206,7 +209,8 @@ void integrateHashTsdfVolumeUnit(
|
||||
|
||||
//! Mark volumes in the camera frustum as active
|
||||
Range inFrustumRange(0, (int)volumeUnits.size());
|
||||
parallel_for_(inFrustumRange, [&](const Range& range) {
|
||||
parallel_for_(inFrustumRange, [&](const Range& range)
|
||||
{
|
||||
const Affine3f vol2cam(Affine3f(cameraPose.inv()) * pose);
|
||||
const Intr::Projector proj(intrinsics.makeProjector());
|
||||
|
||||
@ -236,7 +240,8 @@ void integrateHashTsdfVolumeUnit(
|
||||
|
||||
|
||||
//! Integrate the correct volumeUnits
|
||||
parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range) {
|
||||
parallel_for_(Range(0, (int)totalVolUnits.size()), [&](const Range& range)
|
||||
{
|
||||
for (int i = range.start; i < range.end; i++)
|
||||
{
|
||||
Vec3i tsdf_idx = totalVolUnits[i];
|
||||
@ -474,7 +479,7 @@ void markActive(
|
||||
|
||||
|
||||
void ocl_integrateHashTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree,
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags)
|
||||
{
|
||||
CV_TRACE_FUNCTION();
|
||||
@ -510,6 +515,8 @@ void ocl_integrateHashTsdfVolumeUnit(
|
||||
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);
|
||||
@ -551,6 +558,7 @@ void ocl_integrateHashTsdfVolumeUnit(
|
||||
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);
|
||||
@ -596,8 +604,6 @@ void ocl_integrateHashTsdfVolumeUnit(
|
||||
|
||||
if (!k.run(3, globalSize, NULL, true))
|
||||
throw std::runtime_error("Failed to run kernel");
|
||||
|
||||
//std::cout << "ocl_integrateHashTsdfVolumeUnit() end" << std::endl;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -283,11 +283,10 @@ public:
|
||||
|
||||
int calcVolumeUnitDegree(Point3i volumeResolution);
|
||||
|
||||
typedef std::unordered_set<cv::Vec3i, tsdf_hash> VolumeUnitIndexSet;
|
||||
typedef std::unordered_map<cv::Vec3i, VolumeUnit, tsdf_hash> VolumeUnitIndexes;
|
||||
|
||||
void integrateHashTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree,
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, const int volumeUnitDegree, bool enableGrowth,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volUnitsData, VolumeUnitIndexes& volumeUnits);
|
||||
|
||||
void raycastHashTsdfVolumeUnit(
|
||||
@ -304,7 +303,7 @@ void fetchPointsNormalsFromHashTsdfVolumeUnit(
|
||||
|
||||
#ifdef HAVE_OPENCL
|
||||
void ocl_integrateHashTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree,
|
||||
const VolumeSettings& settings, const Matx44f& cameraPose, int& lastVolIndex, const int frameId, int& bufferSizeDegree, const int volumeUnitDegree, bool enableGrowth,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _lastVisibleIndices, InputArray _volUnitsDataCopy, InputArray _volUnitsData, CustomHashSet& hashTable, InputArray _isActiveFlags);
|
||||
|
||||
void ocl_raycastHashTsdfVolumeUnit(
|
||||
|
||||
@ -38,8 +38,6 @@ void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm)
|
||||
#ifdef HAVE_OPENCL
|
||||
void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm)
|
||||
{
|
||||
//std::cout << "ocl_preCalculationPixNorm" << std::endl;
|
||||
|
||||
// calculating this on CPU then uploading to GPU is faster than calculating this on GPU
|
||||
Mat cpuPixNorm;
|
||||
preCalculationPixNorm(size, intrinsics, cpuPixNorm);
|
||||
@ -59,12 +57,9 @@ void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& came
|
||||
}
|
||||
|
||||
|
||||
void integrateTsdfVolumeUnit(
|
||||
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
|
||||
void integrateTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose,
|
||||
InputArray _depth, InputArray _pixNorms, InputArray _volume)
|
||||
{
|
||||
//std::cout << "integrateTsdfVolumeUnit" << std::endl;
|
||||
|
||||
Depth depth = _depth.getMat();
|
||||
Mat volume = _volume.getMat();
|
||||
Mat pixNorms = _pixNorms.getMat();
|
||||
|
||||
@ -143,7 +143,6 @@ void TsdfVolume::fetchPointsNormals(OutputArray points, OutputArray normals) con
|
||||
ocl_fetchPointsNormalsFromTsdfVolumeUnit(settings, gpu_volume, points, normals);
|
||||
else
|
||||
fetchPointsNormalsFromTsdfVolumeUnit(settings, cpu_volume, points, normals);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -164,7 +163,7 @@ void TsdfVolume::reset()
|
||||
});
|
||||
#else
|
||||
if (useGPU)
|
||||
gpu_volume.setTo(Scalar(0, 0));
|
||||
gpu_volume.setTo(Scalar(floatToTsdf(0.0f), 0));
|
||||
else
|
||||
//TODO: use setTo(Scalar(0, 0))
|
||||
cpu_volume.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
@ -178,6 +177,29 @@ int TsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t TsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
|
||||
Vec6f TsdfVolume::getBoundingBox(int precision) const
|
||||
{
|
||||
if (precision == Volume::BoundingBoxPrecision::VOXEL)
|
||||
{
|
||||
CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
|
||||
}
|
||||
else
|
||||
{
|
||||
float sz = this->settings.getVoxelSize();
|
||||
Vec3f res;
|
||||
this->settings.getVolumeResolution(res);
|
||||
Vec3f volSize = res * sz;
|
||||
return Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]);
|
||||
}
|
||||
}
|
||||
|
||||
void TsdfVolume::setEnableGrowth(bool /*v*/) { }
|
||||
|
||||
bool TsdfVolume::getEnableGrowth() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// HASH_TSDF
|
||||
|
||||
@ -246,16 +268,18 @@ void HashTsdfVolume::integrate(InputArray _depth, InputArray _cameraPose)
|
||||
#endif
|
||||
}
|
||||
#ifndef HAVE_OPENCL
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, pixNorms, volUnitsData, volumeUnits);
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth, pixNorms, volUnitsData, volumeUnits);
|
||||
lastFrameId++;
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, depth, gpu_pixNorms, lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags);
|
||||
ocl_integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, bufferSizeDegree, volumeUnitDegree, enableGrowth, depth, gpu_pixNorms,
|
||||
lastVisibleIndices, volUnitsDataCopy, gpu_volUnitsData, hashTable, isActiveFlags);
|
||||
}
|
||||
else
|
||||
{
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, depth, cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits);
|
||||
integrateHashTsdfVolumeUnit(settings, cameraPose, lastVolIndex, lastFrameId, volumeUnitDegree, enableGrowth, depth,
|
||||
cpu_pixNorms, cpu_volUnitsData, cpu_volumeUnits);
|
||||
lastFrameId++;
|
||||
}
|
||||
#endif
|
||||
@ -324,6 +348,7 @@ void HashTsdfVolume::reset()
|
||||
CV_TRACE_FUNCTION();
|
||||
lastVolIndex = 0;
|
||||
lastFrameId = 0;
|
||||
enableGrowth = true;
|
||||
#ifndef HAVE_OPENCL
|
||||
volUnitsData.forEach<VecTsdfVoxel>([](VecTsdfVoxel& vv, const int* /* position */)
|
||||
{
|
||||
@ -364,6 +389,92 @@ void HashTsdfVolume::reset()
|
||||
int HashTsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t HashTsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
void HashTsdfVolume::setEnableGrowth(bool v)
|
||||
{
|
||||
enableGrowth = v;
|
||||
}
|
||||
|
||||
bool HashTsdfVolume::getEnableGrowth() const
|
||||
{
|
||||
return enableGrowth;
|
||||
}
|
||||
|
||||
Vec6f HashTsdfVolume::getBoundingBox(int precision) const
|
||||
{
|
||||
if (precision == Volume::BoundingBoxPrecision::VOXEL)
|
||||
{
|
||||
CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
|
||||
}
|
||||
else
|
||||
{
|
||||
Vec3i res;
|
||||
this->settings.getVolumeResolution(res);
|
||||
float voxelSize = this->settings.getVoxelSize();
|
||||
float side = res[0] * voxelSize;
|
||||
|
||||
std::vector<Vec3i> vi;
|
||||
#ifndef HAVE_OPENCL
|
||||
for (const auto& keyvalue : volumeUnits)
|
||||
{
|
||||
vi.push_back(keyvalue.first);
|
||||
}
|
||||
#else
|
||||
if (useGPU)
|
||||
{
|
||||
for (int row = 0; row < hashTable.last; row++)
|
||||
{
|
||||
Vec4i idx4 = hashTable.data[row];
|
||||
vi.push_back(Vec3i(idx4[0], idx4[1], idx4[2]));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (const auto& keyvalue : cpu_volumeUnits)
|
||||
{
|
||||
vi.push_back(keyvalue.first);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (vi.empty())
|
||||
{
|
||||
return Vec6f();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<Point3f> pts;
|
||||
for (Vec3i idx : vi)
|
||||
{
|
||||
Point3f base = Point3f(idx[0], idx[1], idx[2]) * side;
|
||||
pts.push_back(base);
|
||||
pts.push_back(base + Point3f(side, 0, 0));
|
||||
pts.push_back(base + Point3f(0, side, 0));
|
||||
pts.push_back(base + Point3f(0, 0, side));
|
||||
pts.push_back(base + Point3f(side, side, 0));
|
||||
pts.push_back(base + Point3f(side, 0, side));
|
||||
pts.push_back(base + Point3f(0, side, side));
|
||||
pts.push_back(base + Point3f(side, side, side));
|
||||
}
|
||||
|
||||
const float mval = std::numeric_limits<float>::max();
|
||||
Vec6f bb(mval, mval, mval, -mval, -mval, -mval);
|
||||
for (auto p : pts)
|
||||
{
|
||||
// pt in local coords
|
||||
Point3f pg = p;
|
||||
bb[0] = min(bb[0], pg.x);
|
||||
bb[1] = min(bb[1], pg.y);
|
||||
bb[2] = min(bb[2], pg.z);
|
||||
bb[3] = max(bb[3], pg.x);
|
||||
bb[4] = max(bb[4], pg.y);
|
||||
bb[5] = max(bb[5], pg.z);
|
||||
}
|
||||
|
||||
return bb;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// COLOR_TSDF
|
||||
|
||||
ColorTsdfVolume::ColorTsdfVolume(const VolumeSettings& _settings) :
|
||||
@ -452,4 +563,27 @@ void ColorTsdfVolume::reset()
|
||||
int ColorTsdfVolume::getVisibleBlocks() const { return 1; }
|
||||
size_t ColorTsdfVolume::getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
Vec6f ColorTsdfVolume::getBoundingBox(int precision) const
|
||||
{
|
||||
if (precision == Volume::BoundingBoxPrecision::VOXEL)
|
||||
{
|
||||
CV_Error(Error::StsNotImplemented, "Voxel mode is not implemented yet");
|
||||
}
|
||||
else
|
||||
{
|
||||
float sz = this->settings.getVoxelSize();
|
||||
Vec3f res;
|
||||
this->settings.getVolumeResolution(res);
|
||||
Vec3f volSize = res * sz;
|
||||
return Vec6f(0, 0, 0, volSize[0], volSize[1], volSize[2]);
|
||||
}
|
||||
}
|
||||
|
||||
void ColorTsdfVolume::setEnableGrowth(bool /*v*/) { }
|
||||
|
||||
bool ColorTsdfVolume::getEnableGrowth() const
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -37,6 +37,10 @@ public:
|
||||
virtual int getVisibleBlocks() const = 0;
|
||||
virtual size_t getTotalVolumeUnits() const = 0;
|
||||
|
||||
virtual Vec6f getBoundingBox(int precision) const = 0;
|
||||
virtual void setEnableGrowth(bool v) = 0;
|
||||
virtual bool getEnableGrowth() const = 0;
|
||||
|
||||
public:
|
||||
const VolumeSettings& settings;
|
||||
#ifdef HAVE_OPENCL
|
||||
@ -65,6 +69,19 @@ public:
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
|
||||
// Gets bounding box in volume coordinates with given precision:
|
||||
// VOLUME_UNIT - up to volume unit
|
||||
// VOXEL - up to voxel
|
||||
// returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
|
||||
virtual Vec6f getBoundingBox(int precision) const override;
|
||||
|
||||
// Enabels or disables new volume unit allocation during integration
|
||||
// Applicable for HashTSDF only
|
||||
virtual void setEnableGrowth(bool v) override;
|
||||
// Returns if new volume units are allocated during integration or not
|
||||
// Applicable for HashTSDF only
|
||||
virtual bool getEnableGrowth() const override;
|
||||
|
||||
public:
|
||||
Vec6f frameParams;
|
||||
#ifndef HAVE_OPENCL
|
||||
@ -105,11 +122,26 @@ public:
|
||||
virtual void reset() override;
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
|
||||
// Enabels or disables new volume unit allocation during integration
|
||||
// Applicable for HashTSDF only
|
||||
virtual void setEnableGrowth(bool v) override;
|
||||
// Returns if new volume units are allocated during integration or not
|
||||
// Applicable for HashTSDF only
|
||||
virtual bool getEnableGrowth() const override;
|
||||
|
||||
// Gets bounding box in volume coordinates with given precision:
|
||||
// VOLUME_UNIT - up to volume unit
|
||||
// VOXEL - up to voxel
|
||||
// returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
|
||||
virtual Vec6f getBoundingBox(int precision) const override;
|
||||
|
||||
public:
|
||||
int lastVolIndex;
|
||||
int lastFrameId;
|
||||
Vec6f frameParams;
|
||||
int volumeUnitDegree;
|
||||
bool enableGrowth;
|
||||
|
||||
#ifndef HAVE_OPENCL
|
||||
Mat volUnitsData;
|
||||
@ -154,6 +186,20 @@ public:
|
||||
virtual void reset() override;
|
||||
virtual int getVisibleBlocks() const override;
|
||||
virtual size_t getTotalVolumeUnits() const override;
|
||||
|
||||
// Gets bounding box in volume coordinates with given precision:
|
||||
// VOLUME_UNIT - up to volume unit
|
||||
// VOXEL - up to voxel
|
||||
// returns (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
|
||||
virtual Vec6f getBoundingBox(int precision) const override;
|
||||
|
||||
// Enabels or disables new volume unit allocation during integration
|
||||
// Applicable for HashTSDF only
|
||||
virtual void setEnableGrowth(bool v) override;
|
||||
// Returns if new volume units are allocated during integration or not
|
||||
// Applicable for HashTSDF only
|
||||
virtual bool getEnableGrowth() const override;
|
||||
|
||||
private:
|
||||
Vec4i volStrides;
|
||||
Vec6f frameParams;
|
||||
@ -203,6 +249,10 @@ void Volume::reset() { this->impl->reset(); }
|
||||
int Volume::getVisibleBlocks() const { return this->impl->getVisibleBlocks(); }
|
||||
size_t Volume::getTotalVolumeUnits() const { return this->impl->getTotalVolumeUnits(); }
|
||||
|
||||
Vec6f Volume::getBoundingBox(int precision) const { return this->impl->getBoundingBox(precision); }
|
||||
void Volume::setEnableGrowth(bool v) { this->impl->setEnableGrowth(v); }
|
||||
bool Volume::getEnableGrowth() const { return this->impl->getEnableGrowth(); }
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -571,6 +571,7 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType
|
||||
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
|
||||
}
|
||||
|
||||
|
||||
TEST(TSDF_GPU, raycast_custom_framesize_normals_mat)
|
||||
{
|
||||
normal_test_custom_framesize(VolumeType::TSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
|
||||
|
||||
@ -424,13 +424,13 @@ void normalsCheck(Mat normals)
|
||||
for (auto pvector = normals.begin<Vec4f>(); pvector < normals.end<Vec4f>(); pvector++)
|
||||
{
|
||||
vector = *pvector;
|
||||
if (!cvIsNaN(vector[0]))
|
||||
if (!(cvIsNaN(vector[0]) || cvIsNaN(vector[1]) || cvIsNaN(vector[2])))
|
||||
{
|
||||
counter++;
|
||||
float length = vector[0] * vector[0] +
|
||||
float l2 = vector[0] * vector[0] +
|
||||
vector[1] * vector[1] +
|
||||
vector[2] * vector[2];
|
||||
ASSERT_LT(abs(1 - length), 0.0001f) << "There is normal with length != 1";
|
||||
ASSERT_LT(abs(1.f - l2), 0.0001f) << "There is normal with length != 1";
|
||||
}
|
||||
}
|
||||
ASSERT_GT(counter, 0) << "There are not normals";
|
||||
@ -537,6 +537,7 @@ void normal_test_custom_framesize(VolumeType volumeType, VolumeTestFunction test
|
||||
normalsCheck(normals);
|
||||
}
|
||||
|
||||
|
||||
void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction testFunction, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
@ -603,6 +604,7 @@ void normal_test_common_framesize(VolumeType volumeType, VolumeTestFunction test
|
||||
normalsCheck(normals);
|
||||
}
|
||||
|
||||
|
||||
void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
@ -679,6 +681,7 @@ void valid_points_test_custom_framesize(VolumeType volumeType, VolumeTestSrcType
|
||||
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
|
||||
}
|
||||
|
||||
|
||||
void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType testSrcType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
@ -755,6 +758,117 @@ void valid_points_test_common_framesize(VolumeType volumeType, VolumeTestSrcType
|
||||
ASSERT_LT(abs(0.5 - percentValidity), 0.3) << "percentValidity out of [0.3; 0.7] (percentValidity=" << percentValidity << ")";
|
||||
}
|
||||
|
||||
|
||||
void debugVolumeDraw(const Volume &volume, Affine3f pose, Mat depth, float depthFactor, std::string objFname)
|
||||
{
|
||||
Vec3f lightPose = Vec3f::all(0.f);
|
||||
Mat points, normals;
|
||||
volume.raycast(pose.matrix, points, normals);
|
||||
|
||||
Mat ptsList, ptsList3, nrmList, nrmList3;
|
||||
volume.fetchPointsNormals(ptsList, nrmList);
|
||||
// transform 4 channels to 3 channels
|
||||
cvtColor(ptsList, ptsList3, COLOR_BGRA2BGR);
|
||||
cvtColor(ptsList, nrmList3, COLOR_BGRA2BGR);
|
||||
savePointCloud(objFname, ptsList3, nrmList3);
|
||||
|
||||
displayImage(depth, points, normals, depthFactor, lightPose);
|
||||
}
|
||||
|
||||
|
||||
// For fixed volumes which are TSDF and ColorTSDF
|
||||
void staticBoundingBoxTest(VolumeType volumeType)
|
||||
{
|
||||
VolumeSettings vs(volumeType);
|
||||
Volume volume(volumeType, vs);
|
||||
|
||||
Vec3i res;
|
||||
vs.getVolumeResolution(res);
|
||||
float voxelSize = vs.getVoxelSize();
|
||||
Matx44f pose;
|
||||
vs.getVolumePose(pose);
|
||||
Vec3f end = voxelSize * Vec3f(res);
|
||||
Vec6f truebb(0, 0, 0, end[0], end[1], end[2]);
|
||||
Vec6f bb = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT);
|
||||
Vec6f diff = bb - truebb;
|
||||
double normdiff = std::sqrt(diff.ddot(diff));
|
||||
ASSERT_LE(normdiff, std::numeric_limits<double>::epsilon());
|
||||
}
|
||||
|
||||
|
||||
// For HashTSDF only
|
||||
void boundingBoxGrowthTest(bool enableGrowth)
|
||||
{
|
||||
VolumeSettings vs(VolumeType::HashTSDF);
|
||||
Volume volume(VolumeType::HashTSDF, vs);
|
||||
|
||||
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
|
||||
Matx33f intr;
|
||||
vs.getCameraIntegrateIntrinsics(intr);
|
||||
bool onlySemisphere = false;
|
||||
float depthFactor = vs.getDepthFactor();
|
||||
Ptr<Scene> scene = Scene::create(frameSize, intr, depthFactor, onlySemisphere);
|
||||
std::vector<Affine3f> poses = scene->getPoses();
|
||||
|
||||
Mat depth = scene->depth(poses[0]);
|
||||
UMat udepth;
|
||||
depth.copyTo(udepth);
|
||||
|
||||
// depth is integrated with multiple weight
|
||||
// TODO: add weight parameter to integrate() call (both scalar and array of 8u/32f)
|
||||
const int nIntegrations = 1;
|
||||
for (int i = 0; i < nIntegrations; i++)
|
||||
volume.integrate(udepth, poses[0].matrix);
|
||||
|
||||
Vec6f bb = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT);
|
||||
Vec6f truebb(-0.9375f, 1.3125f, -0.8906f, 3.9375f, 2.6133f, 1.4004f);
|
||||
Vec6f diff = bb - truebb;
|
||||
double bbnorm = std::sqrt(diff.ddot(diff));
|
||||
|
||||
Vec3f vuRes;
|
||||
vs.getVolumeResolution(vuRes);
|
||||
double vuSize = vs.getVoxelSize() * vuRes[0];
|
||||
// it's OK to have such big difference since this is volume unit size-grained BB calculation
|
||||
// Theoretical max difference can be sqrt(6) =(approx)= 2.4494
|
||||
EXPECT_LE(bbnorm, vuSize * 2.38);
|
||||
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
debugVolumeDraw(volume, poses[0], depth, depthFactor, "pts.obj");
|
||||
}
|
||||
|
||||
// Integrate another depth growth changed
|
||||
|
||||
Mat depth2 = scene->depth(poses[0].translate(Vec3f(0, -0.25f, 0)));
|
||||
UMat udepth2;
|
||||
depth2.copyTo(udepth2);
|
||||
|
||||
volume.setEnableGrowth(enableGrowth);
|
||||
|
||||
for (int i = 0; i < nIntegrations; i++)
|
||||
volume.integrate(udepth2, poses[0].matrix);
|
||||
|
||||
Vec6f bb2 = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT);
|
||||
|
||||
Vec6f truebb2 = truebb + Vec6f(0, -(1.3125f - 1.0723f), -(-0.8906f - (-1.4238f)), 0, 0, 0);
|
||||
Vec6f diff2 = enableGrowth ? bb2 - truebb2 : bb2 - bb;
|
||||
double bbnorm2 = std::sqrt(diff2.ddot(diff2));
|
||||
EXPECT_LE(bbnorm2, enableGrowth ? (vuSize * 2.3) : std::numeric_limits<double>::epsilon());
|
||||
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
debugVolumeDraw(volume, poses[0], depth, depthFactor, enableGrowth ? "pts_growth.obj" : "pts_no_growth.obj");
|
||||
}
|
||||
|
||||
// Reset check
|
||||
|
||||
volume.reset();
|
||||
Vec6f bb3 = volume.getBoundingBox(Volume::BoundingBoxPrecision::VOLUME_UNIT);
|
||||
double bbnorm3 = std::sqrt(bb3.ddot(bb3));
|
||||
EXPECT_LE(bbnorm3, std::numeric_limits<double>::epsilon());
|
||||
}
|
||||
|
||||
|
||||
static Mat nanMask(Mat img)
|
||||
{
|
||||
int depth = img.depth();
|
||||
@ -1013,6 +1127,17 @@ TEST(HashTSDF, valid_points_common_framesize_frame)
|
||||
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<bool>
|
||||
{ };
|
||||
|
||||
TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth)
|
||||
{
|
||||
boundingBoxGrowthTest(GetParam());
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Bool());
|
||||
|
||||
|
||||
TEST(HashTSDF, reproduce_volPoseRot)
|
||||
{
|
||||
regressionVolPoseRot();
|
||||
@ -1068,6 +1193,16 @@ TEST(ColorTSDF, valid_points_common_framesize_fetch)
|
||||
valid_points_test_common_framesize(VolumeType::ColorTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
|
||||
}
|
||||
|
||||
class StaticVolumeBoundingBox : public ::testing::TestWithParam<VolumeType>
|
||||
{ };
|
||||
|
||||
TEST_P(StaticVolumeBoundingBox, boundingBox)
|
||||
{
|
||||
staticBoundingBoxTest(GetParam());
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF));
|
||||
|
||||
#else
|
||||
TEST(TSDF_CPU, raycast_custom_framesize_normals_mat)
|
||||
{
|
||||
@ -1287,12 +1422,77 @@ TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch)
|
||||
}
|
||||
|
||||
|
||||
// used to store current OpenCL status (on/off) and revert it after test is done
|
||||
// works even after exceptions thrown in test body
|
||||
struct OpenCLStatusRevert
|
||||
{
|
||||
OpenCLStatusRevert(bool oclStatus) : originalOpenCLStatus(oclStatus) { }
|
||||
~OpenCLStatusRevert()
|
||||
{
|
||||
cv::ocl::setUseOpenCL(originalOpenCLStatus);
|
||||
}
|
||||
bool originalOpenCLStatus;
|
||||
};
|
||||
|
||||
class StaticVolumeBoundingBox : public ::testing::TestWithParam<VolumeType>
|
||||
{ };
|
||||
|
||||
TEST_P(StaticVolumeBoundingBox, boundingBoxCPU)
|
||||
{
|
||||
OpenCLStatusRevert revert(cv::ocl::useOpenCL());
|
||||
|
||||
cv::ocl::setUseOpenCL(false);
|
||||
|
||||
staticBoundingBoxTest(GetParam());
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF));
|
||||
|
||||
|
||||
// OpenCL tests
|
||||
TEST(HashTSDF_GPU, reproduce_volPoseRot)
|
||||
{
|
||||
regressionVolPoseRot();
|
||||
}
|
||||
|
||||
//TODO: use this when ColorTSDF gets GPU support
|
||||
/*
|
||||
class StaticVolumeBoundingBox : public ::testing::TestWithParam<VolumeType>
|
||||
{ };
|
||||
|
||||
TEST_P(StaticVolumeBoundingBox, GPU)
|
||||
{
|
||||
staticBoundingBoxTest(GetParam());
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(TSDF, StaticVolumeBoundingBox, ::testing::Values(VolumeType::TSDF, VolumeType::ColorTSDF));
|
||||
*/
|
||||
|
||||
// until that, we don't need parametrized test
|
||||
TEST(TSDF, boundingBoxGPU)
|
||||
{
|
||||
staticBoundingBoxTest(VolumeType::TSDF);
|
||||
}
|
||||
|
||||
|
||||
class BoundingBoxEnableGrowthTest : public ::testing::TestWithParam<std::tuple<bool, bool>>
|
||||
{ };
|
||||
|
||||
TEST_P(BoundingBoxEnableGrowthTest, boundingBoxEnableGrowth)
|
||||
{
|
||||
auto p = GetParam();
|
||||
bool gpu = std::get<0>(p);
|
||||
bool enableGrowth = std::get<1>(p);
|
||||
|
||||
OpenCLStatusRevert revert(cv::ocl::useOpenCL());
|
||||
|
||||
if (!gpu)
|
||||
cv::ocl::setUseOpenCL(false);
|
||||
|
||||
boundingBoxGrowthTest(enableGrowth);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(TSDF, BoundingBoxEnableGrowthTest, ::testing::Combine(::testing::Bool(), ::testing::Bool()));
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user