opencv/opencv_contrib
1
0
Fork 0
mirror of https://github.com/opencv/opencv_contrib.git synced 2025-10-21 14:41:58 +08:00
Code Issues Releases Wiki Activity

Merge pull request #2878 from DumDereDum:colored_kinfu

Browse Source 
Colored Kinect Fusion

* create dummy unworked classes

* dummy class works

* colored kinfu demo minor changes

* add simple rgb reader and writer

* add rgb info to tsdf

* add new raycast

* replace TsdfVoxel with RGBTsdfVoxel; integrate done

* add colors info to raycast

* just render colors

* color processing fix

* intergrate color fix

* add simple shift; minor fixes

* add cv_errors for not implemented functions

* minor fixes

* made calibration

* makeColoredFrameFromDepth works

* remove comments

* makeColoredFrameFromDepth fix

* RGBSoure remove extra code

* RGBSoure remove extra code 1

* simple fix of bug with rgb size

* minor fix

* docs fix

* docs fix

* unused parameter fix

* warnings fix

* fix tsdf errors; rgbd_perf warnings

* fix errors

* minor fix

* debug print

* debug print 2

* debug print 3

* invoker fix

* minor fix

* remove debug cout

* add simple tests

* docs fix

* warnings fix

* minor fix

* function warning fix

* add vectorized code

* minor fixes

* minor speed up

* minor fixes; renaming

* new minor fixes

* bug fix

* minor fix

* test update

* extra code removed

* bugfix; warning fix

* simple interpolation

* minor fix

* color work minor fix

* vectorized interpolation

* minor fix
This commit is contained in:
DumDereDum
2021-04-07 23:49:57 +03:00
committed by GitHub
parent dc3cd0e3c0
commit da6a95e0db
22 changed files with 3441 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

340
modules/rgbd/samples/io_utils.hpp
View File

@@ -12,6 +12,7 @@
#include <opencv2/highgui.hpp>
#include <opencv2/rgbd/kinfu.hpp>
#include <opencv2/rgbd/large_kinfu.hpp>
#include <opencv2/rgbd/colored_kinfu.hpp>
namespace cv
{
@@ -79,27 +80,45 @@ struct DepthWriter
namespace Kinect2Params
{
static const Size frameSize = Size(512, 424);
static const Size depth_frameSize = Size(512, 424);
// approximate values, no guarantee to be correct
static const float focal = 366.1f;
static const float cx = 258.2f;
static const float cy = 204.f;
static const float k1 = 0.12f;
static const float k2 = -0.34f;
static const float k3 = 0.12f;
static const float depth_focal = 366.1f;
static const float depth_cx = 258.2f;
static const float depth_cy = 204.f;
static const float depth_k1 = 0.12f;
static const float depth_k2 = -0.34f;
static const float depth_k3 = 0.12f;
static const Size rgb_frameSize = Size(640, 480);
static const float rgb_focal = 525.0f;
static const float rgb_cx = 319.5f;
static const float rgb_cy = 239.5f;
static const float rgb_k1 = 0.0f;
static const float rgb_k2 = 0.0f;
static const float rgb_k3 = 0.0f;
}; // namespace Kinect2Params
namespace AstraParams
{
static const Size frameSize = Size(640, 480);
static const Size depth_frameSize = Size(640, 480);
// approximate values, no guarantee to be correct
static const float fx = 535.4f;
static const float fy = 539.2f;
static const float cx = 320.1f;
static const float cy = 247.6f;
static const float k1 = 0.0f;
static const float k2 = 0.0f;
static const float k3 = 0.0f;
static const float depth_fx = 535.4f;
static const float depth_fy = 539.2f;
static const float depth_cx = 320.1f;
static const float depth_cy = 247.6f;
static const float depth_k1 = 0.0f;
static const float depth_k2 = 0.0f;
static const float depth_k3 = 0.0f;
static const Size rgb_frameSize = Size(640, 480);
static const float rgb_focal = 525.0f;
static const float rgb_cx = 319.5f;
static const float rgb_cy = 239.5f;
static const float rgb_k1 = 0.0f;
static const float rgb_k2 = 0.0f;
static const float rgb_k3 = 0.0f;
}; // namespace Kinect2Params
struct DepthSource
@@ -181,7 +200,7 @@ struct DepthSource
// workaround for Kinect 2
if (sourceType == Type::DEPTH_KINECT2)
{
out = out(Rect(Point(), Kinect2Params::frameSize));
out = out(Rect(Point(), Kinect2Params::depth_frameSize));
UMat outCopy;
// linear remap adds gradient between valid and invalid pixels
@@ -212,20 +231,20 @@ struct DepthSource
Size frameSize;
if (sourceType == Type::DEPTH_KINECT2)
{
fx = fy = Kinect2Params::focal;
cx = Kinect2Params::cx;
cy = Kinect2Params::cy;
fx = fy = Kinect2Params::depth_focal;
cx = Kinect2Params::depth_cx;
cy = Kinect2Params::depth_cy;
frameSize = Kinect2Params::frameSize;
frameSize = Kinect2Params::depth_frameSize;
}
else if (sourceType == Type::DEPTH_ASTRA)
{
fx = AstraParams::fx;
fy = AstraParams::fy;
cx = AstraParams::cx;
cy = AstraParams::cy;
fx = AstraParams::depth_fx;
fy = AstraParams::depth_fy;
cx = AstraParams::depth_cx;
cy = AstraParams::depth_cy;
frameSize = AstraParams::frameSize;
frameSize = AstraParams::depth_frameSize;
}
else
{
@@ -295,9 +314,9 @@ struct DepthSource
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::k1;
distCoeffs(1) = Kinect2Params::k2;
distCoeffs(4) = Kinect2Params::k3;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
@@ -317,9 +336,31 @@ struct DepthSource
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::k1;
distCoeffs(1) = Kinect2Params::k2;
distCoeffs(4) = Kinect2Params::k3;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
void updateParams(colored_kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
updateVolumeParams(params.volumeDims, params.voxelSize,
params.tsdf_trunc_dist, params.volumePose, params.truncateThreshold);
updateICPParams(params.icpDistThresh, params.bilateral_sigma_depth);
if (sourceType == Type::DEPTH_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::depth_k1;
distCoeffs(1) = Kinect2Params::depth_k2;
distCoeffs(4) = Kinect2Params::depth_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
@@ -333,6 +374,243 @@ struct DepthSource
UMat undistortMap1, undistortMap2;
Type sourceType;
};
static std::vector<std::string> readRGB(const std::string& fileList)
{
std::vector<std::string> v;
std::fstream file(fileList);
if (!file.is_open())
throw std::runtime_error("Failed to read rgb list");
std::string dir;
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
while (!file.eof())
{
std::string s, imgPath;
std::getline(file, s);
if (s.empty() || s[0] == '#')
continue;
std::stringstream ss;
ss << s;
double thumb;
ss >> thumb >> imgPath;
v.push_back(dir + '/' + imgPath);
}
return v;
}
struct RGBWriter
{
RGBWriter(std::string fileList) : file(fileList, std::ios::out), count(0), dir()
{
size_t slashIdx = fileList.rfind('/');
slashIdx = slashIdx != std::string::npos ? slashIdx : fileList.rfind('\\');
dir = fileList.substr(0, slashIdx);
if (!file.is_open())
throw std::runtime_error("Failed to write rgb list");
file << "# rgb maps saved from device" << std::endl;
file << "# useless_number filename" << std::endl;
}
void append(InputArray _rgb)
{
Mat rgb = _rgb.getMat();
std::string rgbFname = cv::format("%04d.png", count);
std::string fullRGBFname = dir + '/' + rgbFname;
if (!imwrite(fullRGBFname, rgb))
throw std::runtime_error("Failed to write rgb to file " + fullRGBFname);
file << count++ << " " << rgbFname << std::endl;
}
std::fstream file;
int count;
std::string dir;
};
struct RGBSource
{
public:
enum Type
{
RGB_LIST,
RGB_KINECT2_LIST,
RGB_KINECT2,
RGB_REALSENSE,
RGB_ASTRA
};
RGBSource(int cam) : RGBSource("", cam) {}
RGBSource(String fileListName) : RGBSource(fileListName, -1) {}
RGBSource(String fileListName, int cam)
: rgbFileList(fileListName.empty() ? std::vector<std::string>()
: readRGB(fileListName)),
frameIdx(0),
undistortMap1(),
undistortMap2()
{
if (cam >= 0)
{
vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
if (vc.isOpened())
{
if(cam == 20)
sourceType = Type::RGB_ASTRA;
else
sourceType = Type::RGB_KINECT2;
}
else
{
vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
if (vc.isOpened())
{
sourceType = Type::RGB_REALSENSE;
}
}
}
else
{
vc = VideoCapture();
sourceType = Type::RGB_KINECT2_LIST;
}
}
UMat getRGB()
{
UMat out;
if (!vc.isOpened())
{
if (frameIdx < rgbFileList.size())
{
Mat f = cv::imread(rgbFileList[frameIdx++], IMREAD_COLOR);
f.copyTo(out);
}
else
{
return UMat();
}
}
else
{
vc.grab();
switch (sourceType)
{
case Type::RGB_KINECT2: vc.retrieve(out, CAP_OPENNI_BGR_IMAGE); break;
case Type::RGB_REALSENSE: vc.retrieve(out, CAP_INTELPERC_IMAGE); break;
default:
// unknown rgb source
vc.retrieve(out);
}
// workaround for Kinect 2
if (sourceType == Type::RGB_KINECT2)
{
out = out(Rect(Point(), Kinect2Params::rgb_frameSize));
UMat outCopy;
// linear remap adds gradient between valid and invalid pixels
// which causes garbage, use nearest instead
remap(out, outCopy, undistortMap1, undistortMap2, cv::INTER_NEAREST);
cv::flip(outCopy, out, 1);
}
}
if (out.empty())
throw std::runtime_error("Matrix is empty");
return out;
}
bool empty() { return rgbFileList.empty() && !(vc.isOpened()); }
void updateIntrinsics(Matx33f& _rgb_intrinsics, Size& _rgb_frameSize)
{
if (vc.isOpened())
{
// this should be set in according to user's rgb sensor
int w = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_WIDTH);
int h = (int)vc.get(VideoCaptureProperties::CAP_PROP_FRAME_HEIGHT);
// it's recommended to calibrate sensor to obtain its intrinsics
float rgb_fx, rgb_fy, rgb_cx, rgb_cy;
Size rgb_frameSize;
if (sourceType == Type::RGB_KINECT2)
{
rgb_fx = rgb_fy = Kinect2Params::rgb_focal;
rgb_cx = Kinect2Params::rgb_cx;
rgb_cy = Kinect2Params::rgb_cy;
rgb_frameSize = Kinect2Params::rgb_frameSize;
}
else if (sourceType == Type::RGB_ASTRA)
{
rgb_fx = rgb_fy = AstraParams::rgb_focal;
rgb_cx = AstraParams::rgb_cx;
rgb_cy = AstraParams::rgb_cy;
rgb_frameSize = AstraParams::rgb_frameSize;
}
else
{
// TODO: replace to rgb types
rgb_fx = rgb_fy = Kinect2Params::rgb_focal;
rgb_cx = Kinect2Params::rgb_cx;
rgb_cy = Kinect2Params::rgb_cy;
rgb_frameSize = Size(w, h);
}
Matx33f rgb_camMatrix = Matx33f(rgb_fx, 0, rgb_cx, 0, rgb_fy, rgb_cy, 0, 0, 1);
_rgb_intrinsics = rgb_camMatrix;
_rgb_frameSize = rgb_frameSize;
}
}
void updateVolumeParams(const Vec3i&, float&, float&, Affine3f&)
{
// TODO: do this settings for rgb image
}
void updateICPParams(float&)
{
// TODO: do this settings for rgb image icp
}
void updateParams(colored_kinfu::Params& params)
{
if (vc.isOpened())
{
updateIntrinsics(params.rgb_intr, params.rgb_frameSize);
updateVolumeParams(params.volumeDims, params.voxelSize,
params.tsdf_trunc_dist, params.volumePose);
updateICPParams(params.icpDistThresh);
if (sourceType == Type::RGB_KINECT2)
{
Matx<float, 1, 5> distCoeffs;
distCoeffs(0) = Kinect2Params::rgb_k1;
distCoeffs(1) = Kinect2Params::rgb_k2;
distCoeffs(4) = Kinect2Params::rgb_k3;
initUndistortRectifyMap(params.intr, distCoeffs, cv::noArray(), params.intr,
params.frameSize, CV_16SC2, undistortMap1, undistortMap2);
}
}
}
std::vector<std::string> rgbFileList;
size_t frameIdx;
VideoCapture vc;
UMat undistortMap1, undistortMap2;
Type sourceType;
};
} // namespace io_utils
} // namespace cv