mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-12-04 16:59:12 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #22741 from savuor:hashtsdf_volposerot_fix

Browse Source 
There's a bug which appears when volume pose contains non-trivial rotation.
It results in wrong depth integration which can be observed during raycasting 
or points/normals export.

- This PR fixes the bug for both CPU and OpenCL
- There is a reproducer for the bug
- The copy behavior of VolumeSettings fixed (now copy constructor creates a deep copy)
- Minor changes (e.g. unused vars removed)
This commit is contained in:
Rostislav Vasilikhin 2022-11-17 07:10:35 +01:00 committed by GitHub
parent d976272d23
commit a9d98bfb34
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 195 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
modules/3d/include/opencv2/3d/volume_settings.hpp
View File

@ -31,6 +31,8 @@ public:
* @param volumeType volume type.
*/
VolumeSettings(VolumeType volumeType);
VolumeSettings(const VolumeSettings& vs);
~VolumeSettings();
/** @brief Sets the width of the image for integration.

18
modules/3d/src/opencl/hash_tsdf.cl
View File

@ -238,9 +238,8 @@ __kernel void integrateAllVolumeUnits(
__global const uchar* isActiveFlagsPtr,
int isActiveFlagsStep, int isActiveFlagsOffset,
int isActiveFlagsRows, int isActiveFlagsCols,
// cam matrices:
// cam matrix:
const float16 vol2cam,
const float16 camInv,
// scalars:
const float voxelSize,
const int volUnitResolution,
@ -272,15 +271,16 @@ __kernel void integrateAllVolumeUnits(
(allVolumePtr + table_offset + row * volCubed);
// volUnit2cam = world2cam * volUnit2world =
// camPoseInv * volUnitPose = camPoseInv * (volPose + idx * volUnitSize) =
// camPoseInv * (volPose + idx * volUnitResolution * voxelSize) =
// camPoseInv * (volPose + mulIdx) = camPoseInv * volPose + camPoseInv * mulIdx =
// vol2cam + camPoseInv * mulIdx
// camPoseInv * volUnitPose = camPoseInv * (volPose + volPoseRot*(idx * volUnitSize)) =
// camPoseInv * (volPose + volPoseRot*(idx * volUnitResolution * voxelSize)) =
// camPoseInv * (volPose + volPoseRot*mulIdx) = camPoseInv * volPose + camPoseInv * volPoseRot * mulIdx =
// vol2cam + camPoseInv * volPoseRot * mulIdx
float3 mulIdx = convert_float3(idx * volUnitResolution) * voxelSize;
float16 volUnit2cam = vol2cam;
volUnit2cam.s37b += (float3)(dot(mulIdx, camInv.s012),
dot(mulIdx, camInv.s456),
dot(mulIdx, camInv.s89a));
volUnit2cam.s37b += (float3)(dot(mulIdx, vol2cam.s012),
dot(mulIdx, vol2cam.s456),
dot(mulIdx, vol2cam.s89a));
integrateVolumeUnit(
i, j,

12
modules/3d/src/rgbd/hash_tsdf_functions.cpp
View File

@ -179,7 +179,7 @@ void integrateHashTsdfVolumeUnit(
{
VolumeUnit& vu = volumeUnits.emplace(idx, VolumeUnit()).first->second;
Matx44f subvolumePose = pose.translate(volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
Matx44f subvolumePose = pose.translate(pose.rotation() * volumeUnitIdxToVolume(idx, volumeUnitSize)).matrix;
vu.pose = subvolumePose;
vu.index = lastVolIndex; lastVolIndex++;
@ -204,7 +204,6 @@ void integrateHashTsdfVolumeUnit(
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) {
@ -510,7 +509,6 @@ void ocl_integrateHashTsdfVolumeUnit(
settings.getVolumePose(_pose);
const Affine3f pose = Affine3f(_pose);
Matx44f vol2camMatrix = (Affine3f(cameraPose).inv() * pose).matrix;
Matx44f camInvMatrix = Affine3f(cameraPose).inv().matrix;
// Save length to fill new data in ranges
int sizeBefore = hashTable.last;
@ -580,7 +578,6 @@ void ocl_integrateHashTsdfVolumeUnit(
ocl::KernelArg::ReadOnly(pixNorms),
ocl::KernelArg::ReadOnly(isActiveFlags),
vol2camMatrix,
camInvMatrix,
voxelSize,
volumeUnitResolution,
volStrides.val,
@ -1174,8 +1171,8 @@ void ocl_raycastHashTsdfVolumeUnit(
const float volumeUnitSize = voxelSize * volResolution.x;
Vec4f boxMin, boxMax(volumeUnitSize - voxelSize,
volumeUnitSize - voxelSize,
volumeUnitSize - voxelSize);
volumeUnitSize - voxelSize,
volumeUnitSize - voxelSize);
Matx44f _pose;
settings.getVolumePose(_pose);
@ -1186,9 +1183,6 @@ void ocl_raycastHashTsdfVolumeUnit(
Matx44f cam2volRotGPU = cam2vol.matrix;
Matx44f vol2camRotGPU = vol2cam.matrix;
UMat volPoseGpu = Mat(pose.matrix).getUMat(ACCESS_READ);
UMat invPoseGpu = Mat(pose.inv().matrix).getUMat(ACCESS_READ);
UMat hashesGpu = Mat(hashTable.hashes, false).getUMat(ACCESS_READ);
UMat hashDataGpu = Mat(hashTable.data, false).getUMat(ACCESS_READ);

5
modules/3d/src/rgbd/volume_settings_impl.cpp
View File

@ -233,6 +233,11 @@ VolumeSettings::VolumeSettings(VolumeType volumeType)
this->impl = makePtr<VolumeSettingsImpl>(volumeType);
}
VolumeSettings::VolumeSettings(const VolumeSettings& vs)
{
this->impl = makePtr<VolumeSettingsImpl>(*vs.impl.dynamicCast<VolumeSettingsImpl>());
}
VolumeSettings::~VolumeSettings() {}
void VolumeSettings::setIntegrateWidth(int val) { this->impl->setIntegrateWidth(val); };

176
modules/3d/test/test_tsdf.cpp
View File

@ -755,6 +755,162 @@ 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 << ")";
}
static Mat nanMask(Mat img)
{
int depth = img.depth();
Mat mask(img.size(), CV_8U);
for (int y = 0; y < img.rows; y++)
{
uchar *maskRow = mask.ptr<uchar>(y);
if (depth == CV_32F)
{
Vec4f *imgrow = img.ptr<Vec4f>(y);
for (int x = 0; x < img.cols; x++)
{
maskRow[x] = (imgrow[x] == imgrow[x]) * 255;
}
}
else if (depth == CV_64F)
{
Vec4d *imgrow = img.ptr<Vec4d>(y);
for (int x = 0; x < img.cols; x++)
{
maskRow[x] = (imgrow[x] == imgrow[x]) * 255;
}
}
}
return mask;
}
template <typename VT>
static Mat_<typename VT::value_type> normalsErrorT(Mat_<VT> srcNormals, Mat_<VT> dstNormals)
{
typedef typename VT::value_type Val;
Mat out(srcNormals.size(), cv::traits::Depth<Val>::value, Scalar(0));
for (int y = 0; y < srcNormals.rows; y++)
{
VT *srcrow = srcNormals[y];
VT *dstrow = dstNormals[y];
Val *outrow = out.ptr<Val>(y);
for (int x = 0; x < srcNormals.cols; x++)
{
VT sn = srcrow[x];
VT dn = dstrow[x];
Val dot = sn.dot(dn);
Val v(0.0);
// Just for rounding errors
if (std::abs(dot) < 1)
v = std::min(std::acos(dot), std::acos(-dot));
outrow[x] = v;
}
}
return out;
}
static Mat normalsError(Mat srcNormals, Mat dstNormals)
{
int depth = srcNormals.depth();
int channels = srcNormals.channels();
if (depth == CV_32F)
{
if (channels == 3)
{
return normalsErrorT<Vec3f>(srcNormals, dstNormals);
}
else if (channels == 4)
{
return normalsErrorT<Vec4f>(srcNormals, dstNormals);
}
}
else if (depth == CV_64F)
{
if (channels == 3)
{
return normalsErrorT<Vec3d>(srcNormals, dstNormals);
}
else if (channels == 4)
{
return normalsErrorT<Vec4d>(srcNormals, dstNormals);
}
}
else
{
CV_Error(Error::StsInternal, "This type is unsupported");
}
return Mat();
}
void regressionVolPoseRot()
{
// Make 2 volumes which differ only in their pose (especially rotation)
VolumeSettings vs(VolumeType::HashTSDF);
Volume volume0(VolumeType::HashTSDF, vs);
VolumeSettings vsRot(vs);
Matx44f pose;
vsRot.getVolumePose(pose);
pose = Affine3f(Vec3f(1, 1, 1), Vec3f()).matrix;
vsRot.setVolumePose(pose);
Volume volumeRot(VolumeType::HashTSDF, vsRot);
Size frameSize(vs.getRaycastWidth(), vs.getRaycastHeight());
Matx33f intr;
vs.getCameraIntegrateIntrinsics(intr);
bool onlySemisphere = false;
float depthFactor = vs.getDepthFactor();
Vec3f lightPose = Vec3f::all(0.f);
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);
volume0.integrate(udepth, poses[0].matrix);
volumeRot.integrate(udepth, poses[0].matrix);
UMat upts, unrm, uptsRot, unrmRot;
volume0.raycast(poses[0].matrix, upts, unrm);
volumeRot.raycast(poses[0].matrix, uptsRot, unrmRot);
Mat mpts = upts.getMat(ACCESS_READ), mnrm = unrm.getMat(ACCESS_READ);
Mat mptsRot = uptsRot.getMat(ACCESS_READ), mnrmRot = unrmRot.getMat(ACCESS_READ);
if (cvtest::debugLevel > 0)
{
displayImage(depth, mpts, mnrm, depthFactor, lightPose);
displayImage(depth, mptsRot, mnrmRot, depthFactor, lightPose);
}
std::vector<Mat> ptsCh(3), ptsRotCh(3);
split(mpts, ptsCh);
split(uptsRot, ptsRotCh);
Mat maskPts0 = ptsCh[2] > 0;
Mat maskPtsRot = ptsRotCh[2] > 0;
Mat maskNrm0 = nanMask(mnrm);
Mat maskNrmRot = nanMask(mnrmRot);
Mat maskPtsDiff, maskNrmDiff;
cv::bitwise_xor(maskPts0, maskPtsRot, maskPtsDiff);
cv::bitwise_xor(maskNrm0, maskNrmRot, maskNrmDiff);
double ptsDiffNorm = cv::sum(maskPtsDiff)[0]/255.0;
double nrmDiffNorm = cv::sum(maskNrmDiff)[0]/255.0;
EXPECT_LE(ptsDiffNorm, 786);
EXPECT_LE(nrmDiffNorm, 786);
double normPts = cv::norm(mpts, mptsRot, NORM_INF, (maskPts0 & maskPtsRot));
Mat absdot = normalsError(mnrm, mnrmRot);
double normNrm = cv::norm(absdot, NORM_L2, (maskNrm0 & maskNrmRot));
EXPECT_LE(normPts, 2.0);
EXPECT_LE(normNrm, 73.08);
}
#ifndef HAVE_OPENCL
TEST(TSDF, raycast_custom_framesize_normals_mat)
@ -857,6 +1013,11 @@ TEST(HashTSDF, valid_points_common_framesize_frame)
valid_points_test_common_framesize(VolumeType::HashTSDF, VolumeTestSrcType::ODOMETRY_FRAME);
}
TEST(HashTSDF, reproduce_volPoseRot)
{
regressionVolPoseRot();
}
TEST(ColorTSDF, raycast_custom_framesize_normals_mat)
{
normal_test_custom_framesize(VolumeType::ColorTSDF, VolumeTestFunction::RAYCAST, VolumeTestSrcType::MAT);
@ -1048,6 +1209,13 @@ TEST(HashTSDF_CPU, valid_points_common_framesize_frame)
cv::ocl::setUseOpenCL(true);
}
TEST(HashTSDF_CPU, reproduce_volPoseRot)
{
cv::ocl::setUseOpenCL(false);
regressionVolPoseRot();
cv::ocl::setUseOpenCL(true);
}
TEST(ColorTSDF_CPU, raycast_custom_framesize_normals_mat)
{
cv::ocl::setUseOpenCL(false);
@ -1118,6 +1286,14 @@ TEST(ColorTSDF_CPU, valid_points_common_framesize_fetch)
cv::ocl::setUseOpenCL(true);
}
// OpenCL tests
TEST(HashTSDF_GPU, reproduce_volPoseRot)
{
regressionVolPoseRot();
}
#endif
}
} // namespace