mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 23:33:05 +04:00
depthTo3d: fixed bug, added regression test
RgbdNormals: setMethod() removed as useless RgbdNormals: tests + cross product, to be fixed + cross product LINEMOD: diffThreshold param added + tests fixed minor diffThreshold fix points3dToDepth16U fix normals computer diffThreshold fix random plane generation fixed + diffThreshold fix Rendered normals test rewritten to GTest Params random plane generation: scale RGBD_Normals tests: thresholds tuned Rendered normals tests: 64F support added Random planes normal tests rewritten to GTest Params LINEMOD and CrossProduct fix SRI threshold raised NormalsRandomPlanes: thresholds raised assert on unknown alg; minor fix frame size reduced TIFF replaced by YAML.GZ depthTo3d test changed cv::transform is used fix warning nanMask() flipAxes() absDotPixel() + forgotten code helper functions removed RGBDNormals: checkNormals() and compare LINEMOD's pts3d to depth input Rendered: another criteria; thresholds; LINEMOD's pts3d to depth input comparison thresholds raised a bit SRI slightly optimized assert change normal tests refactored, parametrized, split trailing namespace, thresholds raised SRI caching optimized a lot normal tests rewritten to fixture, no loop minor runCase() joined with testIt() thresholds were put into GTest params ternary operator RgbdNormalsTest merged into NormalsRandomPlanes; RgbdPlanes moved closer to tests normal test minor refactoring plane finder tests refactored to GTest Params skip tests thresholds raised plane test minor plane tests: timers dropped, nPlanes put into GTest Params; refactoring generated normals tests: minor refactoring flipAxes() templated rendered normals tests refactored: thresholds to GTest Params CV_Error -> ASSERT_FALSE
This commit is contained in:
Rostislav Vasilikhin
+587
-289
@@ -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,553 @@ 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;
|
||||
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user