Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
211 lines
6.2 KiB
211 lines
6.2 KiB
// 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. |
|
|
|
#ifndef OPENCV_3D_TSDF_FUNCTIONS_HPP |
|
#define OPENCV_3D_TSDF_FUNCTIONS_HPP |
|
|
|
#include "../precomp.hpp" |
|
#include "utils.hpp" |
|
|
|
namespace cv |
|
{ |
|
|
|
typedef int8_t TsdfType; |
|
typedef uchar WeightType; |
|
|
|
struct TsdfVoxel |
|
{ |
|
TsdfVoxel(TsdfType _tsdf, WeightType _weight) : |
|
tsdf(_tsdf), weight(_weight) |
|
{ } |
|
TsdfType tsdf; |
|
WeightType weight; |
|
}; |
|
|
|
typedef Vec<uchar, sizeof(TsdfVoxel)> VecTsdfVoxel; |
|
|
|
typedef short int ColorType; |
|
struct RGBTsdfVoxel |
|
{ |
|
RGBTsdfVoxel(TsdfType _tsdf, WeightType _weight, ColorType _r, ColorType _g, ColorType _b) : |
|
tsdf(_tsdf), weight(_weight), r(_r), g(_g), b(_b) |
|
{ } |
|
TsdfType tsdf; |
|
WeightType weight; |
|
ColorType r, g, b; |
|
}; |
|
|
|
typedef Vec<uchar, sizeof(RGBTsdfVoxel)> VecRGBTsdfVoxel; |
|
|
|
#if CV_SIMD128 |
|
inline v_float32x4 tsdfToFloat_INTR(const v_int32x4& num) |
|
{ |
|
v_float32x4 num128 = v_setall_f32(-1.f / 128.f); |
|
return v_cvt_f32(num) * num128; |
|
} |
|
#endif |
|
|
|
inline TsdfType floatToTsdf(float num) |
|
{ |
|
//CV_Assert(-1 < num <= 1); |
|
int8_t res = int8_t(num * (-128.f)); |
|
res = res ? res : (num < 0 ? 1 : -1); |
|
return res; |
|
} |
|
|
|
inline float tsdfToFloat(TsdfType num) |
|
{ |
|
return float(num) * (-1.f / 128.f); |
|
} |
|
|
|
inline void colorFix(ColorType& r, ColorType& g, ColorType&b) |
|
{ |
|
if (r > 255) r = 255; |
|
if (g > 255) g = 255; |
|
if (b > 255) b = 255; |
|
} |
|
|
|
inline void colorFix(Point3f& c) |
|
{ |
|
if (c.x > 255) c.x = 255; |
|
if (c.y > 255) c.y = 255; |
|
if (c.z > 255) c.z = 255; |
|
} |
|
|
|
void preCalculationPixNorm(Size size, const Intr& intrinsics, Mat& pixNorm); |
|
#ifdef HAVE_OPENCL |
|
void ocl_preCalculationPixNorm(Size size, const Intr& intrinsics, UMat& pixNorm); |
|
#endif |
|
|
|
inline depthType bilinearDepth(const Depth& m, cv::Point2f pt) |
|
{ |
|
const bool fixMissingData = false; |
|
const depthType defaultValue = qnan; |
|
if (pt.x < 0 || pt.x >= m.cols - 1 || |
|
pt.y < 0 || pt.y >= m.rows - 1) |
|
return defaultValue; |
|
|
|
int xi = cvFloor(pt.x), yi = cvFloor(pt.y); |
|
|
|
const depthType* row0 = m[yi + 0]; |
|
const depthType* row1 = m[yi + 1]; |
|
|
|
depthType v00 = row0[xi + 0]; |
|
depthType v01 = row0[xi + 1]; |
|
depthType v10 = row1[xi + 0]; |
|
depthType v11 = row1[xi + 1]; |
|
|
|
// assume correct depth is positive |
|
bool b00 = v00 > 0; |
|
bool b01 = v01 > 0; |
|
bool b10 = v10 > 0; |
|
bool b11 = v11 > 0; |
|
|
|
if (!fixMissingData) |
|
{ |
|
if (!(b00 && b01 && b10 && b11)) |
|
return defaultValue; |
|
else |
|
{ |
|
float tx = pt.x - xi, ty = pt.y - yi; |
|
depthType v0 = v00 + tx * (v01 - v00); |
|
depthType v1 = v10 + tx * (v11 - v10); |
|
return v0 + ty * (v1 - v0); |
|
} |
|
} |
|
else |
|
{ |
|
int nz = b00 + b01 + b10 + b11; |
|
if (nz == 0) |
|
{ |
|
return defaultValue; |
|
} |
|
if (nz == 1) |
|
{ |
|
if (b00) return v00; |
|
if (b01) return v01; |
|
if (b10) return v10; |
|
if (b11) return v11; |
|
} |
|
if (nz == 2) |
|
{ |
|
if (b00 && b10) v01 = v00, v11 = v10; |
|
if (b01 && b11) v00 = v01, v10 = v11; |
|
if (b00 && b01) v10 = v00, v11 = v01; |
|
if (b10 && b11) v00 = v10, v01 = v11; |
|
if (b00 && b11) v01 = v10 = (v00 + v11) * 0.5f; |
|
if (b01 && b10) v00 = v11 = (v01 + v10) * 0.5f; |
|
} |
|
if (nz == 3) |
|
{ |
|
if (!b00) v00 = v10 + v01 - v11; |
|
if (!b01) v01 = v00 + v11 - v10; |
|
if (!b10) v10 = v00 + v11 - v01; |
|
if (!b11) v11 = v01 + v10 - v00; |
|
} |
|
|
|
float tx = pt.x - xi, ty = pt.y - yi; |
|
depthType v0 = v00 + tx * (v01 - v00); |
|
depthType v1 = v10 + tx * (v11 - v10); |
|
return v0 + ty * (v1 - v0); |
|
} |
|
} |
|
|
|
void _integrateVolumeUnit( |
|
float truncDist, float voxelSize, int maxWeight, |
|
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, |
|
InputArray _depth, float depthFactor, const cv::Matx44f& cameraPose, |
|
const cv::Intr& intrinsics, InputArray _pixNorms, InputArray _volume); |
|
|
|
void _integrateRGBVolumeUnit( |
|
float truncDist, float voxelSize, int maxWeight, |
|
cv::Matx44f _pose, Point3i volResolution, Vec4i volStrides, |
|
InputArray _depth, InputArray _rgb, float depthFactor, const cv::Matx44f& cameraPose, |
|
const cv::Intr& depth_intrinsics, const cv::Intr& rgb_intrinsics, InputArray _pixNorms, InputArray _volume); |
|
|
|
|
|
void integrateTsdfVolumeUnit( |
|
const VolumeSettings& settings, const Matx44f& cameraPose, |
|
InputArray _depth, InputArray _pixNorms, InputArray _volume); |
|
|
|
void integrateTsdfVolumeUnit( |
|
const VolumeSettings& settings, const Matx44f& volumePose, const Matx44f& cameraPose, |
|
InputArray _depth, InputArray _pixNorms, InputArray _volume); |
|
|
|
|
|
void raycastTsdfVolumeUnit(const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, |
|
InputArray _volume, OutputArray _points, OutputArray _normals); |
|
|
|
void fetchNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, |
|
InputArray _points, OutputArray _normals); |
|
|
|
void fetchPointsNormalsFromTsdfVolumeUnit(const VolumeSettings& settings, InputArray _volume, |
|
OutputArray points, OutputArray normals); |
|
|
|
|
|
#ifdef HAVE_OPENCL |
|
void ocl_integrateTsdfVolumeUnit( |
|
const VolumeSettings& settings, const Matx44f& cameraPose, |
|
InputArray _depth, InputArray _pixNorms, InputArray _volume); |
|
|
|
void ocl_raycastTsdfVolumeUnit( |
|
const VolumeSettings& settings, const Matx44f& cameraPose, int height, int width, InputArray intr, |
|
InputArray _volume, OutputArray _points, OutputArray _normals); |
|
|
|
void ocl_fetchNormalsFromTsdfVolumeUnit( |
|
const VolumeSettings& settings, InputArray _volume, |
|
InputArray _points, OutputArray _normals); |
|
|
|
void ocl_fetchPointsNormalsFromTsdfVolumeUnit( |
|
const VolumeSettings& settings, InputArray _volume, |
|
OutputArray _points, OutputArray _normals); |
|
#endif |
|
|
|
|
|
} // namespace cv |
|
|
|
#endif // include guard
|
|
|