opencv/modules/3d/samples/odometry_evaluation.cpp

229 lines
7.4 KiB
C++
Raw Normal View History

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;
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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;
}
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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
{
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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;
}
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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);
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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();
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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 );
}
Merge pull request #20755 from DumDereDum:new_odometry New odometry Pipeline * first intergation * tests run, but not pass * add previous version of sigma calc * add minor comment * strange fixes * fix fast ICP * test changes; fast icp still not work correctly * finaly, it works * algtype fix * change affine comparison * boolean return * fix bug with angle and cos * test pass correctly * fix for kinfu pipeline * add compute points normals * update for new odometry * change odometry_evaluation * odometry_evaluation works * change debug logs * minor changes * change depth setting in odometryFrame * fastICP works with 4num points * all odometries work with 4mun points * odometry full works on 4num points and normals * replace ICP with DEPTH; comments replacements * create prepareFrame; add docs for Odometry * change getPyramids() * delete extra code * add intrinsics; but dont works * bugfix with nan checking * add gpu impl * change createOdometryFrame func * remove old fastICP code * comments fix * add comments * minor fixes * other minor fixes * add channels assert * add impl for odometry settings * add pimpl to odometry * linux warning fix * linux warning fix 1 * linux warning fix 2 * linux error fix * linux warning fix 3 * linux warning fix 4 * linux error fix 2 * fix test warnings * python build fix * doxygen fix * docs fix * change normal tests for 4channel point * all Normal tests pass * plane works * add warp frame body * minor fix * warning fixes * try to fix * try to fix 1 * review fix * lvls fix * createOdometryFrame fix * add comment * const reference * OPENCV_3D_ prefix * const methods * –OdometryFramePyramidType ifx * add assert * precomp moved upper * delete types_c * add assert for get and set functions * minor fixes * remove core.hpp from header * ocl_run add * warning fix * delete extra comment * minor fix * setDepth fix * delete underscore * odometry settings fix * show debug image fix * build error fix * other minor fix * add const to signatures * fix * conflict fix * getter fix
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;
}