Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
// 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
|
|
|
|
|
|
|
|
#include <opencv2/3d.hpp>
|
|
|
|
|
|
|
|
#include <opencv2/highgui.hpp>
|
|
|
|
#include <opencv2/3d.hpp>
|
|
|
|
#include <opencv2/imgproc.hpp>
|
|
|
|
#include <opencv2/core/utility.hpp>
|
|
|
|
#include <opencv2/core/quaternion.hpp>
|
|
|
|
|
|
|
|
#include <iostream>
|
|
|
|
#include <fstream>
|
|
|
|
|
|
|
|
using namespace std;
|
|
|
|
using namespace cv;
|
|
|
|
|
|
|
|
#define BILATERAL_FILTER 0// if 1 then bilateral filter will be used for the depth
|
|
|
|
|
|
|
|
static
|
|
|
|
void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
|
|
|
|
{
|
|
|
|
CV_Assert( timestamps.size() == Rt.size() );
|
|
|
|
|
|
|
|
ofstream file( filename.c_str() );
|
|
|
|
if( !file.is_open() )
|
|
|
|
return;
|
|
|
|
|
|
|
|
cout.precision(4);
|
|
|
|
for( size_t i = 0; i < Rt.size(); i++ )
|
|
|
|
{
|
|
|
|
const Mat& Rt_curr = Rt[i];
|
|
|
|
if( Rt_curr.empty() )
|
|
|
|
continue;
|
|
|
|
|
|
|
|
CV_Assert( Rt_curr.type() == CV_64FC1 );
|
|
|
|
|
|
|
|
Quatd rot = Quatd::createFromRotMat(Rt_curr(Rect(0, 0, 3, 3)));
|
|
|
|
|
|
|
|
// timestamp tx ty tz qx qy qz qw
|
|
|
|
file << timestamps[i] << " " << fixed
|
|
|
|
<< Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
|
|
|
|
<< rot.x << " " << rot.y << " " << rot.z << " " << rot.w << endl;
|
|
|
|
|
|
|
|
}
|
|
|
|
file.close();
|
|
|
|
}
|
|
|
|
|
|
|
|
static
|
|
|
|
void setCameraMatrixFreiburg1(float& fx, float& fy, float& cx, float& cy)
|
|
|
|
{
|
|
|
|
fx = 517.3f; fy = 516.5f; cx = 318.6f; cy = 255.3f;
|
|
|
|
}
|
|
|
|
|
|
|
|
static
|
|
|
|
void setCameraMatrixFreiburg2(float& fx, float& fy, float& cx, float& cy)
|
|
|
|
{
|
|
|
|
fx = 520.9f; fy = 521.0f; cx = 325.1f; cy = 249.7f;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* This sample helps to evaluate odometry on TUM datasets and benchmark http://vision.in.tum.de/data/datasets/rgbd-dataset.
|
|
|
|
* At this link you can find instructions for evaluation. The sample runs some opencv odometry and saves a camera trajectory
|
|
|
|
* to file of format that the benchmark requires. Saved file can be used for online evaluation.
|
|
|
|
*/
|
|
|
|
int main(int argc, char** argv)
|
|
|
|
{
|
|
|
|
if(argc != 4)
|
|
|
|
{
|
|
|
|
cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
vector<string> timestamps;
|
|
|
|
vector<Mat> Rts;
|
|
|
|
|
|
|
|
const string filename = argv[1];
|
|
|
|
ifstream file( filename.c_str() );
|
|
|
|
if( !file.is_open() )
|
|
|
|
return -1;
|
|
|
|
|
2021-12-03 01:53:44 +08:00
|
|
|
char dlmrt1 = '/';
|
|
|
|
char dlmrt2 = '\\';
|
|
|
|
|
|
|
|
size_t pos1 = filename.rfind(dlmrt1);
|
|
|
|
size_t pos2 = filename.rfind(dlmrt2);
|
|
|
|
size_t pos = pos1 < pos2 ? pos1 : pos2;
|
|
|
|
char dlmrt = pos1 < pos2 ? dlmrt1 : dlmrt2;
|
|
|
|
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;
|
|
|
|
|
|
|
|
const int timestampLength = 17;
|
|
|
|
const int rgbPathLehgth = 17+8;
|
|
|
|
const int depthPathLehgth = 17+10;
|
|
|
|
|
|
|
|
float fx = 525.0f, // default
|
|
|
|
fy = 525.0f,
|
|
|
|
cx = 319.5f,
|
|
|
|
cy = 239.5f;
|
|
|
|
if(filename.find("freiburg1") != string::npos)
|
|
|
|
setCameraMatrixFreiburg1(fx, fy, cx, cy);
|
|
|
|
if(filename.find("freiburg2") != string::npos)
|
|
|
|
setCameraMatrixFreiburg2(fx, fy, cx, cy);
|
|
|
|
Mat cameraMatrix = Mat::eye(3,3,CV_32FC1);
|
|
|
|
{
|
|
|
|
cameraMatrix.at<float>(0,0) = fx;
|
|
|
|
cameraMatrix.at<float>(1,1) = fy;
|
|
|
|
cameraMatrix.at<float>(0,2) = cx;
|
|
|
|
cameraMatrix.at<float>(1,2) = cy;
|
|
|
|
}
|
|
|
|
|
2021-12-03 01:53:44 +08:00
|
|
|
OdometrySettings ods;
|
|
|
|
ods.setCameraMatrix(cameraMatrix);
|
|
|
|
Odometry odometry;
|
|
|
|
String odname = string(argv[3]);
|
|
|
|
if (odname == "Rgbd")
|
|
|
|
odometry = Odometry(OdometryType::RGB, ods, OdometryAlgoType::COMMON);
|
|
|
|
else if (odname == "ICP")
|
|
|
|
odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::COMMON);
|
|
|
|
else if (odname == "RgbdICP")
|
|
|
|
odometry = Odometry(OdometryType::RGB_DEPTH, ods, OdometryAlgoType::COMMON);
|
|
|
|
else if (odname == "FastICP")
|
|
|
|
odometry = Odometry(OdometryType::DEPTH, ods, OdometryAlgoType::FAST);
|
|
|
|
else
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
{
|
2021-12-03 01:53:44 +08:00
|
|
|
std::cout << "Can not create Odometry algorithm. Check the passed odometry name." << std::endl;
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
return -1;
|
|
|
|
}
|
2021-12-03 01:53:44 +08:00
|
|
|
|
|
|
|
OdometryFrame frame_prev = odometry.createOdometryFrame(),
|
|
|
|
frame_curr = odometry.createOdometryFrame();
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
|
|
|
|
TickMeter gtm;
|
|
|
|
int count = 0;
|
|
|
|
for(int i = 0; !file.eof(); i++)
|
|
|
|
{
|
|
|
|
string str;
|
|
|
|
std::getline(file, str);
|
|
|
|
if(str.empty()) break;
|
|
|
|
if(str.at(0) == '#') continue; /* comment */
|
|
|
|
|
|
|
|
Mat image, depth;
|
|
|
|
// Read one pair (rgb and depth)
|
|
|
|
// example: 1305031453.359684 rgb/1305031453.359684.png 1305031453.374112 depth/1305031453.374112.png
|
|
|
|
#if BILATERAL_FILTER
|
|
|
|
TickMeter tm_bilateral_filter;
|
|
|
|
#endif
|
|
|
|
{
|
|
|
|
string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
|
|
|
|
string timestap = str.substr(0, timestampLength);
|
|
|
|
string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
|
|
|
|
|
|
|
|
image = imread(dirname + rgbFilename);
|
|
|
|
depth = imread(dirname + depthFilename, -1);
|
|
|
|
|
|
|
|
CV_Assert(!image.empty());
|
|
|
|
CV_Assert(!depth.empty());
|
|
|
|
CV_Assert(depth.type() == CV_16UC1);
|
|
|
|
|
|
|
|
// scale depth
|
|
|
|
Mat depth_flt;
|
|
|
|
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
|
|
|
|
#if !BILATERAL_FILTER
|
|
|
|
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth == 0);
|
|
|
|
depth = depth_flt;
|
|
|
|
#else
|
|
|
|
tm_bilateral_filter.start();
|
|
|
|
depth = Mat(depth_flt.size(), CV_32FC1, Scalar(0));
|
|
|
|
const double depth_sigma = 0.03;
|
|
|
|
const double space_sigma = 4.5; // in pixels
|
|
|
|
Mat invalidDepthMask = depth_flt == 0.f;
|
|
|
|
depth_flt.setTo(-5*depth_sigma, invalidDepthMask);
|
|
|
|
bilateralFilter(depth_flt, depth, -1, depth_sigma, space_sigma);
|
|
|
|
depth.setTo(std::numeric_limits<float>::quiet_NaN(), invalidDepthMask);
|
|
|
|
tm_bilateral_filter.stop();
|
|
|
|
cout << "Time filter " << tm_bilateral_filter.getTimeSec() << endl;
|
|
|
|
#endif
|
|
|
|
timestamps.push_back( timestap );
|
|
|
|
}
|
|
|
|
|
|
|
|
{
|
|
|
|
Mat gray;
|
|
|
|
cvtColor(image, gray, COLOR_BGR2GRAY);
|
2021-12-03 01:53:44 +08:00
|
|
|
frame_curr.setImage(gray);
|
|
|
|
frame_curr.setDepth(depth);
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
|
|
|
|
Mat Rt;
|
|
|
|
if(!Rts.empty())
|
|
|
|
{
|
|
|
|
TickMeter tm;
|
|
|
|
tm.start();
|
|
|
|
gtm.start();
|
2021-12-03 01:53:44 +08:00
|
|
|
odometry.prepareFrames(frame_curr, frame_prev);
|
|
|
|
bool res = odometry.compute(frame_curr, frame_prev, Rt);
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
gtm.stop();
|
|
|
|
tm.stop();
|
|
|
|
count++;
|
|
|
|
cout << "Time " << tm.getTimeSec() << endl;
|
|
|
|
#if BILATERAL_FILTER
|
|
|
|
cout << "Time ratio " << tm_bilateral_filter.getTimeSec() / tm.getTimeSec() << endl;
|
|
|
|
#endif
|
|
|
|
if(!res)
|
|
|
|
Rt = Mat::eye(4,4,CV_64FC1);
|
|
|
|
}
|
|
|
|
|
|
|
|
if( Rts.empty() )
|
|
|
|
Rts.push_back(Mat::eye(4,4,CV_64FC1));
|
|
|
|
else
|
|
|
|
{
|
|
|
|
Mat& prevRt = *Rts.rbegin();
|
|
|
|
cout << "Rt " << Rt << endl;
|
|
|
|
Rts.push_back( prevRt * Rt );
|
|
|
|
}
|
|
|
|
|
2021-12-03 01:53:44 +08:00
|
|
|
//if (!frame_prev.empty())
|
|
|
|
// frame_prev.release();
|
|
|
|
frame_prev = frame_curr;
|
|
|
|
frame_curr = odometry.createOdometryFrame();
|
|
|
|
//std::swap(frame_prev, frame_curr);
|
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d
* files moved from rgbd module in contrib repo
* header paths fixed
* perf file added
* lapack compilation fixed
* Rodrigues fixed in tests
* rgbd namespace removed
* headers fixed
* initial: rgbd files moved to 3d module
* rgbd updated from latest contrib master; less file duplication
* "std::" for sin(), cos(), etc.
* KinFu family -> back to contrib
* paths & namespaces
* removed duplicates, file version updated
* namespace kinfu removed from 3d module
* forgot to move test_colored_kinfu.cpp to contrib
* tests fixed: Params removed
* kinfu namespace removed
* it works without objc bindings
* include headers fixed
* tests: data paths fixed
* headers moved to/from public API
* Intr -> Matx33f in public API
* from kinfu_frame.hpp to utils.hpp
* submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers
* no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl
* get/setPyramidAt() instead of lots of methods
* Mat -> InputArray, TMat
* prepareFrameCache: refactored
* FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose
* Mat/UMat choose
* minor stuff related to headers
* (un)signed int warnings; compilation minor issues
* minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp
* FastICPOdometry: caching, rgbCameraMatrix
* OdometryFrame: pyramid%s% -> pyramids[]
* drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls
* makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp
* minors
* FastICPOdometry: caching updated, init fields
* OdometryFrameImpl<UMat> fixed
* matrix building fixed; minors
* returning linemode back to contrib
* params.pose is Mat now
* precomp headers reorganized
* minor fixes, header paths, extra header removed
* minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed
* moving declarations from/to headers
* internal headers reorganized (once again)
* fix include
* extra var fix
* fix include, fix (un)singed warning
* calibration.cpp: reverting back
* headers fix
* workaround to fix bindings
* temporary removed wrappers
* VolumeType -> VolumeParams
* (temporarily) removing wrappers for Volume and VolumeParams
* pyopencv_linemod -> contrib
* try to fix test_rgbd.py
* headers fixed
* fixing wrappers for rgbd
* fixing docs
* fixing rgbdPlane
* RgbdNormals wrapped
* wrap Volume and VolumeParams, VolumeType from enum to int
* DepthCleaner wrapped
* header folder "rgbd" -> "3d"
* fixing header path
* VolumeParams referenced by Ptr to support Python wrappers
* render...() fixed
* Ptr<VolumeParams> fixed
* makeVolume(... resolution -> [X, Y, Z])
* fixing static declaration
* try to fix ios objc bindings
* OdometryFrame::release...() removed
* fix for Odometry algos not supporting UMats: prepareFrameCache<>()
* preparePyramidMask(): fix to compile with TMat = UMat
* fixing debug guards
* removing references back; adding makeOdometryFrame() instead
* fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang)
* try to fix objc wrapper warnings; rerun builders
* VolumeType -> VolumeKind
* try to fix OCL bug
* prints removed
* indentation fixed
* headers fixed
* license fix
* WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already
* KinFu license notion shortened
* debugging code removed
* include guards fixed
* KinFu license left in contrib module
* isValidDepth() moved to private header
* indentation fix
* indentation fix in src files
* RgbdNormals rewritten to pImpl
* minor
* DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering
* minors, indentation
* no "private" in public headers
* depthTo3d test moved from separate file
* Normals: setDepth() is useless, removing it
* RgbdPlane => findPlanes()
* rescaleDepth(): minor
* warpFrame: minor
* minor TODO
* all Odometries (except base abstract class) rewritten to pImpl
* FastICPOdometry now supports maxRotation and maxTranslation
* minor
* Odometry's children: now checks are done in setters
* get rid of protected members in Odometry class
* get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl
* cameraMatrix: from double to float
* matrix exponentiation: Eigen -> dual quaternions
* Odometry evaluation fixed to reuse existing code
* "small" macro fixed by undef
* pixNorm is calculated on CPU only now (and then uploads on GPU)
* test registration: no cvtest classes
* test RgbdNormals and findPlanes(): no cvtest classes
* test_rgbd.py: minor fix
* tests for Odometry: no cvtest classes; UMat tests; logging fixed
* more CV_OVERRIDE to overriden functions
* fixing nondependent names to dependent
* more to prev commit
* forgotten fixes: overriden functions, (non)dependent names
* FastICPOdometry: fix UMat support when OpenCL is off
* try to fix compilation: missing namespaces
* Odometry: static const-mimicking functions to internal constants
* forgotten change to prev commit
* more forgotten fixes
* do not expose "submap.hpp" by default
* in-class enums: give names, CamelCase, int=>enums; minors
* namespaces, underscores, String
* std::map is used by pose graph, adding it
* compute()'s signature fixed, computeImpl()'s too
* RgbdNormals: Mat -> InputArray
* depth.hpp: Mat -> InputArray
* cameraMatrix: Matx33f -> InputArray + default value + checks
* "details" headers are not visible by default
* TSDF tests: rearranging checks
* cameraMatrix: no (realistic) default value
* renderPointsNormals*(): no wrappers for them
* debug: assert on empty frame in TSDF tests
* debugging code for TSDF GPU
* debug from integrate to raycast
* no (non-zero) default camera matrix anymore
* drop debugging code (does not help)
* try to fix TSDF GPU: constant -> global const ptr
2021-08-22 21:18:45 +08:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
std::cout << "Average time " << gtm.getAvgTimeSec() << std::endl;
|
|
|
|
writeResults(argv[2], timestamps, Rts);
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|