mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-12-05 01:39:13 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #22241 from savuor:normals_scale_test

Browse Source 
More tests for normals
This commit is contained in:
Alexander Smorkalov 2022-08-24 10:25:55 +03:00 committed by GitHub
commit 335d7bd16e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 783 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
modules/3d/include/opencv2/3d/depth.hpp
View File

@ -30,7 +30,8 @@ public:
{
RGBD_NORMALS_METHOD_FALS = 0,
RGBD_NORMALS_METHOD_LINEMOD = 1,
RGBD_NORMALS_METHOD_SRI = 2
RGBD_NORMALS_METHOD_SRI = 2,
RGBD_NORMALS_METHOD_CROSS_PRODUCT = 3
};
RgbdNormals() { }
@ -42,9 +43,11 @@ public:
* @param depth the depth of the normals (only CV_32F or CV_64F)
* @param K the calibration matrix to use
* @param window_size the window size to compute the normals: can only be 1,3,5 or 7
* @param diff_threshold threshold in depth difference, used in LINEMOD algirithm
* @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
*/
CV_WRAP static Ptr<RgbdNormals> create(int rows = 0, int cols = 0, int depth = 0, InputArray K = noArray(), int window_size = 5,
float diff_threshold = 50.f,
RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RgbdNormalsMethod::RGBD_NORMALS_METHOD_FALS);
/** Given a set of 3d points in a depth image, compute the normals at each point.
@ -68,7 +71,6 @@ public:
CV_WRAP virtual void getK(OutputArray val) const = 0;
CV_WRAP virtual void setK(InputArray val) = 0;
CV_WRAP virtual RgbdNormals::RgbdNormalsMethod getMethod() const = 0;
CV_WRAP virtual void setMethod(RgbdNormals::RgbdNormalsMethod val) = 0;
};
@ -146,7 +148,7 @@ enum RgbdPlaneMethod
/** Find the planes in a depth image
* @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
* @param normals the normals for every point in the depth image
* @param normals the normals for every point in the depth image; optional, can be empty
* @param mask An image where each pixel is labeled with the plane it belongs to
* and 255 if it does not belong to any plane
* @param plane_coefficients the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1

2
modules/3d/src/rgbd/depth_to_3d.cpp
View File

@ -46,7 +46,7 @@ static void depthTo3d_from_uvz(const cv::Mat& in_K, const cv::Mat& u_mat, const
coordinates[0] = coordinates[0].mul(z_mat);
coordinates[1] = (v_mat - cy).mul(z_mat) * (1. / fy);
coordinates[2] = z_mat;
coordinates[3] = 0;
coordinates[3] = Mat(u_mat.size(), CV_32F, Scalar(0));
cv::merge(coordinates, points3d);
}

225
modules/3d/src/rgbd/normal.cpp
View File

@ -225,24 +225,7 @@ public:
{
return method;
}
virtual void setMethod(RgbdNormalsMethod val) CV_OVERRIDE
{
method = val; cacheIsDirty = true;
}
// Helper functions for apply()
virtual void assertOnBadArg(const Mat& points3d_ori) const = 0;
virtual void calcRadiusAnd3d(const Mat& points3d_ori, Mat& points3d, Mat& radius) const
{
// Make the points have the right depth
if (points3d_ori.depth() == dtype)
points3d = points3d_ori;
else
points3d_ori.convertTo(points3d, dtype);
// Compute the distance to the points
radius = computeRadius<T>(points3d);
}
virtual void compute(const Mat& in, Mat& normals) const = 0;
/** Given a set of 3d points in a depth image, compute the normals at each point
@ -256,29 +239,66 @@ public:
CV_Assert(points3d_ori.dims == 2);
// Either we have 3d points or a depth image
assertOnBadArg(points3d_ori);
bool ptsAre4F = (points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
bool ptsAreDepth = (points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
if (method == RGBD_NORMALS_METHOD_FALS || method == RGBD_NORMALS_METHOD_SRI || method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
{
if (!ptsAre4F)
{
CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels");
}
}
else if (method == RGBD_NORMALS_METHOD_LINEMOD)
{
if (!ptsAre4F && !ptsAreDepth)
{
CV_Error(Error::StsBadArg, "Input image should have 4 float-point channels or have 1 ushort or float-point channel");
}
}
else
{
CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
}
// Initialize the pimpl
cache();
// Precompute something for RGBD_NORMALS_METHOD_SRI and RGBD_NORMALS_METHOD_FALS
Mat points3d, radius;
calcRadiusAnd3d(points3d_ori, points3d, radius);
Mat points3d;
if (method != RGBD_NORMALS_METHOD_LINEMOD)
{
// Make the points have the right depth
if (points3d_ori.depth() == dtype)
points3d = points3d_ori;
else
points3d_ori.convertTo(points3d, dtype);
}
// Get the normals
normals_out.create(points3d_ori.size(), CV_MAKETYPE(dtype, 4));
if (points3d_in.empty())
if (points3d_ori.empty())
return;
Mat normals = normals_out.getMat();
if ((method == RGBD_NORMALS_METHOD_FALS) || (method == RGBD_NORMALS_METHOD_SRI))
{
// Compute the distance to the points
Mat radius = computeRadius<T>(points3d);
compute(radius, normals);
}
else // LINEMOD
else if (method == RGBD_NORMALS_METHOD_LINEMOD)
{
compute(points3d_ori, normals);
}
else if (method == RGBD_NORMALS_METHOD_CROSS_PRODUCT)
{
compute(points3d, normals);
}
else
{
CV_Error(Error::StsInternal, "Unknown normal computer algorithm");
}
}
int rows, cols;
@ -406,13 +426,6 @@ public:
}
}
virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
{
//CV_Assert(points3d_ori.channels() == 3);
CV_Assert(points3d_ori.channels() == 4);
CV_Assert(points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F);
}
// Cached data
mutable Mat_<Vec3T> V_;
mutable Mat_<Vec9T> M_inv_;
@ -448,14 +461,17 @@ public:
typedef Vec<T, 3> Vec3T;
typedef Matx<T, 3, 3> Mat33T;
LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K) :
RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
LINEMOD(int _rows, int _cols, int _windowSize, const Mat& _K, float _diffThr = 50.f) :
RgbdNormalsImpl<T>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD),
differenceThreshold(_diffThr)
{ }
/** Compute cached data
*/
virtual void cache() const CV_OVERRIDE
{ }
{
this->cacheIsDirty = false;
}
/** Compute the normals
* @param r
@ -536,7 +552,9 @@ public:
Vec3T X1_minus_X, X2_minus_X;
ContainerDepth difference_threshold = 50;
ContainerDepth difference_threshold(differenceThreshold);
//TODO: fixit, difference threshold should not depend on input type
difference_threshold *= (std::is_same<DepthDepth, ushort>::value ? 1000.f : 1.f);
normals.setTo(std::numeric_limits<DepthDepth>::quiet_NaN());
for (int y = r; y < this->rows - r - 1; ++y)
{
@ -591,14 +609,7 @@ public:
return normals;
}
virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
{
CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)) ||
((points3d_ori.channels() == 1) && (points3d_ori.depth() == CV_16U || points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
}
virtual void calcRadiusAnd3d(const Mat& /*points3d_ori*/, Mat& /*points3d*/, Mat& /*radius*/) const CV_OVERRIDE
{ }
float differenceThreshold;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@ -648,25 +659,28 @@ public:
for (int phi_int = 0, k = 0; phi_int < this->rows; ++phi_int)
{
float phi = min_phi + phi_int * phi_step_;
float phi_sin = std::sin(phi), phi_cos = std::cos(phi);
for (int theta_int = 0; theta_int < this->cols; ++theta_int, ++k)
{
float theta = min_theta + theta_int * theta_step_;
float theta_sin = std::sin(theta), theta_cos = std::cos(theta);
// Store the 3d point to project it later
points3d[k] = Point3f(std::sin(theta) * std::cos(phi), std::sin(phi), std::cos(theta) * std::cos(phi));
Point3f pp(theta_sin * phi_cos, phi_sin, theta_cos * phi_cos);
points3d[k] = pp;
// Cache the rotation matrix and negate it
Mat_<T> mat =
(Mat_ < T >(3, 3) << 0, 1, 0, 0, 0, 1, 1, 0, 0) *
((Mat_ < T >(3, 3) << std::cos(theta), -std::sin(theta), 0, std::sin(theta), std::cos(theta), 0, 0, 0, 1)) *
((Mat_ < T >(3, 3) << std::cos(phi), 0, -std::sin(phi), 0, 1, 0, std::sin(phi), 0, std::cos(phi)));
for (unsigned char i = 0; i < 3; ++i)
mat(i, 1) = mat(i, 1) / std::cos(phi);
// The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
mat(0, 0) = mat(0, 0) - 2 * std::cos(phi) * std::sin(theta);
mat(1, 0) = mat(1, 0) - 2 * std::sin(phi);
mat(2, 0) = mat(2, 0) - 2 * std::cos(phi) * std::cos(theta);
Matx<T, 3, 3> mat = Matx<T, 3, 3> (0, 1, 0, 0, 0, 1, 1, 0, 0) *
Matx<T, 3, 3> (theta_cos, -theta_sin, 0, theta_sin, theta_cos, 0, 0, 0, 1) *
Matx<T, 3, 3> (phi_cos, 0, -phi_sin, 0, 1, 0, phi_sin, 0, phi_cos);
R_hat_(phi_int, theta_int) = Vec9T((T*)(mat.data));
for (unsigned char i = 0; i < 3; ++i)
mat(i, 1) = mat(i, 1) / phi_cos;
// The second part of the matrix is never explained in the paper ... but look at the wikipedia normal article
mat(0, 0) = mat(0, 0) - 2 * pp.x;
mat(1, 0) = mat(1, 0) - 2 * pp.y;
mat(2, 0) = mat(2, 0) - 2 * pp.z;
R_hat_(phi_int, theta_int) = Vec9T(mat.val);
}
}
@ -747,8 +761,8 @@ public:
T r_phi_over_r = (*r_phi_ptr) / (*r_ptr);
// R(1,1) is 0
signNormal((*R)(0, 0) + (*R)(0, 1) * r_theta_over_r + (*R)(0, 2) * r_phi_over_r,
(*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
(*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
(*R)(1, 0) + (*R)(1, 2) * r_phi_over_r,
(*R)(2, 0) + (*R)(2, 1) * r_theta_over_r + (*R)(2, 2) * r_phi_over_r, *normal);
}
}
@ -759,11 +773,6 @@ public:
signNormal((*normal)[0], (*normal)[1], (*normal)[2], *normal);
}
virtual void assertOnBadArg(const Mat& points3d_ori) const CV_OVERRIDE
{
CV_Assert(((points3d_ori.channels() == 4) && (points3d_ori.depth() == CV_32F || points3d_ori.depth() == CV_64F)));
}
// Cached data
/** Stores R */
mutable Mat_<Vec9T> R_hat_;
@ -781,14 +790,91 @@ public:
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K, int windowSize, RgbdNormalsMethod method)
/* Uses the simpliest possible method for normals calculation: calculates cross product between two vectors
(pointAt(x+1, y) - pointAt(x, y)) and (pointAt(x, y+1) - pointAt(x, y)) */
template<typename DataType>
class CrossProduct : public RgbdNormalsImpl<DataType>
{
public:
typedef Vec<DataType, 3> Vec3T;
typedef Vec<DataType, 4> Vec4T;
typedef Point3_<DataType> Point3T;
CrossProduct(int _rows, int _cols, int _windowSize, const Mat& _K) :
RgbdNormalsImpl<DataType>(_rows, _cols, _windowSize, _K, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT)
{ }
/** Compute cached data
*/
virtual void cache() const CV_OVERRIDE
{
this->cacheIsDirty = false;
}
static inline Point3T fromVec(Vec4T v)
{
return {v[0], v[1], v[2]};
}
static inline Vec4T toVec4(Point3T p)
{
return {p.x, p.y, p.z, 0};
}
static inline bool haveNaNs(Point3T p)
{
return cvIsNaN(p.x) || cvIsNaN(p.y) || cvIsNaN(p.z);
}
/** Compute the normals
* @param points reprojected depth points
* @param normals generated normals
* @return
*/
virtual void compute(const Mat& points, Mat& normals) const CV_OVERRIDE
{
for(int y = 0; y < this->rows; y++)
{
const Vec4T* ptsRow0 = points.ptr<Vec4T>(y);
const Vec4T* ptsRow1 = (y < this->rows - 1) ? points.ptr<Vec4T>(y + 1) : nullptr;
Vec4T *normRow = normals.ptr<Vec4T>(y);
for (int x = 0; x < this->cols; x++)
{
Point3T v00 = fromVec(ptsRow0[x]);
const float qnan = std::numeric_limits<float>::quiet_NaN();
Point3T n(qnan, qnan, qnan);
if ((x < this->cols - 1) && (y < this->rows - 1) && !haveNaNs(v00))
{
Point3T v01 = fromVec(ptsRow0[x + 1]);
Point3T v10 = fromVec(ptsRow1[x]);
if (!haveNaNs(v01) && !haveNaNs(v10))
{
Vec3T vec = (v10 - v00).cross(v01 - v00);
n = normalize(vec);
}
}
normRow[x] = toVec4(n);
}
}
}
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K, int windowSize, float diffThreshold, RgbdNormalsMethod method)
{
CV_Assert(rows > 0 && cols > 0 && (depth == CV_32F || depth == CV_64F));
CV_Assert(windowSize == 1 || windowSize == 3 || windowSize == 5 || windowSize == 7);
CV_Assert(K.cols() == 3 && K.rows() == 3 && (K.depth() == CV_32F || K.depth() == CV_64F));
Mat mK = K.getMat();
CV_Assert(method == RGBD_NORMALS_METHOD_FALS || method == RGBD_NORMALS_METHOD_LINEMOD || method == RGBD_NORMALS_METHOD_SRI);
Ptr<RgbdNormals> ptr;
switch (method)
{
@ -802,10 +888,11 @@ Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K
}
case (RGBD_NORMALS_METHOD_LINEMOD):
{
CV_Assert(diffThreshold > 0);
if (depth == CV_32F)
ptr = makePtr<LINEMOD<float> >(rows, cols, windowSize, mK);
ptr = makePtr<LINEMOD<float> >(rows, cols, windowSize, mK, diffThreshold);
else
ptr = makePtr<LINEMOD<double>>(rows, cols, windowSize, mK);
ptr = makePtr<LINEMOD<double>>(rows, cols, windowSize, mK, diffThreshold);
break;
}
case RGBD_NORMALS_METHOD_SRI:
@ -816,6 +903,16 @@ Ptr<RgbdNormals> RgbdNormals::create(int rows, int cols, int depth, InputArray K
ptr = makePtr<SRI<double>>(rows, cols, windowSize, mK);
break;
}
case RGBD_NORMALS_METHOD_CROSS_PRODUCT:
{
if (depth == CV_32F)
ptr = makePtr<CrossProduct<float> >(rows, cols, windowSize, mK);
else
ptr = makePtr<CrossProduct<double>>(rows, cols, windowSize, mK);
break;
}
default:
CV_Error(Error::StsBadArg, "Unknown normals compute algorithm");
}
return ptr;

878
modules/3d/test/test_normal.cpp
View File

@ -4,40 +4,10 @@
#include "test_precomp.hpp"
#include <opencv2/3d.hpp>
#include <opencv2/core/quaternion.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
Vec4f rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec4d& normal, const Matx33d& Kinv)
{
Matx31d L = Kinv * uv1; //a ray passing through camera optical center
//and uv.
L = L * (1.0 / cv::norm(L));
double LdotNormal = L.dot(Vec3d(normal[0], normal[1], normal[2]));
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;
}
Vec4f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)), 0);
return xyz;
}
const int W = 640;
const int H = 480;
//int window_size = 5;
@ -63,61 +33,104 @@ void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
Vec3f T(0.0, 0.0, 0.0);
cv::projectPoints(points3dvec, R, T, K, Mat(), img_points);
float maxv = 0.f;
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
float value = (points3d(i, j))[2]; // value is the z
depthMap.at<float>(cvRound(img_points[index].y), cvRound(img_points[index].x)) = value;
maxv = std::max(maxv, value);
index++;
}
}
depthMap.convertTo(depthMap, CV_16U, 1000);
double scale = ((1 << 16) - 1) / maxv;
depthMap.convertTo(depthMap, CV_16U, scale);
}
static RNG rng;
struct Plane
{
Vec4d n, p;
double p_dot_n;
Plane()
public:
Vec4d nd;
Plane() : nd(1, 0, 0, 0) { }
static Plane generate(RNG& rng)
{
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);
n[3] = 0.;
n = n / cv::norm(n);
set_d((float)rng.uniform(-2.0, 0.6));
// Gaussian 3D distribution is separable and spherically symmetrical
// Being normalized, its points represent uniformly distributed points on a sphere (i.e. normal directions)
double sigma = 1.0;
Vec3d ngauss;
ngauss[0] = rng.gaussian(sigma);
ngauss[1] = rng.gaussian(sigma);
ngauss[2] = rng.gaussian(sigma);
ngauss = ngauss * (1.0 / cv::norm(ngauss));
double d = rng.uniform(-2.0, 2.0);
Plane p;
p.nd = Vec4d(ngauss[0], ngauss[1], ngauss[2], d);
return p;
}
void
set_d(float d)
Vec3d pixelIntersection(double u, double v, const Matx33d& K_inv)
{
p = Vec4d(0, 0, d / n[2], 0);
p_dot_n = p.dot(n);
}
Vec3d uv1(u, v, 1);
// pixel reprojected to camera space
Matx31d pspace = K_inv * uv1;
Vec4f
intersection(float u, float v, const Matx33f& Kinv_in) const
{
return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
double d = this->nd[3];
double dotp = pspace.ddot({this->nd[0], this->nd[1], this->nd[2]});
double d_over_dotp = d / dotp;
if (std::fabs(dotp) <= 1e-9)
{
d_over_dotp = 1.0;
CV_LOG_INFO(NULL, "warning, dotp nearly 0! " << dotp);
}
Matx31d pmeet = pspace * (- d_over_dotp);
return {pmeet(0, 0), pmeet(1, 0), pmeet(2, 0)};
}
};
void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals,
int n_planes)
int n_planes, float scale, RNG& rng)
{
const double minGoodZ = 0.0001;
const double maxGoodZ = 1000.0;
std::vector<Plane> planes;
for (int i = 0; i < n_planes; i++)
{
Plane px;
for (int j = 0; j < 1; j++)
bool found = false;
for (int j = 0; j < 100; j++)
{
px.set_d(rng.uniform(-3.f, -0.5f));
planes.push_back(px);
Plane px = Plane::generate(rng);
// Check that area corners have good z values
// So that they won't break rendering
double x0 = double(i) * double(W) / double(n_planes);
double x1 = double(i+1) * double(W) / double(n_planes);
std::vector<Point2d> corners = {{x0, 0}, {x0, H - 1}, {x1, 0}, {x1, H - 1}};
double minz = std::numeric_limits<double>::max();
double maxz = 0.0;
for (auto p : corners)
{
Vec3d v = px.pixelIntersection(p.x, p.y, Kinv);
minz = std::min(minz, v[2]);
maxz = std::max(maxz, v[2]);
}
if (minz > minGoodZ && maxz < maxGoodZ)
{
planes.push_back(px);
found = true;
break;
}
}
ASSERT_TRUE(found) << "Failed to generate proper random plane" << std::endl;
}
Mat_ < Vec4f > outp(H, W);
Mat_ < Vec4f > outn(H, W);
@ -134,8 +147,9 @@ void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_ma
{
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;
Vec3f pt = Vec3f(plane.pixelIntersection((double)u, (double)v, Kinv) * scale);
outp(v, u) = {pt[0], pt[1], pt[2], 0};
outn(v, u) = {(float)plane.nd[0], (float)plane.nd[1], (float)plane.nd[2], 0};
plane_mask(v, u) = (uchar)plane_index;
}
}
@ -146,269 +160,555 @@ void gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_ma
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class RgbdNormalsTest
CV_ENUM(NormalComputers, RgbdNormals::RGBD_NORMALS_METHOD_FALS,
RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD,
RgbdNormals::RGBD_NORMALS_METHOD_SRI,
RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT);
typedef std::tuple<MatDepth, NormalComputers, bool, double, double, double, double, double> NormalsTestData;
typedef std::tuple<NormalsTestData, int> NormalsTestParams;
const double threshold3d1d = 1e-12;
// Right angle is the maximum angle possible between two normals
const double hpi = CV_PI / 2.0;
const int nTestCasesNormals = 5;
class NormalsRandomPlanes : public ::testing::TestWithParam<NormalsTestParams>
{
public:
RgbdNormalsTest() { }
~RgbdNormalsTest() { }
void run()
protected:
void SetUp() override
{
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
p = GetParam();
depth = std::get<0>(std::get<0>(p));
alg = static_cast<RgbdNormals::RgbdNormalsMethod>(int(std::get<1>(std::get<0>(p))));
scale = std::get<2>(std::get<0>(p));
int idx = std::get<1>(p);
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;
}
rng = cvtest::TS::ptr()->get_rng();
rng.state += idx + nTestCasesNormals*int(scale) + alg*16 + depth*64;
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 diffThreshold = scale ? 100000.f : 50.f;
normalsComputer = RgbdNormals::create(H, W, depth, K, 5, diffThreshold, alg);
normalsComputer->cache();
}
float testit(const Mat& points3d, const Mat& in_ground_normals, const Ptr<RgbdNormals>& normals_computer)
struct NormalsCompareResult
{
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();
double meanErr;
double maxErr;
};
Mat_<Vec4f> normals, ground_normals;
in_normals.convertTo(normals, CV_32FC4);
in_ground_normals.convertTo(ground_normals, CV_32FC4);
float err = 0;
static NormalsCompareResult checkNormals(Mat_<Vec4f> normals, Mat_<Vec4f> ground_normals)
{
double meanErr = 0, maxErr = 0;
for (int y = 0; y < normals.rows; ++y)
{
for (int x = 0; x < normals.cols; ++x)
{
Vec4f vec1 = normals(y, x), vec2 = ground_normals(y, x);
vec1 = vec1 / cv::norm(vec1);
vec2 = vec2 / cv::norm(vec2);
float dot = vec1.dot(vec2);
double dot = vec1.ddot(vec2);
// Just for rounding errors
double err = std::abs(dot) < 1.0 ? std::min(std::acos(dot), std::acos(-dot)) : 0.0;
meanErr += err;
maxErr = std::max(maxErr, err);
}
}
meanErr /= normals.rows * normals.cols;
return { meanErr, maxErr };
}
void runCase(bool scaleUp, int nPlanes, bool makeDepth,
double meanThreshold, double maxThreshold, double threshold3d)
{
std::vector<Plane> plane_params;
Mat_<unsigned char> plane_mask;
Mat points3d, ground_normals;
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, nPlanes, scaleUp ? 5000.f : 1.f, rng);
Mat in;
if (makeDepth)
{
points3dToDepth16U(points3d, in);
}
else
{
in = points3d;
}
TickMeter tm;
tm.start();
Mat in_normals, normals3d;
//TODO: check other methods when 16U input is implemented for them
if (normalsComputer->getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && in.channels() == 3)
{
std::vector<Mat> channels;
split(in, channels);
normalsComputer->apply(channels[2], in_normals);
normalsComputer->apply(in, normals3d);
}
else
normalsComputer->apply(in, in_normals);
tm.stop();
CV_LOG_INFO(NULL, "Speed: " << tm.getTimeMilli() << " ms");
Mat_<Vec4f> normals;
in_normals.convertTo(normals, CV_32FC4);
NormalsCompareResult res = checkNormals(normals, ground_normals);
double err3d = 0.0;
if (!normals3d.empty())
{
Mat_<Vec4f> cvtNormals3d;
normals3d.convertTo(cvtNormals3d, CV_32FC4);
err3d = checkNormals(cvtNormals3d, ground_normals).maxErr;
}
EXPECT_LE(res.meanErr, meanThreshold);
EXPECT_LE(res.maxErr, maxThreshold);
EXPECT_LE(err3d, threshold3d);
}
NormalsTestParams p;
int depth;
RgbdNormals::RgbdNormalsMethod alg;
bool scale;
RNG rng;
Ptr<RgbdNormals> normalsComputer;
};
//TODO Test NaNs in data
TEST_P(NormalsRandomPlanes, check1plane)
{
double meanErr = std::get<3>(std::get<0>(p));
double maxErr = std::get<4>(std::get<0>(p));
// 1 plane, continuous scene, very low error..
runCase(scale, 1, false, meanErr, maxErr, threshold3d1d);
}
TEST_P(NormalsRandomPlanes, check3planes)
{
double meanErr = std::get<5>(std::get<0>(p));
double maxErr = hpi;
// 3 discontinuities, more error expected
runCase(scale, 3, false, meanErr, maxErr, threshold3d1d);
}
TEST_P(NormalsRandomPlanes, check1plane16u)
{
// TODO: check other algos as soon as they support 16U depth inputs
if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale)
{
double meanErr = std::get<6>(std::get<0>(p));
double maxErr = hpi;
runCase(false, 1, true, meanErr, maxErr, threshold3d1d);
}
else
{
throw SkipTestException("Not implemented for anything except LINEMOD with scale");
}
}
TEST_P(NormalsRandomPlanes, check3planes16u)
{
// TODO: check other algos as soon as they support 16U depth inputs
if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && scale)
{
double meanErr = std::get<7>(std::get<0>(p));
double maxErr = hpi;
runCase(false, 3, true, meanErr, maxErr, threshold3d1d);
}
else
{
throw SkipTestException("Not implemented for anything except LINEMOD with scale");
}
}
INSTANTIATE_TEST_CASE_P(RGBD_Normals, NormalsRandomPlanes,
::testing::Combine(::testing::Values(
// 3 normal computer params + 5 thresholds:
//depth, alg, scale, 1plane mean, 1plane max, 3planes mean, 1plane16u mean, 3planes16 mean
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, true, 0.00362, 0.08881, 0.02175, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00374, 0.10309, 0.01902, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, true, 0.00023, 0.00037, 0.01805, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_FALS, false, 0.00023, 0.00037, 0.01805, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, true, 0.00186, 0.08974, 0.04528, 0.21220, 0.17314},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00157, 0.01225, 0.04528, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, true, 0.00160, 0.06526, 0.04371, 0.28837, 0.28918},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, false, 0.00154, 0.06877, 0.04323, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, true, 0.01987, hpi, 0.03463, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01962, hpi, 0.03546, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, true, 0.01958, hpi, 0.03546, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01995, hpi, 0.03474, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, true, 0.000230, 0.00038, 0.00450, 0, 0},
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000230, 0.00038, 0.00478, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, true, 0.000221, 0.00038, 0.00469, 0, 0},
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, false, 0.000238, 0.00038, 0.00477, 0, 0}
), ::testing::Range(0, nTestCasesNormals)));
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
typedef std::tuple<NormalComputers, std::pair<double, double>> NormalComputerThresholds;
struct RenderedNormals: public ::testing::TestWithParam<std::tuple<MatDepth, NormalComputerThresholds, bool>>
{
static Mat readYaml(std::string fname)
{
Mat img;
FileStorage fs(fname, FileStorage::Mode::READ);
if (fs.isOpened() && fs.getFirstTopLevelNode().name() == "testImg")
{
fs["testImg"] >> img;
}
return img;
};
static Mat nanMask(Mat img)
{
int depth = img.depth();
Mat mask(img.size(), CV_8U);
for (int y = 0; y < img.rows; y++)
{
uchar* maskRow = mask.ptr<uchar>(y);
if (depth == CV_32F)
{
Vec3f *imgrow = img.ptr<Vec3f>(y);
for (int x = 0; x < img.cols; x++)
{
maskRow[x] = (imgrow[x] == imgrow[x])*255;
}
}
else if (depth == CV_64F)
{
Vec3d *imgrow = img.ptr<Vec3d>(y);
for (int x = 0; x < img.cols; x++)
{
maskRow[x] = (imgrow[x] == imgrow[x])*255;
}
}
}
return mask;
}
template<typename VT>
static Mat flipAxesT(Mat pts, int flip)
{
Mat flipped(pts.size(), pts.type());
for (int y = 0; y < pts.rows; y++)
{
VT *inrow = pts.ptr<VT>(y);
VT *outrow = flipped.ptr<VT>(y);
for (int x = 0; x < pts.cols; x++)
{
VT n = inrow[x];
n[0] = (flip & FLIP_X) ? -n[0] : n[0];
n[1] = (flip & FLIP_Y) ? -n[1] : n[1];
n[2] = (flip & FLIP_Z) ? -n[2] : n[2];
outrow[x] = n;
}
}
return flipped;
}
static const int FLIP_X = 1;
static const int FLIP_Y = 2;
static const int FLIP_Z = 4;
static Mat flipAxes(Mat pts, int flip)
{
int depth = pts.depth();
if (depth == CV_32F)
{
return flipAxesT<Vec3f>(pts, flip);
}
else if (depth == CV_64F)
{
return flipAxesT<Vec3d>(pts, flip);
}
else
{
return Mat();
}
}
template<typename VT>
static Mat_<typename VT::value_type> normalsErrorT(Mat_<VT> srcNormals, Mat_<VT> dstNormals)
{
typedef typename VT::value_type Val;
Mat out(srcNormals.size(), cv::traits::Depth<Val>::value, Scalar(0));
for (int y = 0; y < srcNormals.rows; y++)
{
VT *srcrow = srcNormals[y];
VT *dstrow = dstNormals[y];
Val *outrow = out.ptr<Val>(y);
for (int x = 0; x < srcNormals.cols; x++)
{
VT sn = srcrow[x];
VT dn = dstrow[x];
Val dot = sn.dot(dn);
Val v(0.0);
// Just for rounding errors
if (std::abs(dot) < 1)
err += std::min(std::acos(dot), std::acos(-dot));
}
v = 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;
outrow[x] = v;
}
}
return out;
}
static Mat normalsError(Mat srcNormals, Mat dstNormals)
{
int depth = srcNormals.depth();
int channels = srcNormals.channels();
if (depth == CV_32F)
{
if (channels == 3)
{
return normalsErrorT<Vec3f>(srcNormals, dstNormals);
}
else if (channels == 4)
{
return normalsErrorT<Vec4f>(srcNormals, dstNormals);
}
}
else if (depth == CV_64F)
{
if (channels == 3)
{
return normalsErrorT<Vec3d>(srcNormals, dstNormals);
}
else if (channels == 4)
{
return normalsErrorT<Vec4d>(srcNormals, dstNormals);
}
}
else
{
CV_Error(Error::StsInternal, "This type is unsupported");
}
return Mat();
}
};
TEST_P(RenderedNormals, check)
{
auto p = GetParam();
int depth = std::get<0>(p);
auto alg = static_cast<RgbdNormals::RgbdNormalsMethod>(int(std::get<0>(std::get<1>(p))));
bool scale = std::get<2>(p);
std::string dataPath = cvtest::TS::ptr()->get_data_path();
// The depth rendered from scene OPENCV_TEST_DATA_PATH + "/cv/rgbd/normals_check/normals_scene.blend"
std::string srcDepthFilename = dataPath + "/cv/rgbd/normals_check/depth.yaml.gz";
std::string srcNormalsFilename = dataPath + "/cv/rgbd/normals_check/normals%d.yaml.gz";
Mat srcDepth = readYaml(srcDepthFilename);
ASSERT_FALSE(srcDepth.empty()) << "Failed to load depth data";
Size depthSize = srcDepth.size();
Mat srcNormals;
std::array<Mat, 3> srcNormalsCh;
for (int i = 0; i < 3; i++)
{
Mat m = readYaml(cv::format(srcNormalsFilename.c_str(), i));
ASSERT_FALSE(m.empty()) << "Failed to load normals data";
if (depth == CV_64F)
{
Mat c;
m.convertTo(c, CV_64F);
m = c;
}
srcNormalsCh[i] = m;
}
cv::merge(srcNormalsCh, srcNormals);
// Convert saved normals from [0; 1] range to [-1; 1]
srcNormals = srcNormals * 2.0 - 1.0;
// Data obtained from Blender scene
Matx33f intr(666.6667f, 0.f, 320.f,
0.f, 666.6667f, 240.f,
0.f, 0.f, 1.f);
// Inverted camera rotation
Matx33d rotm = cv::Quatd(0.7805, 0.4835, 0.2087, 0.3369).conjugate().toRotMat3x3();
cv::transform(srcNormals, srcNormals, rotm);
Mat srcMask = srcDepth > 0;
float diffThreshold = 50.f;
if (scale)
{
srcDepth = srcDepth * 5000.0;
diffThreshold = 100000.f;
}
Mat srcCloud;
// The function with mask produces 1x(w*h) vector, this is not what we need
// depthTo3d(srcDepth, intr, srcCloud, srcMask);
depthTo3d(srcDepth, intr, srcCloud);
Scalar qnan = Scalar::all(std::numeric_limits<double>::quiet_NaN());
srcCloud.setTo(qnan, ~srcMask);
srcDepth.setTo(qnan, ~srcMask);
// For further result comparison
srcNormals.setTo(qnan, ~srcMask);
Ptr<RgbdNormals> normalsComputer = RgbdNormals::create(depthSize.height, depthSize.width, depth, intr, 5, diffThreshold, alg);
normalsComputer->cache();
Mat dstNormals, dstNormalsOrig, dstNormalsDepth;
normalsComputer->apply(srcCloud, dstNormals);
//TODO: add for other methods too when it's implemented
if (alg == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
{
normalsComputer->apply(srcDepth, dstNormalsDepth);
dstNormalsOrig = dstNormals.clone();
}
// Remove 4th channel from dstNormals
Mat newDstNormals;
std::vector<Mat> dstNormalsCh;
split(dstNormals, dstNormalsCh);
dstNormalsCh.resize(3);
merge(dstNormalsCh, newDstNormals);
dstNormals = newDstNormals;
Mat dstMask = nanMask(dstNormals);
// div by 8 because uchar is 8-bit
double maskl2 = cv::norm(dstMask, srcMask, NORM_HAMMING) / 8;
// Flipping Y and Z to correspond to srcNormals
Mat flipped = flipAxes(dstNormals, FLIP_Y | FLIP_Z);
dstNormals = flipped;
Mat absdot = normalsError(srcNormals, dstNormals);
Mat cmpMask = srcMask & dstMask;
EXPECT_GT(countNonZero(cmpMask), 0);
double nrml2 = cv::norm(absdot, NORM_L2, cmpMask);
if (!dstNormalsDepth.empty())
{
Mat abs3d = normalsError(dstNormalsOrig, dstNormalsDepth);
double errInf = cv::norm(abs3d, NORM_INF, cmpMask);
double errL2 = cv::norm(abs3d, NORM_L2, cmpMask);
EXPECT_LE(errInf, 0.00085);
EXPECT_LE(errL2, 0.07718);
}
auto th = std::get<1>(std::get<1>(p));
EXPECT_LE(nrml2, th.first);
EXPECT_LE(maskl2, th.second);
}
INSTANTIATE_TEST_CASE_P(RGBD_Normals, RenderedNormals, ::testing::Combine(::testing::Values(CV_32F, CV_64F),
::testing::Values(
NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_FALS, { 81.8210, 0}},
NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD, { 107.2710, 29168}},
NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_SRI, { 73.2027, 17693}},
NormalComputerThresholds { RgbdNormals::RGBD_NORMALS_METHOD_CROSS_PRODUCT, { 57.9832, 2531}}),
::testing::Values(true, false)));
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
class RgbdPlaneTest
class RgbdPlaneGenerate : public ::testing::TestWithParam<std::tuple<int, bool, int>>
{
public:
RgbdPlaneTest() { }
~RgbdPlaneTest() { }
void run()
protected:
void SetUp() override
{
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.
}
auto p = GetParam();
idx = std::get<0>(p);
checkNormals = std::get<1>(p);
nPlanes = std::get<2>(p);
}
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]);
Vec4d n = gt_planes[j].n;
ASSERT_GE(std::abs(Vec3d(n[0], n[1], n[2]).dot(normal)), 0.95);
}
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");
}
}
int idx;
bool checkNormals;
int nPlanes;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(RGBD_Normals, compute)
TEST_P(RgbdPlaneGenerate, compute)
{
RgbdNormalsTest test;
test.run();
RNG &rng = cvtest::TS::ptr()->get_rng();
rng.state += idx;
std::vector<Plane> planes;
Mat points3d, ground_normals;
Mat_<unsigned char> gt_plane_mask;
gen_points_3d(planes, gt_plane_mask, points3d, ground_normals, nPlanes, 1.f, rng);
Mat plane_mask;
std::vector<Vec4f> plane_coefficients;
Mat normals;
if (checkNormals)
{
// First, get the normals
int depth = CV_32F;
Ptr<RgbdNormals> normalsComputer = RgbdNormals::create(H, W, depth, K, 5, 50.f, RgbdNormals::RGBD_NORMALS_METHOD_FALS);
normalsComputer->apply(points3d, normals);
}
findPlanes(points3d, normals, plane_mask, plane_coefficients);
// Compare each found plane to each ground truth plane
int n_planes = (int)plane_coefficients.size();
int n_gt_planes = (int)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]);
Vec4d nd = planes[j].nd;
ASSERT_GE(std::abs(Vec3d(nd[0], nd[1], nd[2]).dot(normal)), 0.95);
}
}
TEST(RGBD_Plane, compute)
{
RgbdPlaneTest test;
test.run();
}
// 1 plane, continuous scene, very low error
// 3 planes, 3 discontinuities, more error expected
INSTANTIATE_TEST_CASE_P(RGBD_Plane, RgbdPlaneGenerate, ::testing::Combine(::testing::Range(0, 10),
::testing::Values(false, true),
::testing::Values(1, 3)));
TEST(RGBD_Plane, regression_2309_valgrind_check)
TEST(RGBD_Plane, regression2309ValgrindCheck)
{
Mat points(640, 480, CV_32FC3, Scalar::all(0));
// Note, 640%9 is 1 and 480%9 is 3

27
modules/3d/test/test_odometry.cpp
View File

@ -627,4 +627,31 @@ TEST(RGBD_Odometry_WarpFrame, bigScale)
ASSERT_LE(lidiff, 0.99951172);
}
TEST(RGBD_DepthTo3D, mask)
{
std::string dataPath = cvtest::TS::ptr()->get_data_path();
std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png";
Mat srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED);
ASSERT_FALSE(srcDepth.empty()) << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl;
ASSERT_TRUE(srcDepth.type() == CV_16UC1);
Mat srcMask = srcDepth > 0;
// test data used to generate warped depth and rgb
// the script used to generate is in opencv_extra repo
// at testdata/cv/rgbd/warped_depth_generator/warp_test.py
double fx = 525.0, fy = 525.0,
cx = 319.5, cy = 239.5;
Matx33d intr(fx, 0, cx,
0, fy, cy,
0, 0, 1);
Mat srcCloud;
depthTo3d(srcDepth, intr, srcCloud, srcMask);
size_t npts = countNonZero(srcMask);
ASSERT_EQ(npts, srcCloud.total());
}
}} // namespace