vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e
This commit is contained in:
Gitea Mirror Bot
@@ -0,0 +1,276 @@
|
||||
// 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
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/geometry.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/rgbd/colored_kinfu.hpp>
|
||||
|
||||
#include "io_utils.hpp"
|
||||
|
||||
using namespace cv;
|
||||
using namespace cv::kinfu;
|
||||
using namespace cv::colored_kinfu;
|
||||
using namespace cv::io_utils;
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
#include <opencv2/viz.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
const std::string vizWindowName = "cloud";
|
||||
|
||||
struct PauseCallbackArgs
|
||||
{
|
||||
PauseCallbackArgs(ColoredKinFu& _kf) : kf(_kf)
|
||||
{ }
|
||||
|
||||
ColoredKinFu& kf;
|
||||
};
|
||||
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args);
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args)
|
||||
{
|
||||
if(me.type == viz::MouseEvent::Type::MouseMove ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollUp)
|
||||
{
|
||||
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
|
||||
viz::Viz3d window(vizWindowName);
|
||||
UMat rendered;
|
||||
pca.kf.render(rendered, window.getViewerPose().matrix);
|
||||
imshow("render", rendered);
|
||||
waitKey(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static const char* keys =
|
||||
{
|
||||
"{help h usage ? | | print this message }"
|
||||
"{depth | | Path to folder with depth.txt and rgb.txt files listing a set of depth and rgb images }"
|
||||
"{camera |0| Index of depth camera to be used as a depth source }"
|
||||
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
|
||||
" in coarse mode points and normals are displayed }"
|
||||
"{idle | | Do not run KinFu, just display depth frames }"
|
||||
"{record | | Write depth frames to specified file list"
|
||||
" (the same format as for the 'depth' key) }"
|
||||
};
|
||||
|
||||
static const std::string message =
|
||||
"\nThis demo uses live depth input or RGB-D dataset taken from"
|
||||
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
|
||||
"\nto demonstrate KinectFusion implementation \n";
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
bool coarse = false;
|
||||
bool idle = false;
|
||||
std::string recordPath;
|
||||
|
||||
CommandLineParser parser(argc, argv, keys);
|
||||
parser.about(message);
|
||||
|
||||
if(!parser.check())
|
||||
{
|
||||
parser.printMessage();
|
||||
parser.printErrors();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(parser.has("help"))
|
||||
{
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
if(parser.has("coarse"))
|
||||
{
|
||||
coarse = true;
|
||||
}
|
||||
if(parser.has("record"))
|
||||
{
|
||||
recordPath = parser.get<String>("record");
|
||||
}
|
||||
if(parser.has("idle"))
|
||||
{
|
||||
idle = true;
|
||||
}
|
||||
|
||||
Ptr<DepthSource> ds;
|
||||
Ptr<RGBSource> rgbs;
|
||||
|
||||
if (parser.has("depth"))
|
||||
ds = makePtr<DepthSource>(parser.get<String>("depth") + "/depth.txt");
|
||||
else
|
||||
ds = makePtr<DepthSource>(parser.get<int>("camera"));
|
||||
|
||||
//TODO: intrinsics for camera
|
||||
rgbs = makePtr<RGBSource>(parser.get<String>("depth") + "/rgb.txt");
|
||||
|
||||
if (ds->empty())
|
||||
{
|
||||
std::cerr << "Failed to open depth source" << std::endl;
|
||||
parser.printMessage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Ptr<DepthWriter> depthWriter;
|
||||
Ptr<RGBWriter> rgbWriter;
|
||||
|
||||
if (!recordPath.empty())
|
||||
{
|
||||
depthWriter = makePtr<DepthWriter>(recordPath);
|
||||
rgbWriter = makePtr<RGBWriter>(recordPath);
|
||||
}
|
||||
Ptr<colored_kinfu::Params> params;
|
||||
Ptr<ColoredKinFu> kf;
|
||||
|
||||
params = colored_kinfu::Params::coloredTSDFParams(coarse);
|
||||
|
||||
// These params can be different for each depth sensor
|
||||
ds->updateParams(*params);
|
||||
|
||||
rgbs->updateParams(*params);
|
||||
|
||||
// Enables OpenCL explicitly (by default can be switched-off)
|
||||
cv::setUseOptimized(false);
|
||||
|
||||
// Scene-specific params should be tuned for each scene individually
|
||||
//float cubeSize = 1.f;
|
||||
//params->voxelSize = cubeSize/params->volumeDims[0]; //meters
|
||||
//params->tsdf_trunc_dist = 0.01f; //meters
|
||||
//params->icpDistThresh = 0.01f; //meters
|
||||
//params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
|
||||
//params->tsdf_max_weight = 16;
|
||||
|
||||
if(!idle)
|
||||
kf = ColoredKinFu::create(params);
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
cv::viz::Viz3d window(vizWindowName);
|
||||
window.setViewerPose(Affine3f::Identity());
|
||||
bool pause = false;
|
||||
#endif
|
||||
|
||||
UMat rendered;
|
||||
UMat points;
|
||||
UMat normals;
|
||||
|
||||
int64 prevTime = getTickCount();
|
||||
|
||||
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
|
||||
{
|
||||
if(depthWriter)
|
||||
depthWriter->append(frame);
|
||||
UMat rgb_frame = rgbs->getRGB();
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
if(pause)
|
||||
{
|
||||
// doesn't happen in idle mode
|
||||
kf->getCloud(points, normals);
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
|
||||
Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(kf->getParams().volumePose));
|
||||
PauseCallbackArgs pca(*kf);
|
||||
window.registerMouseCallback(pauseCallback, (void*)&pca);
|
||||
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
|
||||
"Close the window or press Q to resume"), Point()));
|
||||
window.spin();
|
||||
window.removeWidget("text");
|
||||
window.removeWidget("cloud");
|
||||
window.removeWidget("normals");
|
||||
window.registerMouseCallback(0);
|
||||
}
|
||||
|
||||
pause = false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
UMat cvt8;
|
||||
float depthFactor = params->depthFactor;
|
||||
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
|
||||
if(!idle)
|
||||
{
|
||||
imshow("depth", cvt8);
|
||||
imshow("rgb", rgb_frame);
|
||||
if(!kf->update(frame, rgb_frame))
|
||||
{
|
||||
kf->reset();
|
||||
}
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
else
|
||||
{
|
||||
if(coarse)
|
||||
{
|
||||
kf->getCloud(points, normals);
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
}
|
||||
}
|
||||
|
||||
//window.showWidget("worldAxes", viz::WCoordinateSystem());
|
||||
Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(kf->getParams().volumePose));
|
||||
window.setViewerPose(kf->getPose());
|
||||
window.spinOnce(1, true);
|
||||
}
|
||||
#endif
|
||||
|
||||
kf->render(rendered);
|
||||
}
|
||||
else
|
||||
{
|
||||
rendered = cvt8;
|
||||
}
|
||||
}
|
||||
|
||||
int64 newTime = getTickCount();
|
||||
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
|
||||
(int)(getTickFrequency()/(newTime - prevTime))),
|
||||
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
|
||||
prevTime = newTime;
|
||||
|
||||
imshow("render", rendered);
|
||||
|
||||
int c = waitKey(1);
|
||||
switch (c)
|
||||
{
|
||||
case 'r':
|
||||
if(!idle)
|
||||
kf->reset();
|
||||
break;
|
||||
case 'q':
|
||||
return 0;
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
case 'p':
|
||||
if(!idle)
|
||||
pause = true;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,304 @@
|
||||
// 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
|
||||
|
||||
#define CV_LOG_STRIP_LEVEL CV_LOG_LEVEL_VERBOSE
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/geometry.hpp>
|
||||
#include <opencv2/ptcloud.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/core/utils/logger.hpp>
|
||||
#include <opencv2/rgbd.hpp>
|
||||
#include "io_utils.hpp"
|
||||
|
||||
using namespace cv;
|
||||
using namespace cv::dynafu;
|
||||
using namespace cv::io_utils;
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
#include <opencv2/viz.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
const std::string vizWindowName = "cloud";
|
||||
|
||||
struct PauseCallbackArgs
|
||||
{
|
||||
PauseCallbackArgs(DynaFu& _df) : df(_df)
|
||||
{ }
|
||||
|
||||
DynaFu& df;
|
||||
};
|
||||
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args);
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args)
|
||||
{
|
||||
if(me.type == viz::MouseEvent::Type::MouseMove ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollUp)
|
||||
{
|
||||
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
|
||||
viz::Viz3d window(vizWindowName);
|
||||
UMat rendered;
|
||||
pca.df.render(rendered, window.getViewerPose().matrix);
|
||||
imshow("render", rendered);
|
||||
waitKey(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static const char* keys =
|
||||
{
|
||||
"{help h usage ? | | print this message }"
|
||||
"{depth | | Path to depth.txt file listing a set of depth images }"
|
||||
"{camera |0| Index of depth camera to be used as a depth source }"
|
||||
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
|
||||
" in coarse mode points and normals are displayed }"
|
||||
"{idle | | Do not run DynaFu, just display depth frames }"
|
||||
"{record | | Write depth frames to specified file list"
|
||||
" (the same format as for the 'depth' key) }"
|
||||
};
|
||||
|
||||
static const std::string message =
|
||||
"\nThis demo uses live depth input or RGB-D dataset taken from"
|
||||
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
|
||||
"\nto demonstrate KinectFusion implementation \n";
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
bool coarse = false;
|
||||
bool idle = false;
|
||||
std::string recordPath;
|
||||
|
||||
CommandLineParser parser(argc, argv, keys);
|
||||
parser.about(message);
|
||||
|
||||
if(!parser.check())
|
||||
{
|
||||
parser.printMessage();
|
||||
parser.printErrors();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(parser.has("help"))
|
||||
{
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
if(parser.has("coarse"))
|
||||
{
|
||||
coarse = true;
|
||||
}
|
||||
if(parser.has("record"))
|
||||
{
|
||||
recordPath = parser.get<String>("record");
|
||||
}
|
||||
if(parser.has("idle"))
|
||||
{
|
||||
idle = true;
|
||||
}
|
||||
|
||||
Ptr<DepthSource> ds;
|
||||
if (parser.has("depth"))
|
||||
ds = makePtr<DepthSource>(parser.get<String>("depth"));
|
||||
else
|
||||
ds = makePtr<DepthSource>(parser.get<int>("camera"));
|
||||
|
||||
if (ds->empty())
|
||||
{
|
||||
std::cerr << "Failed to open depth source" << std::endl;
|
||||
parser.printMessage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Ptr<DepthWriter> depthWriter;
|
||||
if(!recordPath.empty())
|
||||
depthWriter = makePtr<DepthWriter>(recordPath);
|
||||
|
||||
Ptr<kinfu::Params> params;
|
||||
Ptr<DynaFu> df;
|
||||
|
||||
if(coarse)
|
||||
params = kinfu::Params::coarseParams();
|
||||
else
|
||||
params = kinfu::Params::defaultParams();
|
||||
|
||||
// These params can be different for each depth sensor
|
||||
ds->updateParams(*params);
|
||||
|
||||
// Enables OpenCL explicitly (by default can be switched-off)
|
||||
cv::setUseOptimized(false);
|
||||
|
||||
// Scene-specific params should be tuned for each scene individually
|
||||
//params->volumePose = params->volumePose.translate(Vec3f(0.f, 0.f, 0.5f));
|
||||
//params->tsdf_max_weight = 16;
|
||||
|
||||
namedWindow("OpenGL Window", WINDOW_OPENGL);
|
||||
resizeWindow("OpenGL Window", 1, 1);
|
||||
if(!idle)
|
||||
df = DynaFu::create(params);
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
cv::viz::Viz3d window(vizWindowName);
|
||||
window.setViewerPose(Affine3f::Identity());
|
||||
bool pause = false;
|
||||
#endif
|
||||
|
||||
UMat rendered;
|
||||
UMat points;
|
||||
UMat normals;
|
||||
|
||||
int64 prevTime = getTickCount();
|
||||
|
||||
|
||||
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
|
||||
{
|
||||
Mat depthImg, vertImg, normImg;
|
||||
setOpenGlContext("OpenGL Window");
|
||||
df->renderSurface(depthImg, vertImg, normImg);
|
||||
if(!depthImg.empty())
|
||||
{
|
||||
UMat depthCvt8, vertCvt8, normCvt8;
|
||||
convertScaleAbs(depthImg, depthCvt8, 0.33*255);
|
||||
vertImg.convertTo(vertCvt8, CV_8UC3, 255);
|
||||
normImg.convertTo(normCvt8, CV_8UC3, 255);
|
||||
|
||||
imshow("Surface prediction", depthCvt8);
|
||||
imshow("vertex prediction", vertCvt8);
|
||||
imshow("normal prediction", normCvt8);
|
||||
}
|
||||
|
||||
if(depthWriter)
|
||||
depthWriter->append(frame);
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
if(pause)
|
||||
{
|
||||
// doesn't happen in idle mode
|
||||
df->getCloud(points, normals);
|
||||
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
|
||||
Vec3d volSize = df->getParams().voxelSize*Vec3d(df->getParams().volumeDims);
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(df->getParams().volumePose));
|
||||
PauseCallbackArgs pca(*df);
|
||||
window.registerMouseCallback(pauseCallback, (void*)&pca);
|
||||
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
|
||||
"Close the window or press Q to resume"), Point()));
|
||||
window.spin();
|
||||
window.removeWidget("text");
|
||||
//window.removeWidget("cloud");
|
||||
//window.removeWidget("normals");
|
||||
window.registerMouseCallback(0);
|
||||
}
|
||||
|
||||
pause = false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
UMat cvt8;
|
||||
float depthFactor = params->depthFactor;
|
||||
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
|
||||
if(!idle)
|
||||
{
|
||||
imshow("depth", cvt8);
|
||||
|
||||
if(!df->update(frame))
|
||||
{
|
||||
df->reset();
|
||||
std::cout << "reset" << std::endl;
|
||||
}
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
else
|
||||
{
|
||||
Mat meshCloud, meshEdges, meshPoly;
|
||||
df->marchCubes(meshCloud, meshEdges);
|
||||
for(int i = 0; i < meshEdges.size().height; i += 3)
|
||||
{
|
||||
meshPoly.push_back<int>(3);
|
||||
meshPoly.push_back<int>(meshEdges.at<int>(i, 0));
|
||||
meshPoly.push_back<int>(meshEdges.at<int>(i+1, 0));
|
||||
meshPoly.push_back<int>(meshEdges.at<int>(i+2, 0));
|
||||
}
|
||||
|
||||
viz::WMesh mesh(meshCloud.t(), meshPoly);
|
||||
window.showWidget("mesh", mesh);
|
||||
|
||||
if(coarse)
|
||||
{
|
||||
df->getCloud(points, normals);
|
||||
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
//window.showWidget("cloud", cloudWidget);
|
||||
//window.showWidget("normals", cloudNormals);
|
||||
if(!df->getNodesPos().empty())
|
||||
{
|
||||
viz::WCloud nodeCloud(df->getNodesPos(), viz::Color::red());
|
||||
nodeCloud.setRenderingProperty(viz::POINT_SIZE, 4);
|
||||
window.showWidget("nodes", nodeCloud);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//window.showWidget("worldAxes", viz::WCoordinateSystem());
|
||||
Vec3d volSize = df->getParams().voxelSize*df->getParams().volumeDims;
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(df->getParams().volumePose));
|
||||
window.setViewerPose(df->getPose());
|
||||
window.spinOnce(1, true);
|
||||
}
|
||||
#endif
|
||||
|
||||
df->render(rendered);
|
||||
}
|
||||
else
|
||||
{
|
||||
rendered = cvt8;
|
||||
}
|
||||
}
|
||||
|
||||
int64 newTime = getTickCount();
|
||||
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
|
||||
(int)(getTickFrequency()/(newTime - prevTime))),
|
||||
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
|
||||
prevTime = newTime;
|
||||
|
||||
imshow("render", rendered);
|
||||
|
||||
int c = waitKey(1);
|
||||
switch (c)
|
||||
{
|
||||
case 'r':
|
||||
if(!idle)
|
||||
df->reset();
|
||||
break;
|
||||
case 'q':
|
||||
return 0;
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
case 'p':
|
||||
if(!idle)
|
||||
pause = true;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,629 @@
|
||||
// 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_RGBS_IO_UTILS_HPP
|
||||
#define OPENCV_RGBS_IO_UTILS_HPP
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <opencv2/geometry.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/rgbd/kinfu.hpp>
|
||||
#include <opencv2/rgbd/large_kinfu.hpp>
|
||||
#include <opencv2/rgbd/colored_kinfu.hpp>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace io_utils
|
||||
{
|
||||
|
||||
static std::vector<std::string> readDepth(const std::string& fileList)
|
||||
{
|
||||
std::vector<std::string> v;
|
||||
|
||||
std::fstream file(fileList);
|
||||
if (!file.is_open())
|
||||
throw std::runtime_error("Failed to read depth 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 DepthWriter
|
||||
{
|
||||
DepthWriter(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 depth list");
|
||||
|
||||
file << "# depth maps saved from device" << std::endl;
|
||||
file << "# useless_number filename" << std::endl;
|
||||
}
|
||||
|
||||
void append(InputArray _depth)
|
||||
{
|
||||
Mat depth = _depth.getMat();
|
||||
std::string depthFname = cv::format("%04d.png", count);
|
||||
std::string fullDepthFname = dir + '/' + depthFname;
|
||||
if (!imwrite(fullDepthFname, depth))
|
||||
throw std::runtime_error("Failed to write depth to file " + fullDepthFname);
|
||||
file << count++ << " " << depthFname << std::endl;
|
||||
}
|
||||
|
||||
std::fstream file;
|
||||
int count;
|
||||
std::string dir;
|
||||
};
|
||||
|
||||
namespace Kinect2Params
|
||||
{
|
||||
static const Size depth_frameSize = Size(512, 424);
|
||||
// approximate values, no guarantee to be correct
|
||||
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 depth_frameSize = Size(640, 480);
|
||||
// approximate values, no guarantee to be correct
|
||||
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
|
||||
{
|
||||
public:
|
||||
enum Type
|
||||
{
|
||||
DEPTH_LIST,
|
||||
DEPTH_KINECT2_LIST,
|
||||
DEPTH_KINECT2,
|
||||
DEPTH_REALSENSE,
|
||||
DEPTH_ASTRA
|
||||
};
|
||||
|
||||
DepthSource(int cam) : DepthSource("", cam) {}
|
||||
|
||||
DepthSource(String fileListName) : DepthSource(fileListName, -1) {}
|
||||
|
||||
DepthSource(String fileListName, int cam)
|
||||
: depthFileList(fileListName.empty() ? std::vector<std::string>()
|
||||
: readDepth(fileListName)),
|
||||
frameIdx(0),
|
||||
undistortMap1(),
|
||||
undistortMap2()
|
||||
{
|
||||
if (cam >= 0)
|
||||
{
|
||||
vc = VideoCapture(VideoCaptureAPIs::CAP_OPENNI2 + cam);
|
||||
if (vc.isOpened())
|
||||
{
|
||||
if(cam == 20)
|
||||
sourceType = Type::DEPTH_ASTRA;
|
||||
else
|
||||
sourceType = Type::DEPTH_KINECT2;
|
||||
}
|
||||
else
|
||||
{
|
||||
vc = VideoCapture(VideoCaptureAPIs::CAP_REALSENSE + cam);
|
||||
if (vc.isOpened())
|
||||
{
|
||||
sourceType = Type::DEPTH_REALSENSE;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
vc = VideoCapture();
|
||||
sourceType = Type::DEPTH_KINECT2_LIST;
|
||||
}
|
||||
}
|
||||
|
||||
UMat getDepth()
|
||||
{
|
||||
UMat out;
|
||||
if (!vc.isOpened())
|
||||
{
|
||||
if (frameIdx < depthFileList.size())
|
||||
{
|
||||
Mat f = cv::imread(depthFileList[frameIdx++], IMREAD_ANYDEPTH);
|
||||
f.copyTo(out);
|
||||
}
|
||||
else
|
||||
{
|
||||
return UMat();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
vc.grab();
|
||||
switch (sourceType)
|
||||
{
|
||||
case Type::DEPTH_KINECT2: vc.retrieve(out, CAP_OPENNI_DEPTH_MAP); break;
|
||||
case Type::DEPTH_REALSENSE: vc.retrieve(out, CAP_INTELPERC_DEPTH_MAP); break;
|
||||
default:
|
||||
// unknown depth source
|
||||
vc.retrieve(out);
|
||||
}
|
||||
|
||||
// workaround for Kinect 2
|
||||
if (sourceType == Type::DEPTH_KINECT2)
|
||||
{
|
||||
out = out(Rect(Point(), Kinect2Params::depth_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 depthFileList.empty() && !(vc.isOpened()); }
|
||||
|
||||
void updateIntrinsics(Matx33f& _intrinsics, Size& _frameSize, float& _depthFactor)
|
||||
{
|
||||
if (vc.isOpened())
|
||||
{
|
||||
// this should be set in according to user's depth 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 fx, fy, cx, cy;
|
||||
float depthFactor = 1000.f;
|
||||
Size frameSize;
|
||||
if (sourceType == Type::DEPTH_KINECT2)
|
||||
{
|
||||
fx = fy = Kinect2Params::depth_focal;
|
||||
cx = Kinect2Params::depth_cx;
|
||||
cy = Kinect2Params::depth_cy;
|
||||
|
||||
frameSize = Kinect2Params::depth_frameSize;
|
||||
}
|
||||
else if (sourceType == Type::DEPTH_ASTRA)
|
||||
{
|
||||
fx = AstraParams::depth_fx;
|
||||
fy = AstraParams::depth_fy;
|
||||
cx = AstraParams::depth_cx;
|
||||
cy = AstraParams::depth_cy;
|
||||
|
||||
frameSize = AstraParams::depth_frameSize;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (sourceType == Type::DEPTH_REALSENSE)
|
||||
{
|
||||
fx = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_HORZ);
|
||||
fy = (float)vc.get(CAP_PROP_INTELPERC_DEPTH_FOCAL_LENGTH_VERT);
|
||||
depthFactor = 1.f / (float)vc.get(CAP_PROP_INTELPERC_DEPTH_SATURATION_VALUE);
|
||||
}
|
||||
else
|
||||
{
|
||||
fx = fy =
|
||||
(float)vc.get(CAP_OPENNI_DEPTH_GENERATOR | CAP_PROP_OPENNI_FOCAL_LENGTH);
|
||||
}
|
||||
|
||||
cx = w / 2 - 0.5f;
|
||||
cy = h / 2 - 0.5f;
|
||||
|
||||
frameSize = Size(w, h);
|
||||
}
|
||||
|
||||
Matx33f camMatrix = Matx33f(fx, 0, cx, 0, fy, cy, 0, 0, 1);
|
||||
_intrinsics = camMatrix;
|
||||
_frameSize = frameSize;
|
||||
_depthFactor = depthFactor;
|
||||
}
|
||||
}
|
||||
|
||||
void updateVolumeParams(const Vec3i& _resolution, float& _voxelSize, float& _tsdfTruncDist,
|
||||
Affine3f& _volumePose, float& _depthTruncateThreshold)
|
||||
{
|
||||
float volumeSize = 3.0f;
|
||||
_depthTruncateThreshold = 0.0f;
|
||||
// RealSense has shorter depth range, some params should be tuned
|
||||
if (sourceType == Type::DEPTH_REALSENSE)
|
||||
{
|
||||
volumeSize = 1.f;
|
||||
_voxelSize = volumeSize / _resolution[0];
|
||||
_tsdfTruncDist = 0.01f;
|
||||
_depthTruncateThreshold = 2.5f;
|
||||
}
|
||||
_volumePose = Affine3f().translate(Vec3f(-volumeSize / 2.f, -volumeSize / 2.f, 0.05f));
|
||||
}
|
||||
|
||||
void updateICPParams(float& _icpDistThresh, float& _bilateralSigmaDepth)
|
||||
{
|
||||
_icpDistThresh = 0.1f;
|
||||
_bilateralSigmaDepth = 0.04f;
|
||||
// RealSense has shorter depth range, some params should be tuned
|
||||
if (sourceType == Type::DEPTH_REALSENSE)
|
||||
{
|
||||
_icpDistThresh = 0.01f;
|
||||
_bilateralSigmaDepth = 0.01f;
|
||||
}
|
||||
}
|
||||
|
||||
void updateParams(large_kinfu::Params& params)
|
||||
{
|
||||
if (vc.isOpened())
|
||||
{
|
||||
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
|
||||
auto& volParams = params.volumeParams;
|
||||
Vec3i volResolution(volParams.resolutionX,
|
||||
volParams.resolutionY,
|
||||
volParams.resolutionZ);
|
||||
Affine3f volPose(Matx44f(volParams.pose));
|
||||
updateVolumeParams(volResolution, volParams.voxelSize, volParams.tsdfTruncDist, volPose,
|
||||
params.truncateThreshold);
|
||||
volParams.pose = Mat(volPose.matrix);
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void updateParams(kinfu::Params& params)
|
||||
{
|
||||
if (vc.isOpened())
|
||||
{
|
||||
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
|
||||
Affine3f volumePose(params.volumePose);
|
||||
updateVolumeParams(params.volumeDims, params.voxelSize,
|
||||
params.tsdf_trunc_dist, volumePose, params.truncateThreshold);
|
||||
params.volumePose = volumePose.matrix;
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void updateParams(colored_kinfu::Params& params)
|
||||
{
|
||||
if (vc.isOpened())
|
||||
{
|
||||
updateIntrinsics(params.intr, params.frameSize, params.depthFactor);
|
||||
Affine3f volumePose(params.volumePose);
|
||||
updateVolumeParams(params.volumeDims, params.voxelSize,
|
||||
params.tsdf_trunc_dist, volumePose, params.truncateThreshold);
|
||||
params.volumePose = volumePose.matrix;
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> depthFileList;
|
||||
size_t frameIdx;
|
||||
VideoCapture vc;
|
||||
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);
|
||||
Affine3f volumePose(params.volumePose);
|
||||
updateVolumeParams(params.volumeDims, params.voxelSize,
|
||||
params.tsdf_trunc_dist, volumePose);
|
||||
params.volumePose = volumePose.matrix;
|
||||
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
|
||||
#endif /* ifndef OPENCV_RGBS_IO_UTILS_HPP */
|
||||
@@ -0,0 +1,277 @@
|
||||
// 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
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/geometry.hpp>
|
||||
#include <opencv2/ptcloud.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/rgbd/kinfu.hpp>
|
||||
|
||||
#include "io_utils.hpp"
|
||||
|
||||
using namespace cv;
|
||||
using namespace cv::kinfu;
|
||||
using namespace cv::io_utils;
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
#include <opencv2/viz.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
const std::string vizWindowName = "cloud";
|
||||
|
||||
struct PauseCallbackArgs
|
||||
{
|
||||
PauseCallbackArgs(KinFu& _kf) : kf(_kf)
|
||||
{ }
|
||||
|
||||
KinFu& kf;
|
||||
};
|
||||
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args);
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args)
|
||||
{
|
||||
if(me.type == viz::MouseEvent::Type::MouseMove ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollUp)
|
||||
{
|
||||
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
|
||||
viz::Viz3d window(vizWindowName);
|
||||
UMat rendered;
|
||||
pca.kf.render(rendered, window.getViewerPose().matrix);
|
||||
imshow("render", rendered);
|
||||
waitKey(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static const char* keys =
|
||||
{
|
||||
"{help h usage ? | | print this message }"
|
||||
"{depth | | Path to depth.txt file listing a set of depth images }"
|
||||
"{camera |0| Index of depth camera to be used as a depth source }"
|
||||
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
|
||||
" in coarse mode points and normals are displayed }"
|
||||
"{useHashTSDF | | Use the newer hashtable based TSDFVolume (relatively fast) and for larger reconstructions}"
|
||||
"{idle | | Do not run KinFu, just display depth frames }"
|
||||
"{record | | Write depth frames to specified file list"
|
||||
" (the same format as for the 'depth' key) }"
|
||||
};
|
||||
|
||||
static const std::string message =
|
||||
"\nThis demo uses live depth input or RGB-D dataset taken from"
|
||||
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
|
||||
"\nto demonstrate KinectFusion implementation \n";
|
||||
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
bool coarse = false;
|
||||
bool idle = false;
|
||||
bool useHashTSDF = false;
|
||||
std::string recordPath;
|
||||
|
||||
CommandLineParser parser(argc, argv, keys);
|
||||
parser.about(message);
|
||||
|
||||
if(!parser.check())
|
||||
{
|
||||
parser.printMessage();
|
||||
parser.printErrors();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if(parser.has("help"))
|
||||
{
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
if(parser.has("coarse"))
|
||||
{
|
||||
coarse = true;
|
||||
}
|
||||
if(parser.has("record"))
|
||||
{
|
||||
recordPath = parser.get<String>("record");
|
||||
}
|
||||
if(parser.has("useHashTSDF"))
|
||||
{
|
||||
useHashTSDF = true;
|
||||
}
|
||||
if(parser.has("idle"))
|
||||
{
|
||||
idle = true;
|
||||
}
|
||||
|
||||
Ptr<DepthSource> ds;
|
||||
if (parser.has("depth"))
|
||||
ds = makePtr<DepthSource>(parser.get<String>("depth"));
|
||||
else
|
||||
ds = makePtr<DepthSource>(parser.get<int>("camera"));
|
||||
|
||||
if (ds->empty())
|
||||
{
|
||||
std::cerr << "Failed to open depth source" << std::endl;
|
||||
parser.printMessage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Ptr<DepthWriter> depthWriter;
|
||||
if(!recordPath.empty())
|
||||
depthWriter = makePtr<DepthWriter>(recordPath);
|
||||
|
||||
Ptr<Params> params;
|
||||
Ptr<KinFu> kf;
|
||||
|
||||
if(coarse)
|
||||
params = Params::coarseParams();
|
||||
else
|
||||
params = Params::defaultParams();
|
||||
|
||||
if(useHashTSDF)
|
||||
params = Params::hashTSDFParams(coarse);
|
||||
|
||||
// These params can be different for each depth sensor
|
||||
ds->updateParams(*params);
|
||||
|
||||
// Enables OpenCL explicitly (by default can be switched-off)
|
||||
cv::setUseOptimized(true);
|
||||
|
||||
// Scene-specific params should be tuned for each scene individually
|
||||
//float cubeSize = 1.f;
|
||||
//params->voxelSize = cubeSize/params->volumeDims[0]; //meters
|
||||
//params->tsdf_trunc_dist = 0.01f; //meters
|
||||
//params->icpDistThresh = 0.01f; //meters
|
||||
//params->volumePose = Affine3f().translate(Vec3f(-cubeSize/2.f, -cubeSize/2.f, 0.25f)); //meters
|
||||
//params->tsdf_max_weight = 16;
|
||||
|
||||
if(!idle)
|
||||
kf = KinFu::create(params);
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
cv::viz::Viz3d window(vizWindowName);
|
||||
window.setViewerPose(Affine3f::Identity());
|
||||
bool pause = false;
|
||||
#endif
|
||||
|
||||
UMat rendered;
|
||||
UMat points;
|
||||
UMat normals;
|
||||
|
||||
int64 prevTime = getTickCount();
|
||||
|
||||
for(UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
|
||||
{
|
||||
if(depthWriter)
|
||||
depthWriter->append(frame);
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
if(pause)
|
||||
{
|
||||
// doesn't happen in idle mode
|
||||
kf->getCloud(points, normals);
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
|
||||
Vec3d volSize = kf->getParams().voxelSize*Vec3d(kf->getParams().volumeDims);
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(kf->getParams().volumePose));
|
||||
PauseCallbackArgs pca(*kf);
|
||||
window.registerMouseCallback(pauseCallback, (void*)&pca);
|
||||
window.showWidget("text", viz::WText(cv::String("Move camera in this window. "
|
||||
"Close the window or press Q to resume"), Point()));
|
||||
window.spin();
|
||||
window.removeWidget("text");
|
||||
window.removeWidget("cloud");
|
||||
window.removeWidget("normals");
|
||||
window.registerMouseCallback(0);
|
||||
}
|
||||
|
||||
pause = false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
UMat cvt8;
|
||||
float depthFactor = params->depthFactor;
|
||||
convertScaleAbs(frame, cvt8, 0.25*256. / depthFactor);
|
||||
if(!idle)
|
||||
{
|
||||
imshow("depth", cvt8);
|
||||
if(!kf->update(frame))
|
||||
{
|
||||
kf->reset();
|
||||
std::cout << "reset" << std::endl;
|
||||
}
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
else
|
||||
{
|
||||
if(coarse)
|
||||
{
|
||||
kf->getCloud(points, normals);
|
||||
if(!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/1, /*scale*/0.05, viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
}
|
||||
}
|
||||
|
||||
//window.showWidget("worldAxes", viz::WCoordinateSystem());
|
||||
Vec3d volSize = kf->getParams().voxelSize*kf->getParams().volumeDims;
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0),
|
||||
volSize),
|
||||
Affine3f(kf->getParams().volumePose));
|
||||
window.setViewerPose(kf->getPose());
|
||||
window.spinOnce(1, true);
|
||||
}
|
||||
#endif
|
||||
|
||||
kf->render(rendered);
|
||||
}
|
||||
else
|
||||
{
|
||||
rendered = cvt8;
|
||||
}
|
||||
}
|
||||
|
||||
int64 newTime = getTickCount();
|
||||
putText(rendered, cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
|
||||
(int)(getTickFrequency()/(newTime - prevTime))),
|
||||
Point(0, rendered.rows-1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
|
||||
prevTime = newTime;
|
||||
|
||||
imshow("render", rendered);
|
||||
|
||||
int c = waitKey(1);
|
||||
switch (c)
|
||||
{
|
||||
case 'r':
|
||||
if(!idle)
|
||||
kf->reset();
|
||||
break;
|
||||
case 'q':
|
||||
return 0;
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
case 'p':
|
||||
if(!idle)
|
||||
pause = true;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
import numpy as np
|
||||
import cv2 as cv
|
||||
import sys
|
||||
|
||||
from argparse import ArgumentParser
|
||||
|
||||
def get_depth_list(folder):
|
||||
f = open(folder + '/depth.txt', 'r')
|
||||
rgb = [folder + '/' + s for s in f.read().split() if s.endswith('.png')]
|
||||
return rgb
|
||||
|
||||
def kinfu_demo():
|
||||
parser = ArgumentParser()
|
||||
parser.add_argument(
|
||||
"-i", "--input", help="Required. Path to folder with a input image file", required=True, type=str)
|
||||
parser.add_argument(
|
||||
"-t", "--large_kinfu", help="Required. Name of KinFu type", required=False, type=str)
|
||||
parser.add_argument(
|
||||
"-ocl", "--use_opencl", help="Required. Flag of OpenCL use", required=False, type=int, default=1)
|
||||
|
||||
args = parser.parse_args()
|
||||
print("Args: ", args)
|
||||
|
||||
cv.ocl.setUseOpenCL(args.use_opencl)
|
||||
|
||||
if (args.large_kinfu == None or args.large_kinfu == "0"):
|
||||
params = cv.kinfu_Params.defaultParams()
|
||||
kf = cv.kinfu_KinFu.create(params)
|
||||
elif (args.large_kinfu == "1"):
|
||||
params = cv.kinfu_Params.hashTSDFParams(False)
|
||||
kf = cv.kinfu_KinFu.create(params)
|
||||
else:
|
||||
raise ValueError("Incorrect kinfu type name")
|
||||
|
||||
depth_list = get_depth_list(args.input)
|
||||
for path in depth_list:
|
||||
|
||||
image = cv.imread(path, cv.IMREAD_ANYDEPTH)
|
||||
(height, width) = image.shape
|
||||
|
||||
cv.imshow('input', image)
|
||||
|
||||
size = height, width, 4
|
||||
cvt8 = np.zeros(size, dtype=np.uint8)
|
||||
|
||||
if not kf.update(image):
|
||||
kf.reset()
|
||||
else:
|
||||
kf.render(cvt8)
|
||||
cv.imshow('render', cvt8)
|
||||
cv.pollKey()
|
||||
cv.waitKey(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
print(__doc__)
|
||||
kinfu_demo()
|
||||
cv.destroyAllWindows()
|
||||
@@ -0,0 +1,263 @@
|
||||
// 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
|
||||
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <opencv2/geometry.hpp>
|
||||
#include <opencv2/ptcloud.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <opencv2/imgproc.hpp>
|
||||
#include <opencv2/rgbd/large_kinfu.hpp>
|
||||
|
||||
#include "io_utils.hpp"
|
||||
|
||||
using namespace cv;
|
||||
using namespace cv::kinfu;
|
||||
using namespace cv::large_kinfu;
|
||||
using namespace cv::io_utils;
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
#include <opencv2/viz.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
const std::string vizWindowName = "cloud";
|
||||
|
||||
struct PauseCallbackArgs
|
||||
{
|
||||
PauseCallbackArgs(LargeKinfu& _largeKinfu) : largeKinfu(_largeKinfu) {}
|
||||
|
||||
LargeKinfu& largeKinfu;
|
||||
};
|
||||
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args);
|
||||
void pauseCallback(const viz::MouseEvent& me, void* args)
|
||||
{
|
||||
if (me.type == viz::MouseEvent::Type::MouseMove ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollDown ||
|
||||
me.type == viz::MouseEvent::Type::MouseScrollUp)
|
||||
{
|
||||
PauseCallbackArgs pca = *((PauseCallbackArgs*)(args));
|
||||
viz::Viz3d window(vizWindowName);
|
||||
UMat rendered;
|
||||
pca.largeKinfu.render(rendered, window.getViewerPose().matrix);
|
||||
imshow("render", rendered);
|
||||
waitKey(1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static const char* keys = {
|
||||
"{help h usage ? | | print this message }"
|
||||
"{depth | | Path to depth.txt file listing a set of depth images }"
|
||||
"{camera |0| Index of depth camera to be used as a depth source }"
|
||||
"{coarse | | Run on coarse settings (fast but ugly) or on default (slow but looks better),"
|
||||
" in coarse mode points and normals are displayed }"
|
||||
"{idle | | Do not run LargeKinfu, just display depth frames }"
|
||||
"{record | | Write depth frames to specified file list"
|
||||
" (the same format as for the 'depth' key) }"
|
||||
};
|
||||
|
||||
static const std::string message =
|
||||
"\nThis demo uses live depth input or RGB-D dataset taken from"
|
||||
"\nhttps://vision.in.tum.de/data/datasets/rgbd-dataset"
|
||||
"\nto demonstrate Submap based large environment reconstruction"
|
||||
"\nThis module uses the newer hashtable based TSDFVolume (relatively fast) for larger "
|
||||
"reconstructions by default\n";
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
bool coarse = false;
|
||||
bool idle = false;
|
||||
std::string recordPath;
|
||||
|
||||
CommandLineParser parser(argc, argv, keys);
|
||||
parser.about(message);
|
||||
|
||||
if (!parser.check())
|
||||
{
|
||||
parser.printMessage();
|
||||
parser.printErrors();
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (parser.has("help"))
|
||||
{
|
||||
parser.printMessage();
|
||||
return 0;
|
||||
}
|
||||
if (parser.has("coarse"))
|
||||
{
|
||||
coarse = true;
|
||||
}
|
||||
if (parser.has("record"))
|
||||
{
|
||||
recordPath = parser.get<String>("record");
|
||||
}
|
||||
if (parser.has("idle"))
|
||||
{
|
||||
idle = true;
|
||||
}
|
||||
|
||||
Ptr<DepthSource> ds;
|
||||
if (parser.has("depth"))
|
||||
ds = makePtr<DepthSource>(parser.get<String>("depth"));
|
||||
else
|
||||
ds = makePtr<DepthSource>(parser.get<int>("camera"));
|
||||
|
||||
if (ds->empty())
|
||||
{
|
||||
std::cerr << "Failed to open depth source" << std::endl;
|
||||
parser.printMessage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Ptr<DepthWriter> depthWriter;
|
||||
if (!recordPath.empty())
|
||||
depthWriter = makePtr<DepthWriter>(recordPath);
|
||||
|
||||
Ptr<large_kinfu::Params> params;
|
||||
Ptr<LargeKinfu> largeKinfu;
|
||||
|
||||
params = large_kinfu::Params::hashTSDFParams(coarse);
|
||||
|
||||
// These params can be different for each depth sensor
|
||||
ds->updateParams(*params);
|
||||
|
||||
cv::setUseOptimized(true);
|
||||
|
||||
if (!idle)
|
||||
largeKinfu = LargeKinfu::create(params);
|
||||
|
||||
const auto& volParams = largeKinfu->getParams().volumeParams;
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
cv::viz::Viz3d window(vizWindowName);
|
||||
window.setViewerPose(Affine3f::Identity());
|
||||
bool pause = false;
|
||||
#endif
|
||||
|
||||
UMat rendered;
|
||||
UMat points;
|
||||
UMat normals;
|
||||
|
||||
int64 prevTime = getTickCount();
|
||||
|
||||
for (UMat frame = ds->getDepth(); !frame.empty(); frame = ds->getDepth())
|
||||
{
|
||||
if (depthWriter)
|
||||
depthWriter->append(frame);
|
||||
|
||||
Vec3i volResolution(volParams.resolutionX,
|
||||
volParams.resolutionY,
|
||||
volParams.resolutionZ);
|
||||
Affine3f volPose(Matx44f(volParams.pose));
|
||||
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
if (pause)
|
||||
{
|
||||
// doesn't happen in idle mode
|
||||
largeKinfu->getCloud(points, normals);
|
||||
if (!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1, /*scale*/ 0.05,
|
||||
viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
|
||||
Vec3d volSize = volParams.voxelSize * Vec3d(volResolution);
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
|
||||
PauseCallbackArgs pca(*largeKinfu);
|
||||
window.registerMouseCallback(pauseCallback, (void*)&pca);
|
||||
window.showWidget("text",
|
||||
viz::WText(cv::String("Move camera in this window. "
|
||||
"Close the window or press Q to resume"),
|
||||
Point()));
|
||||
window.spin();
|
||||
window.removeWidget("text");
|
||||
window.removeWidget("cloud");
|
||||
window.removeWidget("normals");
|
||||
window.registerMouseCallback(0);
|
||||
}
|
||||
|
||||
pause = false;
|
||||
}
|
||||
else
|
||||
#endif
|
||||
{
|
||||
UMat cvt8;
|
||||
float depthFactor = params->depthFactor;
|
||||
convertScaleAbs(frame, cvt8, 0.25 * 256. / depthFactor);
|
||||
if (!idle)
|
||||
{
|
||||
imshow("depth", cvt8);
|
||||
|
||||
if (!largeKinfu->update(frame))
|
||||
{
|
||||
largeKinfu->reset();
|
||||
std::cout << "reset" << std::endl;
|
||||
}
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
else
|
||||
{
|
||||
if (coarse)
|
||||
{
|
||||
largeKinfu->getCloud(points, normals);
|
||||
if (!points.empty() && !normals.empty())
|
||||
{
|
||||
viz::WCloud cloudWidget(points, viz::Color::white());
|
||||
viz::WCloudNormals cloudNormals(points, normals, /*level*/ 1,
|
||||
/*scale*/ 0.05, viz::Color::gray());
|
||||
window.showWidget("cloud", cloudWidget);
|
||||
window.showWidget("normals", cloudNormals);
|
||||
}
|
||||
}
|
||||
|
||||
// window.showWidget("worldAxes", viz::WCoordinateSystem());
|
||||
Vec3d volSize = volParams.voxelSize * volResolution;
|
||||
window.showWidget("cube", viz::WCube(Vec3d::all(0), volSize), volPose);
|
||||
window.setViewerPose(largeKinfu->getPose());
|
||||
window.spinOnce(1, true);
|
||||
}
|
||||
#endif
|
||||
|
||||
largeKinfu->render(rendered);
|
||||
}
|
||||
else
|
||||
{
|
||||
rendered = cvt8;
|
||||
}
|
||||
}
|
||||
|
||||
int64 newTime = getTickCount();
|
||||
putText(rendered,
|
||||
cv::format("FPS: %2d press R to reset, P to pause, Q to quit",
|
||||
(int)(getTickFrequency() / (newTime - prevTime))),
|
||||
Point(0, rendered.rows - 1), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 255, 255));
|
||||
prevTime = newTime;
|
||||
imshow("render", rendered);
|
||||
|
||||
int c = waitKey(1);
|
||||
switch (c)
|
||||
{
|
||||
case 'r':
|
||||
if (!idle)
|
||||
largeKinfu->reset();
|
||||
break;
|
||||
case 'q': return 0;
|
||||
#ifdef HAVE_OPENCV_VIZ
|
||||
case 'p':
|
||||
if (!idle)
|
||||
pause = true;
|
||||
#endif
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user