opencv/modules/3d/test/test_normal.cpp

424 lines
14 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 "test_precomp.hpp"
#include <opencv2/3d.hpp>
namespace opencv_test { namespace {
#if 0
Point3f
rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_<float>& Kinv)
{
Matx33d dKinv(Kinv);
Vec3d dNormal(normal);
return rayPlaneIntersection(Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv);
}
#endif
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
Vec4f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec4d& normal, const Matx33d& Kinv)
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
{
Matx31d L = Kinv * uv1; //a ray passing through camera optical center
//and uv.
L = L * (1.0 / cv::norm(L));
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
double LdotNormal = L.dot(Vec3d(normal[0], normal[1], normal[2]));
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
double d;
if (std::fabs(LdotNormal) > 1e-9)
{
d = centroid_dot_normal / LdotNormal;
}
else
{
d = 1.0;
std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl;
std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl;
}
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
Vec4f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)), 0);
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 xyz;
}
const int W = 640;
const int H = 480;
//int window_size = 5;
float focal_length = 525;
float cx = W / 2.f + 0.5f;
float cy = H / 2.f + 0.5f;
Mat K = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1);
Mat Kinv = K.inv();
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
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap);
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
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
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::vector<Point3f> points3dvec;
for (int i = 0; i < H; i++)
for (int j = 0; j < W; j++)
points3dvec.push_back(Point3f(points3d(i, j)[0], points3d(i, j)[1], points3d(i, j)[2]));
std::vector<Point2f> img_points;
depthMap = Mat::zeros(H, W, CV_32F);
Vec3f R(0.0, 0.0, 0.0);
Vec3f T(0.0, 0.0, 0.0);
cv::projectPoints(points3dvec, R, T, K, Mat(), img_points);
int index = 0;
for (int i = 0; i < H; i++)
{
for (int j = 0; j < W; j++)
{
float value = (points3d.at<Vec3f>(i, j))[2]; // value is the z
depthMap.at<float>(cvRound(img_points[index].y), cvRound(img_points[index].x)) = value;
index++;
}
}
depthMap.convertTo(depthMap, CV_16U, 1000);
}
static RNG rng;
struct Plane
{
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
Vec4d n, p;
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
double p_dot_n;
Plane()
{
n[0] = rng.uniform(-0.5, 0.5);
n[1] = rng.uniform(-0.5, 0.5);
n[2] = -0.3; //rng.uniform(-1.f, 0.5f);
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
n[3] = 0.;
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
n = n / cv::norm(n);
set_d((float)rng.uniform(-2.0, 0.6));
}
void
set_d(float d)
{
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
p = Vec4d(0, 0, d / n[2], 0);
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
p_dot_n = p.dot(n);
}
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
Vec4f
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
intersection(float u, float v, const Matx33f& Kinv_in) const
{
return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
}
};
void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals,
int n_planes)
{
std::vector<Plane> planes;
for (int i = 0; i < n_planes; i++)
{
Plane px;
for (int j = 0; j < 1; j++)
{
px.set_d(rng.uniform(-3.f, -0.5f));
planes.push_back(px);
}
}
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
Mat_ < Vec4f > outp(H, W);
Mat_ < Vec4f > outn(H, W);
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
plane_mask.create(H, W);
// n ( r - r_0) = 0
// n * r_0 = d
//
// r_0 = (0,0,0)
// r[0]
for (int v = 0; v < H; v++)
{
for (int u = 0; u < W; u++)
{
unsigned int plane_index = (unsigned int)((u / float(W)) * planes.size());
Plane plane = planes[plane_index];
outp(v, u) = plane.intersection((float)u, (float)v, Kinv);
outn(v, u) = plane.n;
plane_mask(v, u) = (uchar)plane_index;
}
}
planes_out = planes;
points3d = outp;
normals = outn;
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class RgbdNormalsTest
{
public:
RgbdNormalsTest() { }
~RgbdNormalsTest() { }
void run()
{
Mat_<unsigned char> plane_mask;
for (unsigned char i = 0; i < 3; ++i)
{
RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RGBD_NORMALS_METHOD_FALS;;
// inner vector: whether it's 1 plane or 3 planes
// outer vector: float or double
std::vector<std::vector<float> > errors(2);
errors[0].resize(4);
errors[1].resize(4);
switch (i)
{
case 0:
method = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
CV_LOG_INFO(NULL, "*** FALS");
errors[0][0] = 0.006f;
errors[0][1] = 0.03f;
errors[1][0] = 0.0001f;
errors[1][1] = 0.02f;
break;
case 1:
method = RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD;
CV_LOG_INFO(NULL, "*** LINEMOD");
errors[0][0] = 0.04f;
errors[0][1] = 0.07f;
errors[0][2] = 0.04f; // depth 16U 1 plane
errors[0][3] = 0.07f; // depth 16U 3 planes
errors[1][0] = 0.05f;
errors[1][1] = 0.08f;
errors[1][2] = 0.05f; // depth 16U 1 plane
errors[1][3] = 0.08f; // depth 16U 3 planes
break;
case 2:
method = RgbdNormals::RGBD_NORMALS_METHOD_SRI;
CV_LOG_INFO(NULL, "*** SRI");
errors[0][0] = 0.02f;
errors[0][1] = 0.04f;
errors[1][0] = 0.02f;
errors[1][1] = 0.04f;
break;
}
for (unsigned char j = 0; j < 2; ++j)
{
int depth = (j % 2 == 0) ? CV_32F : CV_64F;
if (depth == CV_32F)
{
CV_LOG_INFO(NULL, " * float");
}
else
{
CV_LOG_INFO(NULL, " * double");
}
Ptr<RgbdNormals> normals_computer = RgbdNormals::create(H, W, depth, K, 5, method);
normals_computer->cache();
std::vector<Plane> plane_params;
Mat points3d, ground_normals;
// 1 plane, continuous scene, very low error..
CV_LOG_INFO(NULL, "1 plane - input 3d points");
float err_mean = 0;
for (int ii = 0; ii < 5; ++ii)
{
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1);
err_mean += testit(points3d, ground_normals, normals_computer);
}
CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5));
EXPECT_LE(err_mean / 5, errors[j][0]);
// 3 discontinuities, more error expected.
CV_LOG_INFO(NULL, "3 planes");
err_mean = 0;
for (int ii = 0; ii < 5; ++ii)
{
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3);
err_mean += testit(points3d, ground_normals, normals_computer);
}
CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5));
EXPECT_LE(err_mean / 5, errors[j][1]);
if (method == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
{
// depth 16U test
CV_LOG_INFO(NULL, "** depth 16U - 1 plane");
err_mean = 0;
for (int ii = 0; ii < 5; ++ii)
{
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1);
Mat depthMap;
points3dToDepth16U(points3d, depthMap);
err_mean += testit(depthMap, ground_normals, normals_computer);
}
CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5));
EXPECT_LE(err_mean / 5, errors[j][2]);
CV_LOG_INFO(NULL, "** depth 16U - 3 plane");
err_mean = 0;
for (int ii = 0; ii < 5; ++ii)
{
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3);
Mat depthMap;
points3dToDepth16U(points3d, depthMap);
err_mean += testit(depthMap, ground_normals, normals_computer);
}
CV_LOG_INFO(NULL, "mean diff: " << (err_mean / 5));
EXPECT_LE(err_mean / 5, errors[j][3]);
}
}
}
//TODO test NaNs in data
}
float testit(const Mat& points3d, const Mat& in_ground_normals, const Ptr<RgbdNormals>& normals_computer)
{
TickMeter tm;
tm.start();
Mat in_normals;
if (normals_computer->getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && points3d.channels() == 3)
{
std::vector<Mat> channels;
split(points3d, channels);
normals_computer->apply(channels[2], in_normals);
}
else
normals_computer->apply(points3d, in_normals);
tm.stop();
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
Mat_<Vec4f> normals, ground_normals;
in_normals.convertTo(normals, CV_32FC4);
in_ground_normals.convertTo(ground_normals, CV_32FC4);
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
float err = 0;
for (int y = 0; y < normals.rows; ++y)
for (int x = 0; x < normals.cols; ++x)
{
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
Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x);
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
vec1 = vec1 / cv::norm(vec1);
vec2 = vec2 / cv::norm(vec2);
float dot = vec1.dot(vec2);
// Just for rounding errors
if (std::abs(dot) < 1)
err += std::min(std::acos(dot), std::acos(-dot));
}
err /= normals.rows * normals.cols;
CV_LOG_INFO(NULL, "Average error: " << err << " Speed: " << tm.getTimeMilli() << " ms");
return err;
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class RgbdPlaneTest
{
public:
RgbdPlaneTest() { }
~RgbdPlaneTest() { }
void run()
{
std::vector<Plane> planes;
Mat points3d, ground_normals;
Mat_<unsigned char> plane_mask;
gen_points_3d(planes, plane_mask, points3d, ground_normals, 1);
testit(planes, plane_mask, points3d); // 1 plane, continuous scene, very low error..
for (int ii = 0; ii < 10; ii++)
{
gen_points_3d(planes, plane_mask, points3d, ground_normals, 3); //three planes
testit(planes, plane_mask, points3d); // 3 discontinuities, more error expected.
}
}
void testit(const std::vector<Plane>& gt_planes, const Mat& gt_plane_mask, const Mat& points3d)
{
for (char i_test = 0; i_test < 2; ++i_test)
{
TickMeter tm1, tm2;
Mat plane_mask;
std::vector<Vec4f> plane_coefficients;
if (i_test == 0)
{
tm1.start();
// First, get the normals
int depth = CV_32F;
Ptr<RgbdNormals> normals_computer = RgbdNormals::create(H, W, depth, K, 5, RgbdNormals::RGBD_NORMALS_METHOD_FALS);
Mat normals;
normals_computer->apply(points3d, normals);
tm1.stop();
tm2.start();
findPlanes(points3d, normals, plane_mask, plane_coefficients);
tm2.stop();
}
else
{
tm2.start();
findPlanes(points3d, noArray(), plane_mask, plane_coefficients);
tm2.stop();
}
// Compare each found plane to each ground truth plane
int n_planes = (int)plane_coefficients.size();
int n_gt_planes = (int)gt_planes.size();
Mat_<int> matching(n_gt_planes, n_planes);
for (int j = 0; j < n_gt_planes; ++j)
{
Mat gt_mask = gt_plane_mask == j;
int n_gt = countNonZero(gt_mask);
int n_max = 0, i_max = 0;
for (int i = 0; i < n_planes; ++i)
{
Mat dst;
bitwise_and(gt_mask, plane_mask == i, dst);
matching(j, i) = countNonZero(dst);
if (matching(j, i) > n_max)
{
n_max = matching(j, i);
i_max = i;
}
}
// Get the best match
ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001);
// Compare the normals
Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
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
Vec4d n = gt_planes[j].n;
ASSERT_GE(std::abs(Vec3d(n[0], n[1], n[2]).dot(normal)), 0.95);
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
}
CV_LOG_INFO(NULL, "Speed: ");
if (i_test == 0)
CV_LOG_INFO(NULL, "normals " << tm1.getTimeMilli() << " ms and ");
CV_LOG_INFO(NULL, "plane " << tm2.getTimeMilli() << " ms");
}
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(RGBD_Normals, compute)
{
RgbdNormalsTest test;
test.run();
}
TEST(RGBD_Plane, compute)
{
RgbdPlaneTest test;
test.run();
}
TEST(RGBD_Plane, regression_2309_valgrind_check)
{
Mat points(640, 480, CV_32FC3, Scalar::all(0));
// Note, 640%9 is 1 and 480%9 is 3
int blockSize = 9;
Mat mask;
std::vector<cv::Vec4f> planes;
// Will corrupt memory; valgrind gets triggered
findPlanes(points, noArray(), mask, planes, blockSize);
}
}} // namespace