diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index fd504a784..dbc5fc4b8 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -84,4 +85,19 @@ namespace temp_viz inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); } template inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } + + Point3d operator*(const Affine3f& affine, const Point3d& point); + + inline bool isNaN( float x ) + { + union { float f; unsigned int u; } v = { x }; + return ((v.u & 0x7f800000) == 0x7f800000) && (v.u & 0x007fffff); + } + + inline bool isNaN( double x ) + { + union { double d; unsigned int u[2]; } v = { x }; + return (v.u[1] & 0x7ff00000) == 0x7ff00000 && + (v.u[0] != 0 || (v.u[1] & 0x000fffff) != 0); + } } diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp index 0baed3a72..9586f6bc7 100644 --- a/modules/viz/src/types.cpp +++ b/modules/viz/src/types.cpp @@ -22,3 +22,12 @@ temp_viz::Color temp_viz::Color::white() { return Color(255, 255, 255); } temp_viz::Color temp_viz::Color::gray() { return Color(128, 128, 128); } +temp_viz::Point3d temp_viz::operator*(const temp_viz::Affine3f& affine, const temp_viz::Point3d& point) +{ + const temp_viz::Matx44f& m = affine.matrix; + temp_viz::Point3d result; + result.x = m.val[0] * point.x + m.val[1] * point.y + m.val[ 2] * point.z + m.val[ 3]; + result.y = m.val[4] * point.x + m.val[5] * point.y + m.val[ 6] * point.z + m.val[ 7]; + result.z = m.val[8] * point.x + m.val[9] * point.y + m.val[10] * point.z + m.val[11]; + return result; +} \ No newline at end of file