mirror of
https://github.com/opencv/opencv.git
synced 2026-07-30 15:53:03 +04:00
Merge pull request #29224 from asmorkalov:as/ptcloud2
Dedicated pointcloud module #29224 OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134 ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [ ] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Smorkalov
committed by
GitHub
GitHub
parent
6a1a2754c8
commit
fc3803c67b
@@ -0,0 +1,10 @@
|
||||
// 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"
|
||||
|
||||
#if defined(HAVE_HPX)
|
||||
#include <hpx/hpx_main.hpp>
|
||||
#endif
|
||||
|
||||
CV_TEST_MAIN("")
|
||||
@@ -0,0 +1,725 @@
|
||||
// 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/geometry.hpp>
|
||||
#include <opencv2/core/quaternion.hpp>
|
||||
|
||||
using namespace cv;
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
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;
|
||||
|
||||
static Mat K() { static Mat res = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1); return res; }
|
||||
static Mat Kinv() { static Mat res = K().inv(); return res; }
|
||||
|
||||
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap);
|
||||
|
||||
void points3dToDepth16U(const Mat_<Vec4f>& points3d, Mat& depthMap)
|
||||
{
|
||||
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);
|
||||
|
||||
float maxv = 0.f;
|
||||
int index = 0;
|
||||
for (int i = 0; i < H; i++)
|
||||
{
|
||||
|
||||
for (int j = 0; j < W; j++)
|
||||
{
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
double scale = ((1 << 16) - 1) / maxv;
|
||||
depthMap.convertTo(depthMap, CV_16U, scale);
|
||||
}
|
||||
|
||||
|
||||
struct Plane
|
||||
{
|
||||
public:
|
||||
Vec4d nd;
|
||||
|
||||
Plane() : nd(1, 0, 0, 0) { }
|
||||
|
||||
static Plane generate(RNG& rng)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
|
||||
Vec3d pixelIntersection(double u, double v, const Matx33d& K_inv)
|
||||
{
|
||||
Vec3d uv1(u, v, 1);
|
||||
// pixel reprojected to camera space
|
||||
Matx31d pspace = K_inv * uv1;
|
||||
|
||||
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, 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++)
|
||||
{
|
||||
bool found = false;
|
||||
for (int j = 0; j < 100; j++)
|
||||
{
|
||||
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);
|
||||
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];
|
||||
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;
|
||||
}
|
||||
}
|
||||
planes_out = planes;
|
||||
points3d = outp;
|
||||
normals = outn;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
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>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
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));
|
||||
idx = std::get<1>(p);
|
||||
|
||||
float diffThreshold = scale ? 100000.f : 50.f;
|
||||
normalsComputer = RgbdNormals::create(H, W, depth, K(), 5, diffThreshold, alg);
|
||||
normalsComputer->cache();
|
||||
}
|
||||
|
||||
struct NormalsCompareResult
|
||||
{
|
||||
double meanErr;
|
||||
double maxErr;
|
||||
};
|
||||
|
||||
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);
|
||||
|
||||
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)
|
||||
{
|
||||
RNG& rng = cv::theRNG();
|
||||
rng.state += idx + nTestCasesNormals*int(scale) + alg*16 + depth*64;
|
||||
|
||||
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;
|
||||
int idx;
|
||||
|
||||
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.02, 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.036, 0, 0},
|
||||
NormalsTestData {CV_32F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01962, hpi, 0.037, 0, 0},
|
||||
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, true, 0.01958, hpi, 0.037, 0, 0},
|
||||
NormalsTestData {CV_64F, RgbdNormals::RGBD_NORMALS_METHOD_SRI, false, 0.01995, hpi, 0.036, 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)
|
||||
v = std::min(std::acos(dot), std::acos(-dot));
|
||||
|
||||
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.8213, 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 RgbdPlaneGenerate : public ::testing::TestWithParam<std::tuple<int, bool, int>>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
auto p = GetParam();
|
||||
idx = std::get<0>(p);
|
||||
checkNormals = std::get<1>(p);
|
||||
nPlanes = std::get<2>(p);
|
||||
}
|
||||
|
||||
int idx;
|
||||
bool checkNormals;
|
||||
int nPlanes;
|
||||
};
|
||||
|
||||
TEST_P(RgbdPlaneGenerate, compute)
|
||||
{
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// 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, regression2309ValgrindCheck)
|
||||
{
|
||||
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
|
||||
@@ -0,0 +1,158 @@
|
||||
// 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"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
using namespace cv;
|
||||
|
||||
class OctreeTest: public testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
pointCloudSize = 1000;
|
||||
resolution = 0.0001;
|
||||
int scale;
|
||||
Point3i pmin, pmax;
|
||||
scale = 1 << 20;
|
||||
pmin = Point3i(-scale, -scale, -scale);
|
||||
pmax = Point3i(scale, scale, scale);
|
||||
|
||||
RNG& rng_Point = theRNG(); // set random seed for fixing output 3D point.
|
||||
|
||||
// Generate 3D PointCloud
|
||||
for(size_t i = 0; i < pointCloudSize; i++)
|
||||
{
|
||||
float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
|
||||
float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
|
||||
float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
|
||||
pointCloud.push_back(Point3f(_x, _y, _z));
|
||||
}
|
||||
|
||||
// Generate Octree From PointCloud
|
||||
treeTest = Octree::createWithResolution(resolution, pointCloud);
|
||||
|
||||
// Randomly generate another 3D point.
|
||||
float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
|
||||
float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
|
||||
float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
|
||||
restPoint = Point3f(_x, _y, _z);
|
||||
}
|
||||
|
||||
public:
|
||||
//Origin point cloud data
|
||||
std::vector<Point3f> pointCloud;
|
||||
|
||||
//Point cloud data from octree
|
||||
std::vector<Point3f> restorePointCloudData;
|
||||
|
||||
//Color attribute of pointCloud from octree
|
||||
std::vector<Point3f> restorePointCloudColor;
|
||||
|
||||
size_t pointCloudSize;
|
||||
Point3f restPoint;
|
||||
Ptr<Octree> treeTest;
|
||||
double resolution;
|
||||
};
|
||||
|
||||
TEST_F(OctreeTest, BasicFunctionTest)
|
||||
{
|
||||
// Check if the point in Bound.
|
||||
EXPECT_TRUE(treeTest->isPointInBound(restPoint));
|
||||
EXPECT_FALSE(treeTest->isPointInBound(restPoint + Point3f(60, 60, 60)));
|
||||
|
||||
// insert, delete Test.
|
||||
EXPECT_FALSE(treeTest->deletePoint(restPoint));
|
||||
EXPECT_FALSE(treeTest->insertPoint(restPoint + Point3f(60, 60, 60)));
|
||||
EXPECT_TRUE(treeTest->insertPoint(restPoint));
|
||||
EXPECT_TRUE(treeTest->deletePoint(restPoint));
|
||||
|
||||
EXPECT_FALSE(treeTest->empty());
|
||||
EXPECT_NO_THROW(treeTest->clear());
|
||||
EXPECT_TRUE(treeTest->empty());
|
||||
}
|
||||
|
||||
TEST_F(OctreeTest, RadiusSearchTest)
|
||||
{
|
||||
float radius = 2.0f;
|
||||
std::vector<Point3f> outputPoints;
|
||||
std::vector<float> outputSquareDist;
|
||||
EXPECT_NO_THROW(treeTest->radiusNNSearch(restPoint, radius, outputPoints, outputSquareDist));
|
||||
EXPECT_EQ(outputPoints.size(),(unsigned int)5);
|
||||
|
||||
// The output is unsorted, so let's sort it before checking
|
||||
std::map<float, Point3f> sortResults;
|
||||
for (int i = 0; i < (int)outputPoints.size(); i++)
|
||||
{
|
||||
sortResults[outputSquareDist[i]] = outputPoints[i];
|
||||
}
|
||||
|
||||
std::vector<Point3f> goldVals = {
|
||||
{-8.1184864044189f, -0.528564453125f, 0.f},
|
||||
{-8.405818939208f, -2.991247177124f, 0.f},
|
||||
{-8.88461112976f, -1.881799697875f, 0.f},
|
||||
{-6.551313400268f, -0.708484649658f, 0.f}
|
||||
};
|
||||
|
||||
auto it = sortResults.begin();
|
||||
for (int i = 0; i < (int)goldVals.size(); i++, it++)
|
||||
{
|
||||
Point3f p = it->second;
|
||||
EXPECT_FLOAT_EQ(goldVals[i].x, p.x);
|
||||
EXPECT_FLOAT_EQ(goldVals[i].y, p.y);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(OctreeTest, KNNSearchTest)
|
||||
{
|
||||
int K = 10;
|
||||
std::vector<Point3f> outputPoints;
|
||||
std::vector<float> outputSquareDist;
|
||||
EXPECT_NO_THROW(treeTest->KNNSearch(restPoint, K, outputPoints, outputSquareDist));
|
||||
|
||||
// The output is unsorted, so let's sort it before checking
|
||||
std::map<float, Point3f> sortResults;
|
||||
for (int i = 0; i < (int)outputPoints.size(); i++)
|
||||
{
|
||||
sortResults[outputSquareDist[i]] = outputPoints[i];
|
||||
}
|
||||
|
||||
std::vector<Point3f> goldVals = {
|
||||
{ -8.118486404418f, -0.528564453125f, 0.f },
|
||||
{ -8.405818939208f, -2.991247177124f, 0.f },
|
||||
{ -8.884611129760f, -1.881799697875f, 0.f },
|
||||
{ -6.551313400268f, -0.708484649658f, 0.f }
|
||||
};
|
||||
|
||||
auto it = sortResults.begin();
|
||||
for (int i = 0; i < (int)goldVals.size(); i++, it++)
|
||||
{
|
||||
Point3f p = it->second;
|
||||
EXPECT_FLOAT_EQ(goldVals[i].x, p.x);
|
||||
EXPECT_FLOAT_EQ(goldVals[i].y, p.y);
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(OctreeTest, restoreTest) {
|
||||
//restore the pointCloud data from octree.
|
||||
EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor));
|
||||
|
||||
//The points in same leaf node will be seen as the same point. So if the resolution is small,
|
||||
//it will work as a downSampling function.
|
||||
EXPECT_LE(restorePointCloudData.size(),pointCloudSize);
|
||||
|
||||
//The distance between the restore point cloud data and origin data should less than resolution.
|
||||
std::vector<Point3f> outputPoints;
|
||||
std::vector<float> outputSquareDist;
|
||||
EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor));
|
||||
EXPECT_NO_THROW(treeTest->KNNSearch(restorePointCloudData[0], 1, outputPoints, outputSquareDist));
|
||||
EXPECT_LE(abs(outputPoints[0].x - restorePointCloudData[0].x), resolution);
|
||||
EXPECT_LE(abs(outputPoints[0].y - restorePointCloudData[0].y), resolution);
|
||||
EXPECT_LE(abs(outputPoints[0].z - restorePointCloudData[0].z), resolution);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // opencv_test
|
||||
@@ -0,0 +1,833 @@
|
||||
// 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"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
static
|
||||
void dilateFrame(Mat& image, Mat& depth)
|
||||
{
|
||||
CV_Assert(!image.empty());
|
||||
CV_Assert(image.type() == CV_8UC1);
|
||||
|
||||
CV_Assert(!depth.empty());
|
||||
CV_Assert(depth.type() == CV_32FC1);
|
||||
CV_Assert(depth.size() == image.size());
|
||||
|
||||
Mat mask;
|
||||
inRange(depth, FLT_EPSILON, 10, mask);
|
||||
|
||||
image.setTo(255, ~mask);
|
||||
Mat minImage;
|
||||
erode(image, minImage, Mat());
|
||||
|
||||
image.setTo(0, ~mask);
|
||||
Mat maxImage;
|
||||
dilate(image, maxImage, Mat());
|
||||
|
||||
depth.setTo(FLT_MAX, ~mask);
|
||||
Mat minDepth;
|
||||
erode(depth, minDepth, Mat());
|
||||
|
||||
depth.setTo(0, ~mask);
|
||||
Mat maxDepth;
|
||||
dilate(depth, maxDepth, Mat());
|
||||
|
||||
Mat dilatedMask;
|
||||
dilate(mask, dilatedMask, Mat(), Point(-1,-1), 1);
|
||||
for(int y = 0; y < depth.rows; y++)
|
||||
for(int x = 0; x < depth.cols; x++)
|
||||
if(!mask.at<uchar>(y,x) && dilatedMask.at<uchar>(y,x))
|
||||
{
|
||||
image.at<uchar>(y,x) = static_cast<uchar>(0.5f * (static_cast<float>(minImage.at<uchar>(y,x)) +
|
||||
static_cast<float>(maxImage.at<uchar>(y,x))));
|
||||
depth.at<float>(y,x) = 0.5f * (minDepth.at<float>(y,x) + maxDepth.at<float>(y,x));
|
||||
}
|
||||
}
|
||||
|
||||
class OdometryTest
|
||||
{
|
||||
public:
|
||||
OdometryTest(OdometryType _otype,
|
||||
OdometryAlgoType _algtype,
|
||||
double _maxError1,
|
||||
double _maxError5,
|
||||
double _idError = DBL_EPSILON) :
|
||||
otype(_otype),
|
||||
algtype(_algtype),
|
||||
maxError1(_maxError1),
|
||||
maxError5(_maxError5),
|
||||
idError(_idError)
|
||||
{ }
|
||||
|
||||
void readData(Mat& image, Mat& depth) const;
|
||||
static Mat getCameraMatrix()
|
||||
{
|
||||
float fx = 525.0f, // default
|
||||
fy = 525.0f,
|
||||
cx = 319.5f,
|
||||
cy = 239.5f;
|
||||
Matx33f K(fx, 0, cx,
|
||||
0, fy, cy,
|
||||
0, 0, 1);
|
||||
return Mat(K);
|
||||
}
|
||||
static void generateRandomTransformation(Mat& R, Mat& t);
|
||||
|
||||
void run();
|
||||
void checkUMats();
|
||||
void prepareFrameCheck();
|
||||
void processedDepthCheck();
|
||||
|
||||
OdometryType otype;
|
||||
OdometryAlgoType algtype;
|
||||
double maxError1;
|
||||
double maxError5;
|
||||
double idError;
|
||||
};
|
||||
|
||||
|
||||
void OdometryTest::readData(Mat& image, Mat& depth) const
|
||||
{
|
||||
std::string dataPath = cvtest::TS::ptr()->get_data_path();
|
||||
std::string imageFilename = dataPath + "/cv/rgbd/rgb.png";
|
||||
std::string depthFilename = dataPath + "/cv/rgbd/depth.png";
|
||||
|
||||
image = imread(imageFilename, 0);
|
||||
depth = imread(depthFilename, -1);
|
||||
|
||||
ASSERT_FALSE(image.empty()) << "Image " << imageFilename.c_str() << " can not be read" << std::endl;
|
||||
ASSERT_FALSE(depth.empty()) << "Depth " << depthFilename.c_str() << "can not be read" << std::endl;
|
||||
|
||||
CV_DbgAssert(image.type() == CV_8UC1);
|
||||
CV_DbgAssert(depth.type() == CV_16UC1);
|
||||
{
|
||||
Mat depth_flt;
|
||||
depth.convertTo(depth_flt, CV_32FC1, 1.f/5000.f);
|
||||
depth_flt.setTo(std::numeric_limits<float>::quiet_NaN(), depth_flt < FLT_EPSILON);
|
||||
depth = depth_flt;
|
||||
}
|
||||
}
|
||||
|
||||
void OdometryTest::generateRandomTransformation(Mat& rvec, Mat& tvec)
|
||||
{
|
||||
const float maxRotation = (float)(3.f / 180.f * CV_PI); //rad
|
||||
const float maxTranslation = 0.02f; //m
|
||||
|
||||
RNG& rng = theRNG();
|
||||
rvec.create(3, 1, CV_64FC1);
|
||||
tvec.create(3, 1, CV_64FC1);
|
||||
|
||||
randu(rvec, Scalar(-1000), Scalar(1000));
|
||||
normalize(rvec, rvec, rng.uniform(0.007f, maxRotation));
|
||||
|
||||
randu(tvec, Scalar(-1000), Scalar(1000));
|
||||
normalize(tvec, tvec, rng.uniform(0.008f, maxTranslation));
|
||||
}
|
||||
|
||||
void OdometryTest::checkUMats()
|
||||
{
|
||||
Mat K = getCameraMatrix();
|
||||
|
||||
Mat image, depth;
|
||||
readData(image, depth);
|
||||
|
||||
UMat uimage, udepth;
|
||||
image.copyTo(uimage);
|
||||
depth.copyTo(udepth);
|
||||
|
||||
OdometrySettings ods;
|
||||
ods.setCameraMatrix(K);
|
||||
Odometry odometry = Odometry(otype, ods, algtype);
|
||||
OdometryFrame odf(udepth, uimage);
|
||||
|
||||
Mat calcRt;
|
||||
|
||||
uimage.release();
|
||||
udepth.release();
|
||||
|
||||
odometry.prepareFrame(odf);
|
||||
bool isComputed = odometry.compute(odf, odf, calcRt);
|
||||
ASSERT_TRUE(isComputed);
|
||||
double diff = cv::norm(calcRt, Mat::eye(4, 4, CV_64FC1));
|
||||
ASSERT_LE(diff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl;
|
||||
}
|
||||
|
||||
void OdometryTest::run()
|
||||
{
|
||||
Mat K = getCameraMatrix();
|
||||
|
||||
Mat image, depth;
|
||||
readData(image, depth);
|
||||
OdometrySettings ods;
|
||||
ods.setCameraMatrix(K);
|
||||
Odometry odometry = Odometry(otype, ods, algtype);
|
||||
OdometryFrame odf(depth, image);
|
||||
Mat calcRt;
|
||||
|
||||
// 1. Try to find Rt between the same frame (try masks also).
|
||||
Mat mask(image.size(), CV_8UC1, Scalar(255));
|
||||
|
||||
odometry.prepareFrame(odf);
|
||||
bool isComputed = odometry.compute(odf, odf, calcRt);
|
||||
|
||||
ASSERT_TRUE(isComputed) << "Can not find Rt between the same frame" << std::endl;
|
||||
double ndiff = cv::norm(calcRt, Mat::eye(4,4,CV_64FC1));
|
||||
ASSERT_LE(ndiff, idError) << "Incorrect transformation between the same frame (not the identity matrix)" << std::endl;
|
||||
|
||||
// 2. Generate random rigid body motion in some ranges several times (iterCount).
|
||||
// On each iteration an input frame is warped using generated transformation.
|
||||
// Odometry is run on the following pair: the original frame and the warped one.
|
||||
// Comparing a computed transformation with an applied one we compute 2 errors:
|
||||
// better_1time_count - count of poses which error is less than ground truth pose,
|
||||
// better_5times_count - count of poses which error is 5 times less than ground truth pose.
|
||||
int iterCount = 100;
|
||||
int better_1time_count = 0;
|
||||
int better_5times_count = 0;
|
||||
for (int iter = 0; iter < iterCount; iter++)
|
||||
{
|
||||
Mat rvec, tvec;
|
||||
generateRandomTransformation(rvec, tvec);
|
||||
Affine3d rt(rvec, tvec);
|
||||
|
||||
Mat warpedImage, warpedDepth;
|
||||
|
||||
warpFrame(depth, image, noArray(), rt.matrix, K, warpedDepth, warpedImage);
|
||||
dilateFrame(warpedImage, warpedDepth); // due to inaccuracy after warping
|
||||
|
||||
OdometryFrame odfSrc(depth, image);
|
||||
OdometryFrame odfDst(warpedDepth, warpedImage);
|
||||
|
||||
odometry.prepareFrames(odfSrc, odfDst);
|
||||
isComputed = odometry.compute(odfSrc, odfDst, calcRt);
|
||||
|
||||
if (!isComputed)
|
||||
{
|
||||
CV_LOG_INFO(NULL, "Iter " << iter << "; Odometry compute returned false");
|
||||
continue;
|
||||
}
|
||||
Mat calcR = calcRt(Rect(0, 0, 3, 3)), calcRvec;
|
||||
cv::Rodrigues(calcR, calcRvec);
|
||||
calcRvec = calcRvec.reshape(rvec.channels(), rvec.rows);
|
||||
Mat calcTvec = calcRt(Rect(3,0,1,3));
|
||||
|
||||
if (cvtest::debugLevel >= 10)
|
||||
{
|
||||
imshow("image", image);
|
||||
imshow("warpedImage", warpedImage);
|
||||
Mat resultImage, resultDepth;
|
||||
|
||||
warpFrame(depth, image, noArray(), calcRt, K, resultDepth, resultImage);
|
||||
imshow("resultImage", resultImage);
|
||||
waitKey(100);
|
||||
}
|
||||
|
||||
// compare rotation
|
||||
double possibleError = algtype == OdometryAlgoType::COMMON ? 0.02f : 0.02f;
|
||||
|
||||
Affine3f src = Affine3f(Vec3f(rvec), Vec3f(tvec));
|
||||
Affine3f res = Affine3f(Vec3f(calcRvec), Vec3f(calcTvec));
|
||||
Affine3f src_inv = src.inv();
|
||||
Affine3f diff = res * src_inv;
|
||||
double rdiffnorm = cv::norm(diff.rvec());
|
||||
double tdiffnorm = cv::norm(diff.translation());
|
||||
|
||||
if (rdiffnorm < possibleError && tdiffnorm < possibleError)
|
||||
better_1time_count++;
|
||||
if (5. * rdiffnorm < possibleError && 5 * tdiffnorm < possibleError)
|
||||
better_5times_count++;
|
||||
|
||||
CV_LOG_INFO(NULL, "Iter " << iter);
|
||||
CV_LOG_INFO(NULL, "rdiff: " << Vec3f(diff.rvec()) << "; rdiffnorm: " << rdiffnorm);
|
||||
CV_LOG_INFO(NULL, "tdiff: " << Vec3f(diff.translation()) << "; tdiffnorm: " << tdiffnorm);
|
||||
|
||||
CV_LOG_INFO(NULL, "better_1time_count " << better_1time_count << "; better_5time_count " << better_5times_count);
|
||||
}
|
||||
|
||||
if(static_cast<double>(better_1time_count) < maxError1 * static_cast<double>(iterCount))
|
||||
{
|
||||
FAIL() << "Incorrect count of accurate poses [1st case]: "
|
||||
<< static_cast<double>(better_1time_count) << " / "
|
||||
<< maxError1 * static_cast<double>(iterCount) << std::endl;
|
||||
}
|
||||
|
||||
if(static_cast<double>(better_5times_count) < maxError5 * static_cast<double>(iterCount))
|
||||
{
|
||||
FAIL() << "Incorrect count of accurate poses [2nd case]: "
|
||||
<< static_cast<double>(better_5times_count) << " / "
|
||||
<< maxError5 * static_cast<double>(iterCount) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void OdometryTest::prepareFrameCheck()
|
||||
{
|
||||
Mat K = getCameraMatrix();
|
||||
|
||||
Mat gtImage, gtDepth;
|
||||
readData(gtImage, gtDepth);
|
||||
OdometrySettings ods;
|
||||
ods.setCameraMatrix(K);
|
||||
Odometry odometry = Odometry(otype, ods, algtype);
|
||||
OdometryFrame odf(gtDepth, gtImage);
|
||||
|
||||
odometry.prepareFrame(odf);
|
||||
|
||||
std::vector<int> iters;
|
||||
ods.getIterCounts(iters);
|
||||
size_t nlevels = iters.size();
|
||||
|
||||
Mat points, mask, depth, gray, rgb, scaled;
|
||||
|
||||
odf.getMask(mask);
|
||||
int masknz = countNonZero(mask);
|
||||
ASSERT_GT(masknz, 0);
|
||||
|
||||
odf.getDepth(depth);
|
||||
Mat patchedDepth = depth.clone();
|
||||
patchNaNs(patchedDepth, 0);
|
||||
int depthnz = countNonZero(patchedDepth);
|
||||
double depthNorm = cv::norm(depth, gtDepth, NORM_INF, mask);
|
||||
ASSERT_LE(depthNorm, 0.0);
|
||||
|
||||
Mat gtGray;
|
||||
if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH)
|
||||
{
|
||||
odf.getGrayImage(gray);
|
||||
odf.getImage(rgb);
|
||||
double rgbNorm = cv::norm(rgb, gtImage);
|
||||
ASSERT_LE(rgbNorm, 0.0);
|
||||
|
||||
if (gtImage.channels() == 3)
|
||||
cvtColor(gtImage, gtGray, COLOR_BGR2GRAY);
|
||||
else
|
||||
gtGray = gtImage;
|
||||
gtGray.convertTo(gtGray, CV_8U);
|
||||
double grayNorm = cv::norm(gray, gtGray);
|
||||
ASSERT_LE(grayNorm, 0.0);
|
||||
}
|
||||
|
||||
odf.getProcessedDepth(scaled);
|
||||
int scalednz = countNonZero(scaled);
|
||||
EXPECT_EQ(scalednz, depthnz);
|
||||
|
||||
std::vector<Mat> gtPyrDepth, gtPyrMask;
|
||||
//TODO: this depth calculation would become incorrect when we implement bilateral filtering, fixit
|
||||
buildPyramid(gtDepth, gtPyrDepth, (int)nlevels - 1);
|
||||
for (const auto& gd : gtPyrDepth)
|
||||
{
|
||||
Mat pm = (gd > ods.getMinDepth()) & (gd < ods.getMaxDepth());
|
||||
gtPyrMask.push_back(pm);
|
||||
}
|
||||
|
||||
size_t npyr = odf.getPyramidLevels();
|
||||
ASSERT_EQ(npyr, nlevels);
|
||||
Matx33f levelK = K;
|
||||
for (size_t i = 0; i < nlevels; i++)
|
||||
{
|
||||
Mat depthi, cloudi, maski;
|
||||
|
||||
odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i);
|
||||
ASSERT_FALSE(maski.empty());
|
||||
double mnorm = cv::norm(maski, gtPyrMask[i]);
|
||||
EXPECT_LE(mnorm, 16 * 255.0) << "Mask diff is too big at pyr level " << i;
|
||||
|
||||
odf.getPyramidAt(depthi, OdometryFramePyramidType::PYR_DEPTH, i);
|
||||
ASSERT_FALSE(depthi.empty());
|
||||
double dnorm = cv::norm(depthi, gtPyrDepth[i], NORM_INF, maski);
|
||||
EXPECT_LE(dnorm, 8.e-7) << "Depth diff norm is too big at pyr level " << i;
|
||||
|
||||
odf.getPyramidAt(cloudi, OdometryFramePyramidType::PYR_CLOUD, i);
|
||||
ASSERT_FALSE(cloudi.empty());
|
||||
Mat gtCloud;
|
||||
depthTo3d(depthi, levelK, gtCloud);
|
||||
double cnorm = cv::norm(cloudi, gtCloud, NORM_INF, maski);
|
||||
EXPECT_LE(cnorm, 0.0) << "Cloud diff norm is too big at pyr level " << i;
|
||||
// downscale camera matrix for next pyramid level
|
||||
levelK = 0.5f * levelK;
|
||||
levelK(2, 2) = 1.f;
|
||||
}
|
||||
|
||||
if (otype == OdometryType::RGB || otype == OdometryType::RGB_DEPTH)
|
||||
{
|
||||
std::vector<Mat> gtPyrImage;
|
||||
buildPyramid(gtGray, gtPyrImage, (int)nlevels - 1);
|
||||
|
||||
for (size_t i = 0; i < nlevels; i++)
|
||||
{
|
||||
Mat rgbi, texi, dixi, diyi, maski;
|
||||
odf.getPyramidAt(maski, OdometryFramePyramidType::PYR_MASK, i);
|
||||
odf.getPyramidAt(rgbi, OdometryFramePyramidType::PYR_IMAGE, i);
|
||||
ASSERT_FALSE(rgbi.empty());
|
||||
double rnorm = cv::norm(rgbi, gtPyrImage[i], NORM_INF);
|
||||
EXPECT_LE(rnorm, 1.0) << "RGB diff is too big at pyr level " << i;
|
||||
odf.getPyramidAt(texi, OdometryFramePyramidType::PYR_TEXMASK, i);
|
||||
ASSERT_FALSE(texi.empty());
|
||||
int tnz = countNonZero(texi);
|
||||
EXPECT_GE(tnz, 1000) << "Texture mask has too few valid pixels at pyr level " << i;
|
||||
Mat gtDixi, gtDiyi;
|
||||
Sobel(rgbi, gtDixi, CV_16S, 1, 0, ods.getSobelSize());
|
||||
odf.getPyramidAt(dixi, OdometryFramePyramidType::PYR_DIX, i);
|
||||
ASSERT_FALSE(dixi.empty());
|
||||
double dixnorm = cv::norm(dixi, gtDixi, NORM_INF, maski);
|
||||
EXPECT_LE(dixnorm, 0) << "dI/dx diff is too big at pyr level " << i;
|
||||
Sobel(rgbi, gtDiyi, CV_16S, 0, 1, ods.getSobelSize());
|
||||
odf.getPyramidAt(diyi, OdometryFramePyramidType::PYR_DIY, i);
|
||||
ASSERT_FALSE(diyi.empty());
|
||||
double diynorm = cv::norm(diyi, gtDiyi, NORM_INF, maski);
|
||||
EXPECT_LE(diynorm, 0) << "dI/dy diff is too big at pyr level " << i;
|
||||
}
|
||||
}
|
||||
|
||||
if (otype == OdometryType::DEPTH || otype == OdometryType::RGB_DEPTH)
|
||||
{
|
||||
Ptr<RgbdNormals> normalComputer = odometry.getNormalsComputer();
|
||||
ASSERT_FALSE(normalComputer.empty());
|
||||
Mat normals;
|
||||
odf.getNormals(normals);
|
||||
std::vector<Mat> gtPyrNormals;
|
||||
buildPyramid(normals, gtPyrNormals, (int)nlevels - 1);
|
||||
for (size_t i = 0; i < nlevels; i++)
|
||||
{
|
||||
Mat gtNormal = gtPyrNormals[i];
|
||||
CV_Assert(gtNormal.type() == CV_32FC4);
|
||||
for (int y = 0; y < gtNormal.rows; y++)
|
||||
{
|
||||
Vec4f *normals_row = gtNormal.ptr<Vec4f>(y);
|
||||
for (int x = 0; x < gtNormal.cols; x++)
|
||||
{
|
||||
Vec4f n4 = normals_row[x];
|
||||
Point3f n(n4[0], n4[1], n4[2]);
|
||||
double nrm = cv::norm(n);
|
||||
n *= 1.f / nrm;
|
||||
normals_row[x] = Vec4f(n.x, n.y, n.z, 0);
|
||||
}
|
||||
}
|
||||
|
||||
Mat normmaski;
|
||||
odf.getPyramidAt(normmaski, OdometryFramePyramidType::PYR_NORMMASK, i);
|
||||
ASSERT_FALSE(normmaski.empty());
|
||||
int nnm = countNonZero(normmaski);
|
||||
EXPECT_GE(nnm, 1000) << "Normals mask has too few valid pixels at pyr level " << i;
|
||||
|
||||
Mat ptsi;
|
||||
odf.getPyramidAt(ptsi, OdometryFramePyramidType::PYR_CLOUD, i);
|
||||
|
||||
Mat normi;
|
||||
odf.getPyramidAt(normi, OdometryFramePyramidType::PYR_NORM, i);
|
||||
ASSERT_FALSE(normi.empty());
|
||||
double nnorm = cv::norm(normi, gtNormal, NORM_INF, normmaski);
|
||||
EXPECT_LE(nnorm, 3.3e-7) << "Normals diff is too big at pyr level " << i;
|
||||
|
||||
if (i == 0)
|
||||
{
|
||||
double pnnorm = cv::norm(normals, normi, NORM_INF, normmaski);
|
||||
EXPECT_GE(pnnorm, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void OdometryTest::processedDepthCheck()
|
||||
{
|
||||
Mat K = getCameraMatrix();
|
||||
|
||||
Mat gtImage, gtDepth;
|
||||
readData(gtImage, gtDepth);
|
||||
|
||||
gtDepth *= 5000.0;
|
||||
|
||||
OdometrySettings ods;
|
||||
ods.setCameraMatrix(K);
|
||||
Odometry odometry = Odometry(otype, ods, algtype);
|
||||
OdometryFrame odf(gtDepth, gtImage);
|
||||
|
||||
odometry.prepareFrame(odf);
|
||||
|
||||
Mat scaled;
|
||||
odf.getProcessedDepth(scaled);
|
||||
|
||||
//TODO: remove this check when depth rescaling is removed
|
||||
double pmax;
|
||||
cv::minMaxLoc(scaled, nullptr, &pmax);
|
||||
EXPECT_LT(pmax, 10.0);
|
||||
}
|
||||
|
||||
/****************************************************************************************\
|
||||
* Tests registrations *
|
||||
\****************************************************************************************/
|
||||
|
||||
TEST(RGBD_Odometry_Rgb, algorithmic)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.run();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_ICP, algorithmic)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.run();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_RgbdICP, algorithmic)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.run();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_FastICP, algorithmic)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.87, 1.84e-5);
|
||||
test.run();
|
||||
}
|
||||
|
||||
|
||||
TEST(RGBD_Odometry_Rgb, UMats)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.checkUMats();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_ICP, UMats)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.checkUMats();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_RgbdICP, UMats)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.checkUMats();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_FastICP, UMats)
|
||||
{
|
||||
// OpenCL version has slightly less accuracy than CPU version
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, 1.84e-5);
|
||||
test.checkUMats();
|
||||
}
|
||||
|
||||
|
||||
TEST(RGBD_Odometry_Rgb, prepareFrame)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.prepareFrameCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_ICP, prepareFrame)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.prepareFrameCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_RgbdICP, prepareFrame)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.prepareFrameCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_FastICP, prepareFrame)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON);
|
||||
test.prepareFrameCheck();
|
||||
}
|
||||
|
||||
|
||||
TEST(RGBD_Odometry_Rgb, processedDepth)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.processedDepthCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_ICP, processedDepth)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.processedDepthCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_RgbdICP, processedDepth)
|
||||
{
|
||||
OdometryTest test(OdometryType::RGB_DEPTH, OdometryAlgoType::COMMON, 0.99, 0.99);
|
||||
test.processedDepthCheck();
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_FastICP, processedDepth)
|
||||
{
|
||||
OdometryTest test(OdometryType::DEPTH, OdometryAlgoType::FAST, 0.99, 0.99, FLT_EPSILON);
|
||||
test.processedDepthCheck();
|
||||
}
|
||||
|
||||
|
||||
struct WarpFrameTest
|
||||
{
|
||||
WarpFrameTest() :
|
||||
srcDepth(), srcRgb(), srcMask(),
|
||||
dstDepth(), dstRgb(), dstMask(),
|
||||
warpedDepth(), warpedRgb(), warpedMask()
|
||||
{}
|
||||
|
||||
void run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int imageType);
|
||||
|
||||
Mat srcDepth, srcRgb, srcMask;
|
||||
Mat dstDepth, dstRgb, dstMask;
|
||||
Mat warpedDepth, warpedRgb, warpedMask;
|
||||
};
|
||||
|
||||
void WarpFrameTest::run(bool needRgb, bool scaleDown, bool checkMask, bool identityTransform, int depthType, int rgbType)
|
||||
{
|
||||
std::string dataPath = cvtest::TS::ptr()->get_data_path();
|
||||
std::string srcDepthFilename = dataPath + "/cv/rgbd/depth.png";
|
||||
std::string srcRgbFilename = dataPath + "/cv/rgbd/rgb.png";
|
||||
// The depth was generated using the script at testdata/cv/rgbd/warped_depth_generator/warp_test.py
|
||||
std::string warpedDepthFilename = dataPath + "/cv/rgbd/warpedDepth.png";
|
||||
std::string warpedRgbFilename = dataPath + "/cv/rgbd/warpedRgb.png";
|
||||
|
||||
srcDepth = imread(srcDepthFilename, IMREAD_UNCHANGED);
|
||||
ASSERT_FALSE(srcDepth.empty()) << "Depth " << srcDepthFilename.c_str() << "can not be read" << std::endl;
|
||||
|
||||
if (identityTransform)
|
||||
{
|
||||
warpedDepth = srcDepth;
|
||||
}
|
||||
else
|
||||
{
|
||||
warpedDepth = imread(warpedDepthFilename, IMREAD_UNCHANGED);
|
||||
ASSERT_FALSE(warpedDepth.empty()) << "Depth " << warpedDepthFilename.c_str() << "can not be read" << std::endl;
|
||||
}
|
||||
|
||||
ASSERT_TRUE(srcDepth.type() == CV_16UC1);
|
||||
ASSERT_TRUE(warpedDepth.type() == CV_16UC1);
|
||||
|
||||
Mat epsSrc = srcDepth > 0, epsWarped = warpedDepth > 0;
|
||||
|
||||
const double depthFactor = 5000.0;
|
||||
// scale float types only
|
||||
double depthScaleCoeff = scaleDown ? ( depthType == CV_16U ? 1. : 1./depthFactor ) : 1.;
|
||||
double transScaleCoeff = scaleDown ? ( depthType == CV_16U ? depthFactor : 1. ) : depthFactor;
|
||||
|
||||
Mat srcDepthCvt, warpedDepthCvt;
|
||||
srcDepth.convertTo(srcDepthCvt, depthType, depthScaleCoeff);
|
||||
srcDepth = srcDepthCvt;
|
||||
warpedDepth.convertTo(warpedDepthCvt, depthType, depthScaleCoeff);
|
||||
warpedDepth = warpedDepthCvt;
|
||||
|
||||
Scalar badVal;
|
||||
switch (depthType)
|
||||
{
|
||||
case CV_16U:
|
||||
badVal = 0;
|
||||
break;
|
||||
case CV_32F:
|
||||
badVal = std::numeric_limits<float>::quiet_NaN();
|
||||
break;
|
||||
case CV_64F:
|
||||
badVal = std::numeric_limits<double>::quiet_NaN();
|
||||
break;
|
||||
default:
|
||||
CV_Error(Error::StsBadArg, "Unsupported depth data type");
|
||||
}
|
||||
|
||||
srcDepth.setTo(badVal, ~epsSrc);
|
||||
warpedDepth.setTo(badVal, ~epsWarped);
|
||||
|
||||
if (checkMask)
|
||||
{
|
||||
srcMask = epsSrc; warpedMask = epsWarped;
|
||||
}
|
||||
else
|
||||
{
|
||||
srcMask = Mat(); warpedMask = Mat();
|
||||
}
|
||||
|
||||
if (needRgb)
|
||||
{
|
||||
srcRgb = imread(srcRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR);
|
||||
ASSERT_FALSE(srcRgb.empty()) << "Image " << srcRgbFilename.c_str() << "can not be read" << std::endl;
|
||||
|
||||
if (identityTransform)
|
||||
{
|
||||
srcRgb.copyTo(warpedRgb, epsSrc);
|
||||
}
|
||||
else
|
||||
{
|
||||
warpedRgb = imread(warpedRgbFilename, rgbType == CV_8UC1 ? IMREAD_GRAYSCALE : IMREAD_COLOR);
|
||||
ASSERT_FALSE (warpedRgb.empty()) << "Image " << warpedRgbFilename.c_str() << "can not be read" << std::endl;
|
||||
}
|
||||
|
||||
if (rgbType == CV_8UC4)
|
||||
{
|
||||
Mat newSrcRgb, newWarpedRgb;
|
||||
cvtColor(srcRgb, newSrcRgb, COLOR_RGB2RGBA);
|
||||
srcRgb = newSrcRgb;
|
||||
// let's keep alpha channel
|
||||
std::vector<Mat> warpedRgbChannels;
|
||||
split(warpedRgb, warpedRgbChannels);
|
||||
warpedRgbChannels.push_back(epsWarped);
|
||||
merge(warpedRgbChannels, newWarpedRgb);
|
||||
warpedRgb = newWarpedRgb;
|
||||
}
|
||||
|
||||
ASSERT_TRUE(srcRgb.type() == rgbType);
|
||||
ASSERT_TRUE(warpedRgb.type() == rgbType);
|
||||
}
|
||||
else
|
||||
{
|
||||
srcRgb = Mat(); warpedRgb = Mat();
|
||||
}
|
||||
|
||||
// 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 K(fx, 0, cx,
|
||||
0, fy, cy,
|
||||
0, 0, 1);
|
||||
cv::Affine3d rt;
|
||||
cv::Vec3d tr(-0.04, 0.05, 0.6);
|
||||
rt = identityTransform ? cv::Affine3d() : cv::Affine3d(cv::Vec3d(0.1, 0.2, 0.3), tr * transScaleCoeff);
|
||||
|
||||
warpFrame(srcDepth, srcRgb, srcMask, rt.matrix, K, dstDepth, dstRgb, dstMask);
|
||||
}
|
||||
|
||||
typedef std::pair<int, int> WarpFrameInputTypes;
|
||||
typedef testing::TestWithParam<WarpFrameInputTypes> WarpFrameInputs;
|
||||
|
||||
TEST_P(WarpFrameInputs, checkTypes)
|
||||
{
|
||||
const double shortl2diff = 233.983;
|
||||
const double shortlidiff = 1;
|
||||
const double floatl2diff = 0.038209;
|
||||
const double floatlidiff = 0.00020004;
|
||||
|
||||
int depthType = GetParam().first;
|
||||
int rgbType = GetParam().second;
|
||||
|
||||
WarpFrameTest w;
|
||||
// scale down does not happen on CV_16U
|
||||
// to avoid integer overflow
|
||||
w.run(/* needRgb */ true, /* scaleDown*/ true,
|
||||
/* checkMask */ true, /* identityTransform */ false, depthType, rgbType);
|
||||
|
||||
double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
|
||||
double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
|
||||
|
||||
EXPECT_EQ(0, maskDiff);
|
||||
EXPECT_EQ(0, rgbDiff);
|
||||
|
||||
double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
|
||||
double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
|
||||
|
||||
double l2threshold = depthType == CV_16U ? shortl2diff : floatl2diff;
|
||||
double lithreshold = depthType == CV_16U ? shortlidiff : floatlidiff;
|
||||
|
||||
EXPECT_LE(l2diff, l2threshold);
|
||||
EXPECT_LE(lidiff, lithreshold);
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(RGBD_Odometry, WarpFrameInputs, ::testing::Values(
|
||||
WarpFrameInputTypes { CV_16U, CV_8UC3 },
|
||||
WarpFrameInputTypes { CV_32F, CV_8UC3 },
|
||||
WarpFrameInputTypes { CV_64F, CV_8UC3 },
|
||||
WarpFrameInputTypes { CV_32F, CV_8UC1 },
|
||||
WarpFrameInputTypes { CV_32F, CV_8UC4 }));
|
||||
|
||||
|
||||
TEST(RGBD_Odometry_WarpFrame, identity)
|
||||
{
|
||||
WarpFrameTest w;
|
||||
w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ true, CV_32F, CV_8UC3);
|
||||
|
||||
double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
|
||||
double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
|
||||
|
||||
ASSERT_EQ(0, rgbDiff);
|
||||
ASSERT_EQ(0, maskDiff);
|
||||
|
||||
double depthDiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.dstMask);
|
||||
|
||||
ASSERT_LE(depthDiff, DBL_EPSILON);
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_WarpFrame, noRgb)
|
||||
{
|
||||
WarpFrameTest w;
|
||||
w.run(/* needRgb */ false, /* scaleDown*/ true, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3);
|
||||
|
||||
double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
|
||||
ASSERT_EQ(0, maskDiff);
|
||||
|
||||
double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
|
||||
double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
|
||||
|
||||
ASSERT_LE(l2diff, 0.038209);
|
||||
ASSERT_LE(lidiff, 0.00020004);
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_WarpFrame, nansAreMasked)
|
||||
{
|
||||
WarpFrameTest w;
|
||||
w.run(/* needRgb */ true, /* scaleDown*/ true, /* checkMask */ false, /* identityTransform */ false, CV_32F, CV_8UC3);
|
||||
|
||||
double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
|
||||
|
||||
ASSERT_EQ(0, rgbDiff);
|
||||
|
||||
Mat goodVals;
|
||||
finiteMask(w.warpedDepth, goodVals);
|
||||
|
||||
double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, goodVals);
|
||||
double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, goodVals);
|
||||
|
||||
ASSERT_LE(l2diff, 0.038209);
|
||||
ASSERT_LE(lidiff, 0.00020004);
|
||||
}
|
||||
|
||||
TEST(RGBD_Odometry_WarpFrame, bigScale)
|
||||
{
|
||||
WarpFrameTest w;
|
||||
w.run(/* needRgb */ true, /* scaleDown*/ false, /* checkMask */ true, /* identityTransform */ false, CV_32F, CV_8UC3);
|
||||
|
||||
double rgbDiff = cv::norm(w.dstRgb, w.warpedRgb, NORM_L2);
|
||||
double maskDiff = cv::norm(w.dstMask, w.warpedMask, NORM_L2);
|
||||
|
||||
ASSERT_EQ(0, maskDiff);
|
||||
ASSERT_EQ(0, rgbDiff);
|
||||
|
||||
double l2diff = cv::norm(w.dstDepth, w.warpedDepth, NORM_L2, w.warpedMask);
|
||||
double lidiff = cv::norm(w.dstDepth, w.warpedDepth, NORM_INF, w.warpedMask);
|
||||
|
||||
ASSERT_LE(l2diff, 191.026565);
|
||||
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
|
||||
@@ -0,0 +1,299 @@
|
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <vector>
|
||||
#include <cstdio>
|
||||
|
||||
#include "test_precomp.hpp"
|
||||
#include "opencv2/ts.hpp"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
struct OriginalObjGoldValues
|
||||
{
|
||||
OriginalObjGoldValues()
|
||||
{
|
||||
std::array<float, 6> vals = { -5.93915f, -0.13257f, 2.55837f, 1.86743f, -1.16339f, 0.399941f };
|
||||
points =
|
||||
{
|
||||
{ vals[0], vals[1], vals[2] },
|
||||
{ vals[0], vals[3], vals[2] },
|
||||
{ vals[0], vals[1], vals[4] },
|
||||
{ vals[0], vals[3], vals[4] },
|
||||
{ vals[5], vals[1], vals[2] },
|
||||
{ vals[5], vals[3], vals[2] },
|
||||
{ vals[5], vals[1], vals[4] },
|
||||
{ vals[5], vals[3], vals[4] },
|
||||
};
|
||||
|
||||
normals =
|
||||
{
|
||||
{-1.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, -1.0f},
|
||||
{ 1.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 0.0f, 1.0f},
|
||||
{ 0.0f, -1.0f, 0.0f},
|
||||
{ 0.0f, 1.0f, 0.0f}
|
||||
};
|
||||
|
||||
rgb =
|
||||
{
|
||||
{0.0756f, 0.5651f, 0.5829f},
|
||||
{0.8596f, 0.1105f, 0.8455f},
|
||||
{0.8534f, 0.6143f, 0.3950f},
|
||||
{0.0438f, 0.6308f, 0.3065f},
|
||||
{0.9716f, 0.7170f, 0.8378f},
|
||||
{0.2472f, 0.7701f, 0.0234f},
|
||||
{0.6472f, 0.7467f, 0.5981f},
|
||||
{0.3502f, 0.7954f, 0.0443f}
|
||||
};
|
||||
|
||||
std::vector<std::pair<int, int>> tcvals =
|
||||
{
|
||||
{ 3, 0 },
|
||||
{ 5, 0 },
|
||||
{ 5, 2 },
|
||||
{ 3, 2 },
|
||||
{ 5, 4 },
|
||||
{ 3, 4 },
|
||||
{ 5, 6 },
|
||||
{ 3, 6 },
|
||||
{ 5, 8 },
|
||||
{ 3, 8 },
|
||||
{ 1, 4 },
|
||||
{ 1, 6 },
|
||||
{ 7, 4 },
|
||||
{ 7, 6 },
|
||||
};
|
||||
|
||||
for (const auto& p : tcvals)
|
||||
{
|
||||
texCoords.push_back({p.first * 0.125f, p.second * 0.125f});
|
||||
}
|
||||
|
||||
// mesh data is duplicated for each face
|
||||
std::vector<std::array<int, 9>> fileIndices =
|
||||
{
|
||||
{ 1, 1, 1, /**/ 2, 2, 1, /**/ 4, 3, 1 },
|
||||
{ 3, 4, 2, /**/ 4, 3, 2, /**/ 8, 5, 2 },
|
||||
{ 7, 6, 3, /**/ 8, 5, 3, /**/ 6, 7, 3 },
|
||||
{ 5, 8, 4, /**/ 6, 7, 4, /**/ 2, 9, 4 },
|
||||
{ 3, 11, 5, /**/ 7, 6, 5, /**/ 5, 8, 5 },
|
||||
{ 8, 5, 6, /**/ 4, 13, 6, /**/ 2, 14, 6 },
|
||||
};
|
||||
|
||||
for (const auto& fi : fileIndices)
|
||||
{
|
||||
pointsMesh.push_back(points.at(fi[0] - 1));
|
||||
pointsMesh.push_back(points.at(fi[3] - 1));
|
||||
pointsMesh.push_back(points.at(fi[6] - 1));
|
||||
rgbMesh.push_back(rgb.at(fi[0] - 1));
|
||||
rgbMesh.push_back(rgb.at(fi[3] - 1));
|
||||
rgbMesh.push_back(rgb.at(fi[6] - 1));
|
||||
|
||||
texCoordsMesh.push_back(texCoords.at(fi[1] - 1));
|
||||
texCoordsMesh.push_back(texCoords.at(fi[4] - 1));
|
||||
texCoordsMesh.push_back(texCoords.at(fi[7] - 1));
|
||||
|
||||
normalsMesh.push_back(normals.at(fi[2] - 1));
|
||||
normalsMesh.push_back(normals.at(fi[5] - 1));
|
||||
normalsMesh.push_back(normals.at(fi[8] - 1));
|
||||
}
|
||||
|
||||
indices =
|
||||
{
|
||||
{ 0, 1, 2},
|
||||
{ 3, 4, 5},
|
||||
{ 6, 7, 8},
|
||||
{ 9, 10, 11},
|
||||
{12, 13, 14},
|
||||
{15, 16, 17},
|
||||
};
|
||||
}
|
||||
|
||||
std::vector<Point3f> points, pointsMesh, normals, normalsMesh, rgb, rgbMesh;
|
||||
std::vector<Point2f> texCoords, texCoordsMesh;
|
||||
std::vector<std::vector<int32_t>> indices;
|
||||
};
|
||||
|
||||
OriginalObjGoldValues origGold;
|
||||
|
||||
TEST(PointCloud, LoadPointCloudObj)
|
||||
{
|
||||
std::vector<cv::Point3f> points, normals, rgb;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
cv::loadPointCloud(folder + "pointcloudio/orig.obj", points, normals, rgb);
|
||||
|
||||
EXPECT_EQ(origGold.points, points);
|
||||
EXPECT_EQ(origGold.rgb, rgb);
|
||||
EXPECT_EQ(origGold.normals, normals);
|
||||
}
|
||||
|
||||
TEST(PointCloud, LoadObjNoNormals)
|
||||
{
|
||||
std::vector<cv::Point3f> points, normals;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
cv::loadPointCloud(folder + "pointcloudio/orig_no_norms.obj", points, normals);
|
||||
|
||||
EXPECT_EQ(origGold.points, points);
|
||||
EXPECT_TRUE(normals.empty());
|
||||
}
|
||||
|
||||
TEST(PointCloud, SaveObj)
|
||||
{
|
||||
std::vector<cv::Point3f> points_gold, normals_gold, rgb_gold;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
auto new_path = tempfile("new.obj");
|
||||
|
||||
cv::loadPointCloud(folder + "pointcloudio/orig.obj", points_gold, normals_gold, rgb_gold);
|
||||
cv::savePointCloud(new_path, points_gold, normals_gold, rgb_gold);
|
||||
|
||||
std::vector<cv::Point3f> points, normals, rgb;
|
||||
|
||||
cv::loadPointCloud(new_path, points, normals, rgb);
|
||||
|
||||
EXPECT_EQ(normals, normals_gold);
|
||||
EXPECT_EQ(points, points_gold);
|
||||
EXPECT_EQ(rgb, rgb_gold);
|
||||
std::remove(new_path.c_str());
|
||||
}
|
||||
|
||||
TEST(PointCloud, LoadSavePly)
|
||||
{
|
||||
std::vector<cv::Point3f> points, normals, rgb;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
std::string new_path = tempfile("new.ply");
|
||||
|
||||
cv::loadPointCloud(folder + "pointcloudio/orig.ply", points, normals, rgb);
|
||||
cv::savePointCloud(new_path, points, normals, rgb);
|
||||
|
||||
std::vector<cv::Point3f> points_gold, normals_gold, rgb_gold;
|
||||
|
||||
cv::loadPointCloud(new_path, points_gold, normals_gold, rgb_gold);
|
||||
|
||||
EXPECT_EQ(normals_gold, normals);
|
||||
EXPECT_EQ(points_gold, points);
|
||||
EXPECT_EQ(rgb_gold, rgb);
|
||||
std::remove(new_path.c_str());
|
||||
}
|
||||
|
||||
TEST(PointCloud, LoadSaveMeshObj)
|
||||
{
|
||||
std::vector<cv::Point3f> points, normals, colors;
|
||||
std::vector<cv::Point2f> texCoords;
|
||||
std::vector<std::vector<int32_t>> indices;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
std::string new_path = tempfile("new_mesh.obj");
|
||||
|
||||
cv::loadMesh(folder + "pointcloudio/orig.obj", points, indices, normals, colors, texCoords);
|
||||
EXPECT_EQ(origGold.pointsMesh, points);
|
||||
EXPECT_EQ(origGold.indices, indices);
|
||||
EXPECT_EQ(origGold.normalsMesh, normals);
|
||||
EXPECT_EQ(origGold.rgbMesh, colors);
|
||||
EXPECT_EQ(origGold.texCoordsMesh, texCoords);
|
||||
cv::saveMesh(new_path, points, indices, normals, colors, texCoords);
|
||||
|
||||
std::vector<cv::Point3f> points_gold, normals_gold, colors_gold;
|
||||
std::vector<cv::Point2f> texCoords_gold;
|
||||
std::vector<std::vector<int32_t>> indices_gold;
|
||||
|
||||
cv::loadMesh(new_path, points_gold, indices_gold, normals_gold, colors_gold, texCoords_gold);
|
||||
EXPECT_FALSE(points_gold.empty());
|
||||
EXPECT_FALSE(indices_gold.empty());
|
||||
EXPECT_FALSE(normals_gold.empty());
|
||||
EXPECT_FALSE(colors_gold.empty());
|
||||
EXPECT_FALSE(texCoords_gold.empty());
|
||||
|
||||
EXPECT_EQ(normals_gold, normals);
|
||||
EXPECT_EQ(points_gold, points);
|
||||
EXPECT_EQ(indices_gold, indices);
|
||||
EXPECT_TRUE(!indices.empty());
|
||||
std::remove(new_path.c_str());
|
||||
}
|
||||
|
||||
typedef std::string PlyTestParamsType;
|
||||
typedef testing::TestWithParam<PlyTestParamsType> PlyTest;
|
||||
|
||||
TEST_P(PlyTest, LoadSaveMesh)
|
||||
{
|
||||
std::string fname = GetParam();
|
||||
|
||||
std::vector<cv::Point3f> points_gold, normals_gold, colors_gold;
|
||||
std::vector<cv::Vec3i> indices_gold;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
std::string new_path = tempfile("new_mesh.ply");
|
||||
|
||||
cv::loadMesh(folder + fname, points_gold, indices_gold, normals_gold, colors_gold);
|
||||
size_t truePts, trueFaces;
|
||||
if (fname.find("/dragon.ply") != fname.npos)
|
||||
{
|
||||
truePts = 50000; trueFaces = 100000;
|
||||
}
|
||||
else
|
||||
{
|
||||
truePts = 8; trueFaces = 12;
|
||||
}
|
||||
EXPECT_EQ(points_gold.size(), truePts);
|
||||
EXPECT_EQ(indices_gold.size(), trueFaces);
|
||||
|
||||
cv::saveMesh(new_path, points_gold, indices_gold, normals_gold, colors_gold);
|
||||
|
||||
std::vector<cv::Point3f> points, normals, colors;
|
||||
std::vector<cv::Vec3i> indices;
|
||||
cv::loadMesh(new_path, points, indices, normals, colors);
|
||||
|
||||
if (!normals.empty())
|
||||
{
|
||||
EXPECT_LE(cv::norm(normals_gold, normals, NORM_INF), 0);
|
||||
}
|
||||
EXPECT_LE(cv::norm(points_gold, points, NORM_INF), 0);
|
||||
EXPECT_LE(cv::norm(colors_gold, colors, NORM_INF), 0);
|
||||
EXPECT_EQ(indices_gold, indices);
|
||||
std::remove(new_path.c_str());
|
||||
}
|
||||
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(PointCloud, PlyTest,
|
||||
::testing::Values("pointcloudio/orig.ply", "pointcloudio/orig_ascii_fidx.ply", "pointcloudio/orig_bin_fidx.ply",
|
||||
"pointcloudio/orig_ascii_vidx.ply", "pointcloudio/orig_bin.ply", "viz/dragon.ply"));
|
||||
|
||||
TEST(PointCloud, NonexistentFile)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
std::vector<cv::Point3f> normals;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
cv::loadPointCloud(folder + "pointcloudio/fake.obj", points, normals);
|
||||
EXPECT_TRUE(points.empty());
|
||||
EXPECT_TRUE(normals.empty());
|
||||
}
|
||||
|
||||
TEST(PointCloud, LoadBadExtension)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
std::vector<cv::Point3f> normals;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
cv::loadPointCloud(folder + "pointcloudio/fake.fake", points, normals);
|
||||
EXPECT_TRUE(points.empty());
|
||||
EXPECT_TRUE(normals.empty());
|
||||
}
|
||||
|
||||
TEST(PointCloud, SaveBadExtension)
|
||||
{
|
||||
std::vector<cv::Point3f> points;
|
||||
std::vector<cv::Point3f> normals;
|
||||
|
||||
auto folder = cvtest::TS::ptr()->get_data_path();
|
||||
cv::savePointCloud(folder + "pointcloudio/fake.fake", points, normals);
|
||||
}
|
||||
|
||||
}} /* namespace opencv_test */
|
||||
@@ -0,0 +1,562 @@
|
||||
// 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/ptcloud/detail/pose_graph.hpp>
|
||||
|
||||
#include <opencv2/core/dualquaternion.hpp>
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
using namespace cv;
|
||||
|
||||
#ifdef HAVE_EIGEN
|
||||
|
||||
static Affine3d readAffine(std::istream& input)
|
||||
{
|
||||
Vec3d p;
|
||||
Vec4d q;
|
||||
input >> p[0] >> p[1] >> p[2];
|
||||
input >> q[1] >> q[2] >> q[3] >> q[0];
|
||||
// Normalize the quaternion to account for precision loss due to
|
||||
// serialization.
|
||||
return Affine3d(Quatd(q).toRotMat3x3(), p);
|
||||
};
|
||||
|
||||
// Rewritten from Ceres pose graph demo: https://ceres-solver.org/
|
||||
static Ptr<detail::PoseGraph> readG2OFile(const std::string& g2oFileName)
|
||||
{
|
||||
Ptr<detail::PoseGraph> pg = detail::PoseGraph::create();
|
||||
|
||||
// for debugging purposes
|
||||
size_t minId = 0, maxId = 1 << 30;
|
||||
|
||||
std::ifstream infile(g2oFileName.c_str());
|
||||
if (!infile)
|
||||
{
|
||||
CV_Error(cv::Error::StsError, "failed to open file");
|
||||
}
|
||||
|
||||
while (infile.good())
|
||||
{
|
||||
std::string data_type;
|
||||
// Read whether the type is a node or a constraint
|
||||
infile >> data_type;
|
||||
if (data_type == "VERTEX_SE3:QUAT")
|
||||
{
|
||||
size_t id;
|
||||
infile >> id;
|
||||
Affine3d pose = readAffine(infile);
|
||||
|
||||
if (id < minId || id >= maxId)
|
||||
continue;
|
||||
|
||||
bool fixed = (id == minId);
|
||||
|
||||
// Ensure we don't have duplicate poses
|
||||
if (pg->isNodeExist(id))
|
||||
{
|
||||
CV_LOG_INFO(NULL, "duplicated node, id=" << id);
|
||||
}
|
||||
pg->addNode(id, pose, fixed);
|
||||
}
|
||||
else if (data_type == "EDGE_SE3:QUAT")
|
||||
{
|
||||
size_t startId, endId;
|
||||
infile >> startId >> endId;
|
||||
Affine3d pose = readAffine(infile);
|
||||
|
||||
Matx66d info;
|
||||
for (int i = 0; i < 6 && infile.good(); ++i)
|
||||
{
|
||||
for (int j = i; j < 6 && infile.good(); ++j)
|
||||
{
|
||||
infile >> info(i, j);
|
||||
if (i != j)
|
||||
{
|
||||
info(j, i) = info(i, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if ((startId >= minId && startId < maxId) && (endId >= minId && endId < maxId))
|
||||
{
|
||||
pg->addEdge(startId, endId, pose, info);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
CV_Error(cv::Error::StsError, "unknown tag");
|
||||
}
|
||||
|
||||
// Clear any trailing whitespace from the line
|
||||
infile >> std::ws;
|
||||
}
|
||||
|
||||
return pg;
|
||||
}
|
||||
|
||||
|
||||
TEST(PoseGraph, sphereG2O)
|
||||
{
|
||||
// Test takes 15+ sec in Release mode and 400+ sec in Debug mode
|
||||
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
|
||||
|
||||
// The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
|
||||
// Connected paper:
|
||||
// L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
|
||||
// Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
|
||||
// In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
|
||||
|
||||
std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o";
|
||||
|
||||
Ptr<detail::PoseGraph> pg = readG2OFile(filename);
|
||||
|
||||
// You may change logging level to view detailed optimization report
|
||||
// For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
|
||||
|
||||
// geoScale=1 is experimental, not guaranteed to work on other problems
|
||||
// the rest are default params
|
||||
pg->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
|
||||
.setMaxIterations(100)
|
||||
.setCheckRelEnergyChange(true)
|
||||
.setRelEnergyDeltaTolerance(1e-6)
|
||||
.setGeodesic(true));
|
||||
|
||||
auto r = pg->optimize();
|
||||
|
||||
EXPECT_TRUE(r.found);
|
||||
EXPECT_LE(r.iters, 20); // should converge in 31 iterations
|
||||
|
||||
EXPECT_LE(r.energy, 1.47723e+06); // should converge to 1.47722e+06 or less
|
||||
|
||||
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
// Write edge-only model of how nodes are located in space
|
||||
std::string fname = "pgout.obj";
|
||||
std::fstream of(fname, std::fstream::out);
|
||||
std::vector<size_t> ids = pg->getNodesIds();
|
||||
for (const size_t& id : ids)
|
||||
{
|
||||
Point3d d = pg->getNodePose(id).translation();
|
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
|
||||
}
|
||||
|
||||
size_t esz = pg->getNumEdges();
|
||||
for (size_t i = 0; i < esz; i++)
|
||||
{
|
||||
size_t sid = pg->getEdgeStart(i), tid = pg->getEdgeEnd(i);
|
||||
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
|
||||
}
|
||||
|
||||
of.close();
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PoseGraphMST, optimization)
|
||||
{
|
||||
applyTestTag(CV_TEST_TAG_LONG, CV_TEST_TAG_DEBUG_VERYLONG);
|
||||
|
||||
// The dataset was taken from here: https://lucacarlone.mit.edu/datasets/
|
||||
// Connected paper:
|
||||
// L.Carlone, R.Tron, K.Daniilidis, and F.Dellaert.
|
||||
// Initialization Techniques for 3D SLAM : a Survey on Rotation Estimation and its Use in Pose Graph Optimization.
|
||||
// In IEEE Intl.Conf.on Robotics and Automation(ICRA), pages 4597 - 4604, 2015.
|
||||
|
||||
std::string filename = cvtest::TS::ptr()->get_data_path() + "/cv/rgbd/sphere_bignoise_vertex3.g2o";
|
||||
|
||||
Ptr<detail::PoseGraph> pgOptimizerOnly = readG2OFile(filename);
|
||||
Ptr<detail::PoseGraph> pgWithMSTAndOptimizer = readG2OFile(filename);
|
||||
Ptr<detail::PoseGraph> init = readG2OFile(filename);
|
||||
|
||||
double lambda = 0.485;
|
||||
pgWithMSTAndOptimizer->initializePosesWithMST(lambda);
|
||||
|
||||
// You may change logging level to view detailed optimization report
|
||||
// For example, set env. variable like this: OPENCV_LOG_LEVEL=INFO
|
||||
|
||||
// geoScale=1 is experimental, not guaranteed to work on other problems
|
||||
// the rest are default params
|
||||
pgOptimizerOnly->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
|
||||
.setMaxIterations(100)
|
||||
.setCheckRelEnergyChange(true)
|
||||
.setRelEnergyDeltaTolerance(1e-6)
|
||||
.setGeodesic(true));
|
||||
pgWithMSTAndOptimizer->createOptimizer(LevMarq::Settings().setGeoScale(1.0)
|
||||
.setMaxIterations(100)
|
||||
.setCheckRelEnergyChange(true)
|
||||
.setRelEnergyDeltaTolerance(1e-6)
|
||||
.setGeodesic(true));
|
||||
|
||||
auto r1 = pgWithMSTAndOptimizer->optimize();
|
||||
auto r2 = pgOptimizerOnly->optimize();
|
||||
|
||||
EXPECT_TRUE(r1.found);
|
||||
EXPECT_TRUE(r2.found);
|
||||
EXPECT_LE(r2.energy, 1.47723e+06);
|
||||
// Allow small tolerance due to optimization differences; final energy/iterations are effectively the same
|
||||
EXPECT_LE(std::abs(r1.energy - r2.energy), 1e-2);
|
||||
ASSERT_LE(std::abs(r1.iters - r2.iters), 1);
|
||||
|
||||
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
// Note:
|
||||
// A custom .obj writer is used here instead of cv::saveMesh because saveMesh expects faces
|
||||
// (i.e., polygons with 3 or more vertices) and writes them using the "f i j k..." syntax in
|
||||
// the .obj file. Since pose graphs consist of edges rather than polygonal faces, we represent
|
||||
// them using line segments ("l i j").
|
||||
// As saveMesh does not support writing "l" lines, it is not suitable in this context.
|
||||
|
||||
std::string fname;
|
||||
std::fstream of;
|
||||
std::vector<size_t> ids;
|
||||
size_t esz;
|
||||
|
||||
// Write OBJ for MST-initialized pose graph with optimizer
|
||||
fname = "pg_with_mst_and_optimizer.obj";
|
||||
of.open(fname, std::fstream::out);
|
||||
ids = pgWithMSTAndOptimizer->getNodesIds();
|
||||
for (const size_t& id : ids)
|
||||
{
|
||||
Point3d d = pgWithMSTAndOptimizer->getNodePose(id).translation();
|
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
|
||||
}
|
||||
esz = pgWithMSTAndOptimizer->getNumEdges();
|
||||
for (size_t i = 0; i < esz; i++)
|
||||
{
|
||||
size_t sid = pgWithMSTAndOptimizer->getEdgeStart(i), tid = pgWithMSTAndOptimizer->getEdgeEnd(i);
|
||||
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
|
||||
}
|
||||
of.close();
|
||||
|
||||
// Write OBJ for optimizer-only pose graph
|
||||
fname = "pg_optimizer_only.obj";
|
||||
of.open(fname, std::fstream::out);
|
||||
ids = pgOptimizerOnly->getNodesIds();
|
||||
for (const size_t& id : ids)
|
||||
{
|
||||
Point3d d = pgOptimizerOnly->getNodePose(id).translation();
|
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
|
||||
}
|
||||
esz = pgOptimizerOnly->getNumEdges();
|
||||
for (size_t i = 0; i < esz; i++)
|
||||
{
|
||||
size_t sid = pgOptimizerOnly->getEdgeStart(i), tid = pgOptimizerOnly->getEdgeEnd(i);
|
||||
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
|
||||
}
|
||||
of.close();
|
||||
|
||||
// Write OBJ for initial pose graph
|
||||
fname = "pg_init.obj";
|
||||
of.open(fname, std::fstream::out);
|
||||
ids = init->getNodesIds();
|
||||
for (const size_t& id : ids)
|
||||
{
|
||||
Point3d d = init->getNodePose(id).translation();
|
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
|
||||
}
|
||||
esz = init->getNumEdges();
|
||||
for (size_t i = 0; i < esz; i++)
|
||||
{
|
||||
size_t sid = init->getEdgeStart(i), tid = init->getEdgeEnd(i);
|
||||
of << "l " << sid + 1 << " " << tid + 1 << std::endl;
|
||||
}
|
||||
of.close();
|
||||
}
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
// Wireframe meshes for debugging visualization purposes
|
||||
struct Mesh
|
||||
{
|
||||
std::vector<Point3f> pts;
|
||||
std::vector<Vec2i> lines;
|
||||
|
||||
Mesh join(const Mesh& m2) const
|
||||
{
|
||||
Mesh mo;
|
||||
|
||||
size_t sz1 = this->pts.size();
|
||||
std::copy(this->pts.begin(), this->pts.end(), std::back_inserter(mo.pts));
|
||||
std::copy(m2.pts.begin(), m2.pts.end(), std::back_inserter(mo.pts));
|
||||
|
||||
std::copy(this->lines.begin(), this->lines.end(), std::back_inserter(mo.lines));
|
||||
std::transform(m2.lines.begin(), m2.lines.end(), std::back_inserter(mo.lines),
|
||||
[sz1](Vec2i ab) { return Vec2i(ab[0] + (int)sz1, ab[1] + (int)sz1); });
|
||||
|
||||
return mo;
|
||||
}
|
||||
|
||||
Mesh transform(Affine3f a, float scale = 1.f) const
|
||||
{
|
||||
Mesh out;
|
||||
out.lines = this->lines;
|
||||
for (Point3f p : this->pts)
|
||||
{
|
||||
out.pts.push_back(a * (p * scale));
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
// 0-2 - min, 3-5 - max
|
||||
Vec6f getBoundingBox() const
|
||||
{
|
||||
float maxv = std::numeric_limits<float>::max();
|
||||
Vec3f xmin(maxv, maxv, maxv), xmax(-maxv, -maxv, -maxv);
|
||||
for (Point3f p : this->pts)
|
||||
{
|
||||
xmin[0] = min(p.x, xmin[0]); xmin[1] = min(p.y, xmin[1]); xmin[2] = min(p.z, xmin[2]);
|
||||
xmax[0] = max(p.x, xmax[0]); xmax[1] = max(p.y, xmax[1]); xmax[2] = max(p.z, xmax[2]);
|
||||
}
|
||||
return Vec6f(xmin[0], xmin[1], xmin[2], xmax[0], xmax[1], xmax[2]);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Mesh seg7(int d)
|
||||
{
|
||||
const std::vector<Point3f> pt = { {0, 0, 0}, {0, 1, 0},
|
||||
{1, 0, 0}, {1, 1, 0},
|
||||
{2, 0, 0}, {2, 1, 0} };
|
||||
|
||||
std::vector<Mesh> seg(7);
|
||||
seg[0].pts = { pt[0], pt[1] };
|
||||
seg[1].pts = { pt[1], pt[3] };
|
||||
seg[2].pts = { pt[3], pt[5] };
|
||||
seg[3].pts = { pt[5], pt[4] };
|
||||
seg[4].pts = { pt[4], pt[2] };
|
||||
seg[5].pts = { pt[2], pt[0] };
|
||||
seg[6].pts = { pt[2], pt[3] };
|
||||
for (int i = 0; i < 7; i++)
|
||||
seg[i].lines = { {0, 1} };
|
||||
|
||||
vector<Mesh> digits = {
|
||||
seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]), // 0
|
||||
seg[1].join(seg[2]), // 1
|
||||
seg[0].join(seg[1]).join(seg[3]).join(seg[4]).join(seg[6]), // 2
|
||||
seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[6]), // 3
|
||||
seg[1].join(seg[2]).join(seg[5]).join(seg[6]), // 4
|
||||
seg[0].join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 5
|
||||
seg[0].join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 6
|
||||
seg[0].join(seg[1]).join(seg[2]), // 7
|
||||
seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[4]).join(seg[5]).join(seg[6]), // 8
|
||||
seg[0].join(seg[1]).join(seg[2]).join(seg[3]).join(seg[5]).join(seg[6]), // 9
|
||||
seg[6], // -
|
||||
};
|
||||
|
||||
return digits[d];
|
||||
}
|
||||
|
||||
Mesh drawId(size_t x)
|
||||
{
|
||||
vector<int> digits;
|
||||
do
|
||||
{
|
||||
digits.push_back(x % 10);
|
||||
x /= 10;
|
||||
}
|
||||
while (x > 0);
|
||||
float spacing = 0.2f;
|
||||
Mesh m;
|
||||
for (size_t i = 0; i < digits.size(); i++)
|
||||
{
|
||||
Mesh digit = seg7(digits[digits.size() - 1 - i]);
|
||||
Vec6f bb = digit.getBoundingBox();
|
||||
digit = digit.transform(Affine3f().translate(-Vec3f(0, bb[1], 0)));
|
||||
Vec3f tr;
|
||||
if (m.pts.empty())
|
||||
tr = Vec3f();
|
||||
else
|
||||
tr = Vec3f(0, (m.getBoundingBox()[4] + spacing), 0);
|
||||
m = m.join(digit.transform( Affine3f().translate(tr) ));
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
|
||||
Mesh drawFromTo(size_t f, size_t t)
|
||||
{
|
||||
Mesh m;
|
||||
|
||||
Mesh df = drawId(f);
|
||||
Mesh dp = seg7(10);
|
||||
Mesh dt = drawId(t);
|
||||
|
||||
float spacing = 0.2f;
|
||||
m = m.join(df).join(dp.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + spacing, 0))))
|
||||
.join(dt.transform(Affine3f().translate(Vec3f(0, df.getBoundingBox()[4] + 2*spacing + 1, 0))));
|
||||
|
||||
return m;
|
||||
}
|
||||
|
||||
Mesh drawPoseGraph(Ptr<detail::PoseGraph> pg)
|
||||
{
|
||||
Mesh marker;
|
||||
marker.pts = { {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {1, 1, 0} };
|
||||
marker.lines = { {0, 1}, {0, 2}, {0, 3}, {1, 4} };
|
||||
|
||||
Mesh allMeshes;
|
||||
Affine3f margin = Affine3f().translate(Vec3f(0.1f, 0.1f, 0));
|
||||
std::vector<size_t> ids = pg->getNodesIds();
|
||||
for (const size_t& id : ids)
|
||||
{
|
||||
Affine3f pose = pg->getNodePose(id);
|
||||
|
||||
Mesh m = marker.join(drawId(id).transform(margin, 0.25f)).transform(pose);
|
||||
allMeshes = allMeshes.join(m);
|
||||
}
|
||||
|
||||
// edges
|
||||
margin = Affine3f().translate(Vec3f(0.05f, 0.05f, 0));
|
||||
for (size_t i = 0; i < pg->getNumEdges(); i++)
|
||||
{
|
||||
Affine3f pose = pg->getEdgePose(i);
|
||||
size_t sid = pg->getEdgeStart(i);
|
||||
size_t did = pg->getEdgeEnd(i);
|
||||
Affine3f spose = pg->getNodePose(sid);
|
||||
Affine3f dpose = spose * pose;
|
||||
|
||||
Mesh m = marker.join(drawFromTo(sid, did).transform(margin, 0.125f)).transform(dpose);
|
||||
allMeshes = allMeshes.join(m);
|
||||
}
|
||||
|
||||
return allMeshes;
|
||||
}
|
||||
|
||||
void writeObj(const std::string& fname, const Mesh& m)
|
||||
{
|
||||
// Write edge-only model of how nodes are located in space
|
||||
std::fstream of(fname, std::fstream::out);
|
||||
for (const Point3f& d : m.pts)
|
||||
{
|
||||
of << "v " << d.x << " " << d.y << " " << d.z << std::endl;
|
||||
}
|
||||
|
||||
for (const Vec2i& v : m.lines)
|
||||
{
|
||||
of << "l " << v[0] + 1 << " " << v[1] + 1 << std::endl;
|
||||
}
|
||||
|
||||
of.close();
|
||||
}
|
||||
|
||||
|
||||
TEST(PoseGraph, simple)
|
||||
{
|
||||
|
||||
Ptr<detail::PoseGraph> pg = detail::PoseGraph::create();
|
||||
|
||||
DualQuatf true0(1, 0, 0, 0, 0, 0, 0, 0);
|
||||
DualQuatf true1 = DualQuatf::createFromPitch((float)CV_PI / 3.0f, 10.0f, Vec3f(1, 1.5f, 1.2f), Vec3f());
|
||||
|
||||
DualQuatf pose0 = true0;
|
||||
vector<DualQuatf> noise(7);
|
||||
for (size_t i = 0; i < noise.size(); i++)
|
||||
{
|
||||
float angle = cv::theRNG().uniform(-1.f, 1.f);
|
||||
float shift = cv::theRNG().uniform(-2.f, 2.f);
|
||||
Matx31f axis = Vec3f::randu(0.f, 1.f), moment = Vec3f::randu(0.f, 1.f);
|
||||
noise[i] = DualQuatf::createFromPitch(angle, shift,
|
||||
Vec3f(axis(0), axis(1), axis(2)),
|
||||
Vec3f(moment(0), moment(1), moment(2)));
|
||||
}
|
||||
|
||||
DualQuatf pose1 = noise[0] * true1;
|
||||
|
||||
DualQuatf diff = true1 * true0.inv();
|
||||
vector<DualQuatf> cfrom = { diff, diff * noise[1], noise[2] * diff };
|
||||
DualQuatf diffInv = diff.inv();
|
||||
vector<DualQuatf> cto = { diffInv, diffInv * noise[3], noise[4] * diffInv };
|
||||
|
||||
pg->addNode(123, pose0.toAffine3(), true);
|
||||
pg->addNode(456, pose1.toAffine3(), false);
|
||||
|
||||
Matx66f info = Matx66f::eye();
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
pg->addEdge(123, 456, cfrom[i].toAffine3(), info);
|
||||
pg->addEdge(456, 123, cto[i].toAffine3(), info);
|
||||
}
|
||||
|
||||
Mesh allMeshes = drawPoseGraph(pg);
|
||||
|
||||
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
writeObj("pg_simple_in.obj", allMeshes);
|
||||
}
|
||||
|
||||
auto r = pg->optimize();
|
||||
|
||||
Mesh after = drawPoseGraph(pg);
|
||||
|
||||
// Add the "--test_debug" to arguments to see resulting pose graph nodes positions
|
||||
if (cvtest::debugLevel > 0)
|
||||
{
|
||||
writeObj("pg_simple_out.obj", after);
|
||||
}
|
||||
|
||||
EXPECT_TRUE(r.found);
|
||||
}
|
||||
#else
|
||||
|
||||
TEST(PoseGraph, sphereG2O)
|
||||
{
|
||||
throw SkipTestException("Build with Eigen required for pose graph optimization");
|
||||
}
|
||||
|
||||
TEST(PoseGraphMST, optimization)
|
||||
{
|
||||
throw SkipTestException("Build with Eigen required for pose graph optimization");
|
||||
}
|
||||
|
||||
TEST(PoseGraph, simple)
|
||||
{
|
||||
throw SkipTestException("Build with Eigen required for pose graph optimization");
|
||||
}
|
||||
#endif
|
||||
|
||||
TEST(LevMarq, Rosenbrock)
|
||||
{
|
||||
auto f = [](double x, double y) -> double
|
||||
{
|
||||
return (1.0 - x) * (1.0 - x) + 100.0 * (y - x * x) * (y - x * x);
|
||||
};
|
||||
|
||||
auto j = [](double x, double y) -> Matx12d
|
||||
{
|
||||
return {/*dx*/ -2.0 + 2.0 * x - 400.0 * x * y + 400.0 * x*x*x,
|
||||
/*dy*/ 200.0 * y - 200.0 * x*x,
|
||||
};
|
||||
};
|
||||
|
||||
LevMarq solver(2, [f, j](InputOutputArray param, OutputArray err, OutputArray jv) -> bool
|
||||
{
|
||||
Vec2d v = param.getMat();
|
||||
double x = v[0], y = v[1];
|
||||
err.create(1, 1, CV_64F);
|
||||
err.getMat().at<double>(0) = f(x, y);
|
||||
if (jv.needed())
|
||||
{
|
||||
jv.create(1, 2, CV_64F);
|
||||
Mat(j(x, y)).copyTo(jv);
|
||||
}
|
||||
return true;
|
||||
},
|
||||
LevMarq::Settings().setGeodesic(true));
|
||||
|
||||
Mat_<double> x (Vec2d(1, 3));
|
||||
|
||||
auto r = solver.run(x);
|
||||
|
||||
EXPECT_TRUE(r.found);
|
||||
EXPECT_LT(r.energy, 0.035);
|
||||
EXPECT_LE(r.iters, 17);
|
||||
}
|
||||
|
||||
|
||||
}} // namespace
|
||||
@@ -0,0 +1,21 @@
|
||||
// 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.
|
||||
#ifndef __OPENCV_TEST_PRECOMP_HPP__
|
||||
#define __OPENCV_TEST_PRECOMP_HPP__
|
||||
|
||||
#include <functional>
|
||||
#include <numeric>
|
||||
|
||||
#include "opencv2/ts.hpp"
|
||||
#include "opencv2/geometry.hpp"
|
||||
#include "opencv2/ptcloud.hpp"
|
||||
#include <opencv2/core/utils/logger.hpp>
|
||||
#include "opencv2/ptcloud/depth.hpp"
|
||||
#include "opencv2/ptcloud/odometry.hpp"
|
||||
|
||||
#ifdef HAVE_OPENCL
|
||||
#include <opencv2/core/ocl.hpp>
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,107 @@
|
||||
// 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
|
||||
|
||||
// This code is also subject to the license terms in the LICENSE_WillowGarage.md file found in this module's directory
|
||||
|
||||
#include "test_precomp.hpp"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
|
||||
class RgbdDepthRegistrationTest
|
||||
{
|
||||
public:
|
||||
RgbdDepthRegistrationTest() { }
|
||||
~RgbdDepthRegistrationTest() { }
|
||||
|
||||
void run()
|
||||
{
|
||||
// Test all three input types for no-op registrations (where a depth image is registered to itself)
|
||||
noOpRandomRegistrationTest<unsigned short>(100, 2500);
|
||||
noOpRandomRegistrationTest<float>(0.1f, 2.5f);
|
||||
noOpRandomRegistrationTest<double>(0.1, 2.5);
|
||||
|
||||
// Test sentinel value handling, occlusion, and dilation
|
||||
{
|
||||
// K from a VGA Kinect
|
||||
Mat K = (Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
|
||||
|
||||
int width = 640, height = 480;
|
||||
|
||||
// All elements are zero except for first two along the diagonal
|
||||
Mat_<unsigned short> vgaDepth(height, width, (unsigned short)0);
|
||||
vgaDepth(0, 0) = 1001;
|
||||
vgaDepth(1, 1) = 1000;
|
||||
|
||||
Mat_<unsigned short> registeredDepth;
|
||||
registerDepth(K, K, Mat(), Matx44f::eye(), vgaDepth, Size(width, height), registeredDepth, true);
|
||||
|
||||
// We expect the closer depth of 1000 to occlude the more distant depth and occupy the
|
||||
// upper four left pixels in the depth image because of dilation
|
||||
Mat_<unsigned short> expectedResult(height, width, (unsigned short)0);
|
||||
expectedResult(0, 0) = 1000;
|
||||
expectedResult(0, 1) = 1000;
|
||||
expectedResult(1, 0) = 1000;
|
||||
expectedResult(1, 1) = 1000;
|
||||
|
||||
Mat ad;
|
||||
absdiff(registeredDepth, expectedResult, ad);
|
||||
ASSERT_GT(std::numeric_limits<double>::min(), abs(sum(ad)[0])) << "Dilation and occlusion";
|
||||
}
|
||||
}
|
||||
|
||||
template <class DepthDepth>
|
||||
void noOpRandomRegistrationTest(DepthDepth minDepth, DepthDepth maxDepth)
|
||||
{
|
||||
// K from a VGA Kinect
|
||||
Mat K = (Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.);
|
||||
|
||||
// Create a random depth image
|
||||
RNG rng;
|
||||
Mat_<DepthDepth> randomVGADepth(480, 640);
|
||||
rng.fill(randomVGADepth, RNG::UNIFORM, minDepth, maxDepth);
|
||||
|
||||
Mat registeredDepth;
|
||||
registerDepth(K, K, Mat(), Matx44f::eye(), randomVGADepth, Size(640, 480), registeredDepth);
|
||||
|
||||
// Check per-pixel relative difference
|
||||
|
||||
Mat ad;
|
||||
absdiff(registeredDepth, randomVGADepth, ad);
|
||||
Mat mmin;
|
||||
cv::min(registeredDepth, randomVGADepth, mmin);
|
||||
Mat rel = ad / mmin;
|
||||
double maxDiff = cv::norm(rel, NORM_INF);
|
||||
ASSERT_GT(1e-07, maxDiff) << "No-op registration";
|
||||
}
|
||||
};
|
||||
|
||||
TEST(RGBD_DepthRegistration, compute)
|
||||
{
|
||||
RgbdDepthRegistrationTest test;
|
||||
test.run();
|
||||
}
|
||||
|
||||
TEST(RGBD_DepthRegistration, issue_2234)
|
||||
{
|
||||
Matx33f intrinsicsDepth(100, 0, 50, 0, 100, 50, 0, 0, 1);
|
||||
Matx33f intrinsicsColor(100, 0, 200, 0, 100, 50, 0, 0, 1);
|
||||
|
||||
Mat_<float> depthMat(100, 100, (float)0.);
|
||||
for(int i = 1; i <= 100; i++)
|
||||
{
|
||||
for(int j = 1; j <= 100; j++)
|
||||
depthMat(i-1,j-1) = (float)j;
|
||||
}
|
||||
|
||||
Mat registeredDepth;
|
||||
registerDepth(intrinsicsDepth, intrinsicsColor, Mat(), Matx44f::eye(), depthMat, Size(400, 100), registeredDepth);
|
||||
|
||||
Rect roi( 150, 0, 100, 100 );
|
||||
Mat subM(registeredDepth,roi);
|
||||
|
||||
EXPECT_EQ(0, cvtest::norm(subM, depthMat, NORM_INF));
|
||||
}
|
||||
|
||||
|
||||
}} // namespace
|
||||
@@ -0,0 +1,946 @@
|
||||
// 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"
|
||||
|
||||
namespace opencv_test { namespace {
|
||||
using namespace cv;
|
||||
|
||||
// that was easier than using CV_ENUM() macro
|
||||
namespace
|
||||
{
|
||||
using namespace cv;
|
||||
struct CullingModeEnum
|
||||
{
|
||||
static const std::array<TriangleCullingMode, 3> vals;
|
||||
static const std::array<std::string, 3> svals;
|
||||
|
||||
CullingModeEnum(TriangleCullingMode v = RASTERIZE_CULLING_NONE) : val(v) {}
|
||||
operator TriangleCullingMode() const { return val; }
|
||||
void PrintTo(std::ostream *os) const
|
||||
{
|
||||
int v = int(val);
|
||||
if (v >= 0 && v < (int)vals.size())
|
||||
{
|
||||
*os << svals[v];
|
||||
}
|
||||
else
|
||||
{
|
||||
*os << "UNKNOWN";
|
||||
}
|
||||
}
|
||||
static ::testing::internal::ParamGenerator<CullingModeEnum> all()
|
||||
{
|
||||
return ::testing::Values(CullingModeEnum(vals[0]),
|
||||
CullingModeEnum(vals[1]),
|
||||
CullingModeEnum(vals[2]));
|
||||
}
|
||||
|
||||
private:
|
||||
TriangleCullingMode val;
|
||||
};
|
||||
|
||||
const std::array<TriangleCullingMode, 3> CullingModeEnum::vals
|
||||
{
|
||||
RASTERIZE_CULLING_NONE,
|
||||
RASTERIZE_CULLING_CW,
|
||||
RASTERIZE_CULLING_CCW
|
||||
};
|
||||
const std::array<std::string, 3> CullingModeEnum::svals
|
||||
{
|
||||
std::string("None"),
|
||||
std::string("CW"),
|
||||
std::string("CCW")
|
||||
};
|
||||
|
||||
static inline void PrintTo(const CullingModeEnum &t, std::ostream *os) { t.PrintTo(os); }
|
||||
}
|
||||
|
||||
// that was easier than using CV_ENUM() macro
|
||||
namespace
|
||||
{
|
||||
using namespace cv;
|
||||
struct ShadingTypeEnum
|
||||
{
|
||||
static const std::array<TriangleShadingType, 3> vals;
|
||||
static const std::array<std::string, 3> svals;
|
||||
|
||||
ShadingTypeEnum(TriangleShadingType v = RASTERIZE_SHADING_WHITE) : val(v) {}
|
||||
operator TriangleShadingType() const { return val; }
|
||||
void PrintTo(std::ostream *os) const
|
||||
{
|
||||
int v = int(val);
|
||||
if (v >= 0 && v < (int)vals.size())
|
||||
{
|
||||
*os << svals[v];
|
||||
}
|
||||
else
|
||||
{
|
||||
*os << "UNKNOWN";
|
||||
}
|
||||
}
|
||||
static ::testing::internal::ParamGenerator<ShadingTypeEnum> all()
|
||||
{
|
||||
return ::testing::Values(ShadingTypeEnum(vals[0]),
|
||||
ShadingTypeEnum(vals[1]),
|
||||
ShadingTypeEnum(vals[2]));
|
||||
}
|
||||
|
||||
private:
|
||||
TriangleShadingType val;
|
||||
};
|
||||
|
||||
const std::array<TriangleShadingType, 3> ShadingTypeEnum::vals
|
||||
{
|
||||
RASTERIZE_SHADING_WHITE,
|
||||
RASTERIZE_SHADING_FLAT,
|
||||
RASTERIZE_SHADING_SHADED
|
||||
};
|
||||
const std::array<std::string, 3> ShadingTypeEnum::svals
|
||||
{
|
||||
std::string("White"),
|
||||
std::string("Flat"),
|
||||
std::string("Shaded")
|
||||
};
|
||||
|
||||
static inline void PrintTo(const ShadingTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
|
||||
}
|
||||
|
||||
|
||||
enum class ModelType
|
||||
{
|
||||
Empty = 0,
|
||||
File = 1,
|
||||
Clipping = 2,
|
||||
Color = 3,
|
||||
Centered = 4
|
||||
};
|
||||
|
||||
// that was easier than using CV_ENUM() macro
|
||||
namespace
|
||||
{
|
||||
using namespace cv;
|
||||
struct ModelTypeEnum
|
||||
{
|
||||
static const std::array<ModelType, 5> vals;
|
||||
static const std::array<std::string, 5> svals;
|
||||
|
||||
ModelTypeEnum(ModelType v = ModelType::Empty) : val(v) {}
|
||||
operator ModelType() const { return val; }
|
||||
void PrintTo(std::ostream *os) const
|
||||
{
|
||||
int v = int(val);
|
||||
if (v >= 0 && v < (int)vals.size())
|
||||
{
|
||||
*os << svals[v];
|
||||
}
|
||||
else
|
||||
{
|
||||
*os << "UNKNOWN";
|
||||
}
|
||||
}
|
||||
static ::testing::internal::ParamGenerator<ModelTypeEnum> all()
|
||||
{
|
||||
return ::testing::Values(ModelTypeEnum(vals[0]),
|
||||
ModelTypeEnum(vals[1]),
|
||||
ModelTypeEnum(vals[2]),
|
||||
ModelTypeEnum(vals[3]),
|
||||
ModelTypeEnum(vals[4]));
|
||||
}
|
||||
|
||||
private:
|
||||
ModelType val;
|
||||
};
|
||||
|
||||
const std::array<ModelType, 5> ModelTypeEnum::vals
|
||||
{
|
||||
ModelType::Empty,
|
||||
ModelType::File,
|
||||
ModelType::Clipping,
|
||||
ModelType::Color,
|
||||
ModelType::Centered
|
||||
};
|
||||
const std::array<std::string, 5> ModelTypeEnum::svals
|
||||
{
|
||||
std::string("Empty"),
|
||||
std::string("File"),
|
||||
std::string("Clipping"),
|
||||
std::string("Color"),
|
||||
std::string("Centered")
|
||||
};
|
||||
|
||||
static inline void PrintTo(const ModelTypeEnum &t, std::ostream *os) { t.PrintTo(os); }
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
std::string printEnum(T v)
|
||||
{
|
||||
std::ostringstream ss;
|
||||
v.PrintTo(&ss);
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
|
||||
static Matx44d lookAtMatrixCal(const Vec3d& position, const Vec3d& lookat, const Vec3d& upVector)
|
||||
{
|
||||
Vec3d w = cv::normalize(position - lookat);
|
||||
Vec3d u = cv::normalize(upVector.cross(w));
|
||||
|
||||
Vec3d v = w.cross(u);
|
||||
|
||||
Matx44d res(u[0], u[1], u[2], 0,
|
||||
v[0], v[1], v[2], 0,
|
||||
w[0], w[1], w[2], 0,
|
||||
0, 0, 0, 1.0);
|
||||
|
||||
Matx44d translate(1.0, 0, 0, -position[0],
|
||||
0, 1.0, 0, -position[1],
|
||||
0, 0, 1.0, -position[2],
|
||||
0, 0, 0, 1.0);
|
||||
res = res * translate;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
||||
static void generateNormals(const std::vector<Vec3f>& points, const std::vector<std::vector<int>>& indices,
|
||||
std::vector<Vec3f>& normals)
|
||||
{
|
||||
std::vector<std::vector<Vec3f>> preNormals(points.size(), std::vector<Vec3f>());
|
||||
|
||||
for (const auto& tri : indices)
|
||||
{
|
||||
Vec3f p0 = points[tri[0]];
|
||||
Vec3f p1 = points[tri[1]];
|
||||
Vec3f p2 = points[tri[2]];
|
||||
|
||||
Vec3f cross = cv::normalize((p1 - p0).cross(p2 - p0));
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
preNormals[tri[i]].push_back(cross);
|
||||
}
|
||||
}
|
||||
|
||||
normals.reserve(points.size());
|
||||
for (const auto& pn : preNormals)
|
||||
{
|
||||
Vec3f sum { };
|
||||
for (const auto& n : pn)
|
||||
{
|
||||
sum += n;
|
||||
}
|
||||
normals.push_back(cv::normalize(sum));
|
||||
}
|
||||
}
|
||||
|
||||
// load model once and keep it in static memory
|
||||
static void getModelOnce(const std::string& objectPath, std::vector<Vec3f>& vertices,
|
||||
std::vector<Vec3i>& indices, std::vector<Vec3f>& colors)
|
||||
{
|
||||
static bool load = false;
|
||||
static std::vector<Vec3f> vert, col;
|
||||
static std::vector<Vec3i> ind;
|
||||
|
||||
if (!load)
|
||||
{
|
||||
std::vector<vector<int>> indvec;
|
||||
// using per-vertex normals as colors
|
||||
loadMesh(objectPath, vert, indvec);
|
||||
generateNormals(vert, indvec, col);
|
||||
|
||||
for (const auto &vec : indvec)
|
||||
{
|
||||
ind.push_back({vec[0], vec[1], vec[2]});
|
||||
}
|
||||
|
||||
for (auto &color : col)
|
||||
{
|
||||
color = Vec3f(abs(color[0]), abs(color[1]), abs(color[2]));
|
||||
}
|
||||
|
||||
load = true;
|
||||
}
|
||||
|
||||
vertices = vert;
|
||||
colors = col;
|
||||
indices = ind;
|
||||
}
|
||||
|
||||
class ModelData
|
||||
{
|
||||
public:
|
||||
ModelData(ModelType type = ModelType::Empty)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case ModelType::Empty:
|
||||
{
|
||||
position = Vec3d(0.0, 0.0, 0.0);
|
||||
lookat = Vec3d(0.0, 0.0, 0.0);
|
||||
upVector = Vec3d(0.0, 1.0, 0.0);
|
||||
|
||||
fovy = 45.0;
|
||||
|
||||
vertices = std::vector<Vec3f>(4, {2.0f, 0, -2.0f});
|
||||
colors = std::vector<Vec3f>(4, {0, 0, 1.0f});
|
||||
indices = { };
|
||||
}
|
||||
break;
|
||||
case ModelType::File:
|
||||
{
|
||||
string objectPath = findDataFile("viz/dragon.ply");
|
||||
|
||||
position = Vec3d( 1.9, 0.4, 1.3);
|
||||
lookat = Vec3d( 0.0, 0.0, 0.0);
|
||||
upVector = Vec3d( 0.0, 1.0, 0.0);
|
||||
|
||||
fovy = 45.0;
|
||||
|
||||
getModelOnce(objectPath, vertices, indices, colors);
|
||||
}
|
||||
break;
|
||||
case ModelType::Clipping:
|
||||
{
|
||||
position = Vec3d(0.0, 0.0, 5.0);
|
||||
lookat = Vec3d(0.0, 0.0, 0.0);
|
||||
upVector = Vec3d(0.0, 1.0, 0.0);
|
||||
|
||||
fovy = 45.0;
|
||||
|
||||
vertices =
|
||||
{
|
||||
{ 2.0, 0.0, -2.0}, { 0.0, -6.0, -2.0}, {-2.0, 0.0, -2.0},
|
||||
{ 3.5, -1.0, -5.0}, { 2.5, -2.5, -5.0}, {-1.0, 1.0, -5.0},
|
||||
{-6.5, -1.0, -3.0}, {-2.5, -2.0, -3.0}, { 1.0, 1.0, -5.0},
|
||||
};
|
||||
|
||||
indices = { {0, 1, 2}, {3, 4, 5}, {6, 7, 8} };
|
||||
|
||||
Vec3f col1(217.0, 238.0, 185.0);
|
||||
Vec3f col2(185.0, 217.0, 238.0);
|
||||
Vec3f col3(150.0, 10.0, 238.0);
|
||||
|
||||
col1 *= (1.f / 255.f);
|
||||
col2 *= (1.f / 255.f);
|
||||
col3 *= (1.f / 255.f);
|
||||
|
||||
colors =
|
||||
{
|
||||
col1, col2, col3,
|
||||
col2, col3, col1,
|
||||
col3, col1, col2,
|
||||
};
|
||||
}
|
||||
break;
|
||||
case ModelType::Centered:
|
||||
{
|
||||
position = Vec3d(0.0, 0.0, 5.0);
|
||||
lookat = Vec3d(0.0, 0.0, 0.0);
|
||||
upVector = Vec3d(0.0, 1.0, 0.0);
|
||||
|
||||
fovy = 45.0;
|
||||
|
||||
vertices =
|
||||
{
|
||||
{ 2.0, 0.0, -2.0}, { 0.0, -2.0, -2.0}, {-2.0, 0.0, -2.0},
|
||||
{ 3.5, -1.0, -5.0}, { 2.5, -1.5, -5.0}, {-1.0, 0.5, -5.0},
|
||||
};
|
||||
|
||||
indices = { {0, 1, 2}, {3, 4, 5} };
|
||||
|
||||
Vec3f col1(217.0, 238.0, 185.0);
|
||||
Vec3f col2(185.0, 217.0, 238.0);
|
||||
|
||||
col1 *= (1.f / 255.f);
|
||||
col2 *= (1.f / 255.f);
|
||||
|
||||
colors =
|
||||
{
|
||||
col1, col2, col1,
|
||||
col2, col1, col2,
|
||||
};
|
||||
}
|
||||
break;
|
||||
case ModelType::Color:
|
||||
{
|
||||
position = Vec3d(0.0, 0.0, 5.0);
|
||||
lookat = Vec3d(0.0, 0.0, 0.0);
|
||||
upVector = Vec3d(0.0, 1.0, 0.0);
|
||||
|
||||
fovy = 60.0;
|
||||
|
||||
vertices =
|
||||
{
|
||||
{ 2.0, 0.0, -2.0},
|
||||
{ 0.0, 2.0, -3.0},
|
||||
{-2.0, 0.0, -2.0},
|
||||
{ 0.0, -2.0, 1.0},
|
||||
};
|
||||
|
||||
indices = { {0, 1, 2}, {0, 2, 3} };
|
||||
|
||||
colors =
|
||||
{
|
||||
{ 0.0f, 0.0f, 1.0f},
|
||||
{ 0.0f, 1.0f, 0.0f},
|
||||
{ 1.0f, 0.0f, 0.0f},
|
||||
{ 0.0f, 1.0f, 0.0f},
|
||||
};
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
CV_Error(Error::StsBadArg, "Unknown model type");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
Vec3d position;
|
||||
Vec3d lookat;
|
||||
Vec3d upVector;
|
||||
|
||||
double fovy;
|
||||
|
||||
std::vector<Vec3f> vertices;
|
||||
std::vector<Vec3i> indices;
|
||||
std::vector<Vec3f> colors;
|
||||
};
|
||||
|
||||
|
||||
void compareDepth(const cv::Mat& gt, const cv::Mat& mat, cv::Mat& diff, double zFar, double scale,
|
||||
double maskThreshold, double normInfThreshold, double normL2Threshold)
|
||||
{
|
||||
ASSERT_EQ(CV_16UC1, gt.type());
|
||||
ASSERT_EQ(CV_16UC1, mat.type());
|
||||
ASSERT_EQ(gt.size(), mat.size());
|
||||
|
||||
Mat gtMask = gt < zFar*scale;
|
||||
Mat matMask = mat < zFar*scale;
|
||||
|
||||
Mat diffMask = gtMask != matMask;
|
||||
int nzDepthDiff = cv::countNonZero(diffMask);
|
||||
EXPECT_LE(nzDepthDiff, maskThreshold);
|
||||
|
||||
Mat jointMask = gtMask & matMask;
|
||||
int nzJointMask = cv::countNonZero(jointMask);
|
||||
double normInfDepth = cv::norm(gt, mat, cv::NORM_INF, jointMask);
|
||||
EXPECT_LE(normInfDepth, normInfThreshold);
|
||||
double normL2Depth = nzJointMask ? (cv::norm(gt, mat, cv::NORM_L2, jointMask) / nzJointMask) : 0;
|
||||
EXPECT_LE(normL2Depth, normL2Threshold);
|
||||
|
||||
// add --test_debug to output differences
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::cout << "nzDepthDiff: " << nzDepthDiff << " vs " << maskThreshold << std::endl;
|
||||
std::cout << "normInfDepth: " << normInfDepth << " vs " << normInfThreshold << std::endl;
|
||||
std::cout << "normL2Depth: " << normL2Depth << " vs " << normL2Threshold << std::endl;
|
||||
}
|
||||
|
||||
diff = (gt - mat) + (1 << 15);
|
||||
}
|
||||
|
||||
|
||||
void compareRGB(const cv::Mat& gt, const cv::Mat& mat, cv::Mat& diff, double normInfThreshold, double normL2Threshold)
|
||||
{
|
||||
ASSERT_EQ(CV_32FC3, gt.type());
|
||||
ASSERT_EQ(CV_32FC3, mat.type());
|
||||
ASSERT_EQ(gt.size(), mat.size());
|
||||
|
||||
double normInfRgb = cv::norm(gt, mat, cv::NORM_INF);
|
||||
EXPECT_LE(normInfRgb, normInfThreshold);
|
||||
double normL2Rgb = cv::norm(gt, mat, cv::NORM_L2) / gt.total();
|
||||
EXPECT_LE(normL2Rgb, normL2Threshold);
|
||||
// add --test_debug to output differences
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::cout << "normInfRgb: " << normInfRgb << " vs " << normInfThreshold << std::endl;
|
||||
std::cout << "normL2Rgb: " << normL2Rgb << " vs " << normL2Threshold << std::endl;
|
||||
}
|
||||
diff = (gt - mat) * 0.5 + 0.5;
|
||||
}
|
||||
|
||||
|
||||
struct RenderTestThresholds
|
||||
{
|
||||
RenderTestThresholds(
|
||||
double _rgbInfThreshold,
|
||||
double _rgbL2Threshold,
|
||||
double _depthMaskThreshold,
|
||||
double _depthInfThreshold,
|
||||
double _depthL2Threshold) :
|
||||
rgbInfThreshold(_rgbInfThreshold),
|
||||
rgbL2Threshold(_rgbL2Threshold),
|
||||
depthMaskThreshold(_depthMaskThreshold),
|
||||
depthInfThreshold(_depthInfThreshold),
|
||||
depthL2Threshold(_depthL2Threshold)
|
||||
{ }
|
||||
|
||||
double rgbInfThreshold;
|
||||
double rgbL2Threshold;
|
||||
double depthMaskThreshold;
|
||||
double depthInfThreshold;
|
||||
double depthL2Threshold;
|
||||
};
|
||||
|
||||
// resolution, shading type, culling mode, model type, float type, index type
|
||||
typedef std::tuple<std::tuple<int, int>, ShadingTypeEnum, CullingModeEnum, ModelTypeEnum, MatDepth, MatDepth> RenderTestParamType;
|
||||
|
||||
class RenderingTest : public ::testing::TestWithParam<RenderTestParamType>
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
params = GetParam();
|
||||
auto wh = std::get<0>(params);
|
||||
width = std::get<0>(wh);
|
||||
height = std::get<1>(wh);
|
||||
shadingType = std::get<1>(params);
|
||||
cullingMode = std::get<2>(params);
|
||||
modelType = std::get<3>(params);
|
||||
modelData = ModelData(modelType);
|
||||
ftype = std::get<4>(params);
|
||||
itype = std::get<5>(params);
|
||||
|
||||
zNear = 0.1, zFar = 50.0;
|
||||
depthScale = 1000.0;
|
||||
|
||||
depth_buf = Mat(height, width, ftype, zFar);
|
||||
color_buf = Mat(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
|
||||
|
||||
cameraPose = lookAtMatrixCal(modelData.position, modelData.lookat, modelData.upVector);
|
||||
fovYradians = modelData.fovy * (CV_PI / 180.0);
|
||||
|
||||
verts = Mat(modelData.vertices);
|
||||
verts.convertTo(verts, ftype);
|
||||
|
||||
if (shadingType != RASTERIZE_SHADING_WHITE)
|
||||
{
|
||||
// let vertices be in BGR format to avoid later color conversions
|
||||
// mixChannels() does not support in-place operation
|
||||
colors = Mat(modelData.colors);
|
||||
colors.convertTo(colors, ftype);
|
||||
cv::mixChannels(colors.clone(), colors, {0, 2, 1, 1, 2, 0});
|
||||
}
|
||||
|
||||
indices = Mat(modelData.indices);
|
||||
if (itype != CV_32S)
|
||||
{
|
||||
indices.convertTo(indices, itype);
|
||||
}
|
||||
|
||||
settings = TriangleRasterizeSettings().setCullingMode(cullingMode).setShadingType(shadingType);
|
||||
|
||||
triangleRasterize(verts, indices, colors, color_buf, depth_buf,
|
||||
cameraPose, fovYradians, zNear, zFar, settings);
|
||||
}
|
||||
|
||||
public:
|
||||
RenderTestParamType params;
|
||||
int width, height;
|
||||
double zNear, zFar, depthScale;
|
||||
|
||||
Mat depth_buf, color_buf;
|
||||
|
||||
Mat verts, colors, indices;
|
||||
Matx44d cameraPose;
|
||||
double fovYradians;
|
||||
TriangleRasterizeSettings settings;
|
||||
|
||||
ModelData modelData;
|
||||
ModelTypeEnum modelType;
|
||||
ShadingTypeEnum shadingType;
|
||||
CullingModeEnum cullingMode;
|
||||
int ftype, itype;
|
||||
};
|
||||
|
||||
|
||||
// depth-only or RGB-only rendering should produce the same result as usual rendering
|
||||
TEST_P(RenderingTest, noArrays)
|
||||
{
|
||||
Mat depthOnly(height, width, ftype, zFar);
|
||||
Mat colorOnly(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
|
||||
|
||||
triangleRasterizeDepth(verts, indices, depthOnly, cameraPose, fovYradians, zNear, zFar, settings);
|
||||
triangleRasterizeColor(verts, indices, colors, colorOnly, cameraPose, fovYradians, zNear, zFar, settings);
|
||||
|
||||
Mat rgbDiff, depthDiff;
|
||||
compareRGB(color_buf, colorOnly, rgbDiff, 0, 0);
|
||||
depth_buf.convertTo(depth_buf, CV_16U, depthScale);
|
||||
depthOnly.convertTo(depthOnly, CV_16U, depthScale);
|
||||
compareDepth(depth_buf, depthOnly, depthDiff, zFar, depthScale, 0, 0, 0);
|
||||
|
||||
// add --test_debug to output resulting images
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::string modelName = printEnum(modelType);
|
||||
std::string shadingName = printEnum(shadingType);
|
||||
std::string cullingName = printEnum(cullingMode);
|
||||
std::string suffix = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
|
||||
|
||||
std::string outColorPath = "noarray_color_image_" + suffix + "_" + shadingName + ".png";
|
||||
std::string outDepthPath = "noarray_depth_image_" + suffix + "_" + shadingName + ".png";
|
||||
|
||||
imwrite(outColorPath, color_buf * 255.f);
|
||||
imwrite(outDepthPath, depth_buf);
|
||||
imwrite("diff_" + outColorPath, rgbDiff * 255.f);
|
||||
imwrite("diff_" + outDepthPath, depthDiff);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// passing the same parameters in float should give the same result
|
||||
TEST_P(RenderingTest, floatParams)
|
||||
{
|
||||
Mat depth_buf2(height, width, ftype, zFar);
|
||||
Mat color_buf2(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
|
||||
|
||||
// cameraPose can also be float, checking it
|
||||
triangleRasterize(verts, indices, colors, color_buf2, depth_buf2,
|
||||
Matx44f(cameraPose), (float)fovYradians, (float)zNear, (float)zFar, settings);
|
||||
|
||||
RenderTestThresholds thr(0, 0, 0, 0, 0);
|
||||
switch (modelType)
|
||||
{
|
||||
case ModelType::Empty: break;
|
||||
case ModelType::Color: break;
|
||||
case ModelType::Clipping:
|
||||
if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.00127;
|
||||
}
|
||||
else if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.rgbInfThreshold = 3e-7;
|
||||
thr.rgbL2Threshold = 1.86e-10;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.000406;
|
||||
}
|
||||
else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.rgbInfThreshold = 2.39e-07;
|
||||
thr.rgbL2Threshold = 1.86e-10;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.0016;
|
||||
}
|
||||
else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.934;
|
||||
thr.rgbL2Threshold = 0.000102;
|
||||
thr.depthMaskThreshold = 21;
|
||||
}
|
||||
else if (width == 640 && height == 480 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.rgbL2Threshold = 1;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.000248;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.934;
|
||||
thr.rgbL2Threshold = 3.18e-5;
|
||||
thr.depthMaskThreshold = 114;
|
||||
}
|
||||
break;
|
||||
case ModelType::File:
|
||||
thr.depthInfThreshold = 1;
|
||||
if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.000229;
|
||||
thr.rgbL2Threshold = 6.37e-09;
|
||||
thr.depthL2Threshold = 0.00043;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.000277;
|
||||
thr.rgbL2Threshold = 1.8e-09;
|
||||
thr.depthL2Threshold = 0.000124;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.depthL2Threshold = 0.000124;
|
||||
}
|
||||
break;
|
||||
case ModelType::Centered:
|
||||
if (shadingType == RASTERIZE_SHADING_SHADED && cullingMode != RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.rgbInfThreshold = 3.58e-07;
|
||||
thr.rgbL2Threshold = 1.51e-10;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
Mat rgbDiff, depthDiff;
|
||||
compareRGB(color_buf, color_buf2, rgbDiff, thr.rgbInfThreshold, thr.rgbL2Threshold);
|
||||
depth_buf.convertTo(depth_buf, CV_16U, depthScale);
|
||||
depth_buf2.convertTo(depth_buf2, CV_16U, depthScale);
|
||||
compareDepth(depth_buf, depth_buf2, depthDiff, zFar, depthScale, thr.depthMaskThreshold, thr.depthInfThreshold, thr.depthL2Threshold);
|
||||
|
||||
// add --test_debug to output resulting images
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::string modelName = printEnum(modelType);
|
||||
std::string shadingName = printEnum(shadingType);
|
||||
std::string cullingName = printEnum(cullingMode);
|
||||
std::string suffix = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
|
||||
|
||||
std::string outColorPath = "float_color_image_" + suffix + "_" + shadingName + ".png";
|
||||
std::string outDepthPath = "float_depth_image_" + suffix + "_" + shadingName + ".png";
|
||||
|
||||
imwrite(outColorPath, color_buf * 255.f);
|
||||
imwrite(outDepthPath, depth_buf);
|
||||
imwrite("diff_" + outColorPath, rgbDiff * 255.f);
|
||||
imwrite("diff_" + outDepthPath, depthDiff);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// some culling options produce the same pictures, let's join them
|
||||
TriangleCullingMode findSameCulling(ModelType modelType, TriangleShadingType shadingType, TriangleCullingMode cullingMode, bool forRgb)
|
||||
{
|
||||
TriangleCullingMode sameCullingMode = cullingMode;
|
||||
|
||||
if ((modelType == ModelType::Centered && cullingMode == RASTERIZE_CULLING_CCW) ||
|
||||
(modelType == ModelType::Color && cullingMode == RASTERIZE_CULLING_CW) ||
|
||||
(modelType == ModelType::File && shadingType == RASTERIZE_SHADING_WHITE && forRgb) ||
|
||||
(modelType == ModelType::File && cullingMode == RASTERIZE_CULLING_CW))
|
||||
{
|
||||
sameCullingMode = RASTERIZE_CULLING_NONE;
|
||||
}
|
||||
|
||||
return sameCullingMode;
|
||||
}
|
||||
|
||||
// compare rendering results to the ones produced by samples/opengl/opengl_testdata_generator app
|
||||
TEST_P(RenderingTest, accuracy)
|
||||
{
|
||||
depth_buf.convertTo(depth_buf, CV_16U, depthScale);
|
||||
|
||||
if (modelType == ModelType::Empty ||
|
||||
(modelType == ModelType::Centered && cullingMode == RASTERIZE_CULLING_CW) ||
|
||||
(modelType == ModelType::Color && cullingMode == RASTERIZE_CULLING_CCW))
|
||||
{
|
||||
// empty image case
|
||||
EXPECT_EQ(0, cv::norm(color_buf, NORM_INF));
|
||||
|
||||
Mat depthDiff;
|
||||
absdiff(depth_buf, Scalar(zFar * depthScale), depthDiff);
|
||||
EXPECT_EQ(0, cv::norm(depthDiff, cv::NORM_INF));
|
||||
}
|
||||
else
|
||||
{
|
||||
RenderTestThresholds thr(0, 0, 0, 0, 0);
|
||||
switch (modelType)
|
||||
{
|
||||
case ModelType::Centered:
|
||||
if (shadingType == RASTERIZE_SHADING_SHADED)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.00218;
|
||||
thr.rgbL2Threshold = 2.85e-06;
|
||||
}
|
||||
break;
|
||||
case ModelType::Clipping:
|
||||
if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.00163;
|
||||
}
|
||||
else if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.934;
|
||||
thr.rgbL2Threshold = 8.03E-05;
|
||||
thr.depthMaskThreshold = 23;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.000555;
|
||||
}
|
||||
else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.0022;
|
||||
thr.rgbL2Threshold = 2.54E-06;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.00175;
|
||||
}
|
||||
else if (width == 256 && height == 256 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.934;
|
||||
thr.rgbL2Threshold = 0.000102;
|
||||
thr.depthMaskThreshold = 21;
|
||||
}
|
||||
else if (width == 640 && height == 480 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.rgbInfThreshold = 1;
|
||||
thr.rgbL2Threshold = 3.95E-05;
|
||||
thr.depthMaskThreshold = 49;
|
||||
thr.depthInfThreshold = 1;
|
||||
thr.depthL2Threshold = 0.000269;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_FLAT && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.934;
|
||||
thr.rgbL2Threshold = 3.27e-5;
|
||||
thr.depthMaskThreshold = 121;
|
||||
}
|
||||
break;
|
||||
case ModelType::Color:
|
||||
thr.depthInfThreshold = 1;
|
||||
if (width == 320 && height == 240)
|
||||
{
|
||||
thr.depthL2Threshold = 0.00103;
|
||||
}
|
||||
else if (width == 256 && height == 256)
|
||||
{
|
||||
thr.depthL2Threshold = 0.000785;
|
||||
}
|
||||
if (shadingType == RASTERIZE_SHADING_SHADED)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.0022;
|
||||
thr.rgbL2Threshold = 3.13e-06;
|
||||
}
|
||||
break;
|
||||
case ModelType::File:
|
||||
if (width == 320 && height == 240 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CCW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.836;
|
||||
thr.rgbL2Threshold = 2.08e-05;
|
||||
thr.depthMaskThreshold = 1;
|
||||
thr.depthInfThreshold = 99;
|
||||
thr.depthL2Threshold = 0.00544;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_SHADED && cullingMode == RASTERIZE_CULLING_CW)
|
||||
{
|
||||
thr.rgbInfThreshold = 0.973;
|
||||
thr.rgbL2Threshold = 5.2e-06;
|
||||
thr.depthMaskThreshold = 4;
|
||||
thr.depthInfThreshold = 258;
|
||||
thr.depthL2Threshold = 0.00228;
|
||||
}
|
||||
else if (width == 700 && height == 700 && shadingType == RASTERIZE_SHADING_WHITE && cullingMode == RASTERIZE_CULLING_NONE)
|
||||
{
|
||||
thr.rgbInfThreshold = 1;
|
||||
thr.rgbL2Threshold = 7.07e-06;
|
||||
thr.depthMaskThreshold = 4;
|
||||
thr.depthInfThreshold = 258;
|
||||
thr.depthL2Threshold = 0.00228;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
CullingModeEnum cullingModeRgb = findSameCulling(modelType, shadingType, cullingMode, true);
|
||||
CullingModeEnum cullingModeDepth = findSameCulling(modelType, shadingType, cullingMode, false);
|
||||
|
||||
std::string modelName = printEnum(modelType);
|
||||
std::string shadingName = printEnum(shadingType);
|
||||
std::string cullingName = printEnum(cullingMode);
|
||||
std::string cullingRgbName = printEnum(cullingModeRgb);
|
||||
std::string cullingDepthName = printEnum(cullingModeDepth);
|
||||
|
||||
std::string path = findDataDirectory("rendering");
|
||||
std::string suffix = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingName.c_str());
|
||||
std::string suffixRgb = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingRgbName.c_str());
|
||||
std::string suffixDepth = cv::format("%s_%dx%d_Cull%s", modelName.c_str(), width, height, cullingDepthName.c_str());
|
||||
std::string gtPathColor = path + "/example_image_" + suffixRgb + "_" + shadingName + ".png";
|
||||
std::string gtPathDepth = path + "/depth_image_" + suffixDepth + ".png";
|
||||
|
||||
Mat rgbDiff, depthDiff;
|
||||
Mat groundTruthColor = imread(gtPathColor);
|
||||
groundTruthColor.convertTo(groundTruthColor, CV_32F, (1.f / 255.f));
|
||||
compareRGB(groundTruthColor, color_buf, rgbDiff, thr.rgbInfThreshold, thr.rgbL2Threshold);
|
||||
|
||||
Mat groundTruthDepth = imread(gtPathDepth, cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH);
|
||||
compareDepth(groundTruthDepth, depth_buf, depthDiff, zFar, depthScale, thr.depthMaskThreshold, thr.depthInfThreshold, thr.depthL2Threshold);
|
||||
|
||||
// add --test_debug to output resulting images
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::string outColorPath = "color_image_" + suffix + "_" + shadingName + ".png";
|
||||
std::string outDepthPath = "depth_image_" + suffix + "_" + shadingName + ".png";
|
||||
imwrite(outColorPath, color_buf * 255.f);
|
||||
imwrite(outDepthPath, depth_buf);
|
||||
imwrite("diff_" + outColorPath, rgbDiff * 255.f);
|
||||
imwrite("diff_" + outDepthPath, depthDiff);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// drawing model as a whole or as two halves should give the same result
|
||||
TEST_P(RenderingTest, keepDrawnData)
|
||||
{
|
||||
if (modelType != ModelType::Empty)
|
||||
{
|
||||
Mat depth_buf2(height, width, ftype, zFar);
|
||||
Mat color_buf2(height, width, CV_MAKETYPE(ftype, 3), Scalar::all(0));
|
||||
|
||||
Mat idx1, idx2;
|
||||
int nTriangles = (int)indices.total();
|
||||
idx1 = indices.reshape(3, 1)(Range::all(), Range(0, nTriangles / 2));
|
||||
idx2 = indices.reshape(3, 1)(Range::all(), Range(nTriangles / 2, nTriangles));
|
||||
|
||||
triangleRasterize(verts, idx1, colors, color_buf2, depth_buf2, cameraPose, fovYradians, zNear, zFar, settings);
|
||||
triangleRasterize(verts, idx2, colors, color_buf2, depth_buf2, cameraPose, fovYradians, zNear, zFar, settings);
|
||||
|
||||
Mat rgbDiff, depthDiff;
|
||||
compareRGB(color_buf, color_buf2, rgbDiff, 0, 0);
|
||||
depth_buf.convertTo(depth_buf, CV_16U, depthScale);
|
||||
depth_buf2.convertTo(depth_buf2, CV_16U, depthScale);
|
||||
compareDepth(depth_buf, depth_buf2, depthDiff, zFar, depthScale, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST_P(RenderingTest, glCompatibleDepth)
|
||||
{
|
||||
Mat depth_buf2(height, width, ftype, 1.0);
|
||||
|
||||
triangleRasterizeDepth(verts, indices, depth_buf2, cameraPose, fovYradians, zNear, zFar,
|
||||
settings.setGlCompatibleMode(RASTERIZE_COMPAT_INVDEPTH));
|
||||
|
||||
Mat convertedDepth(height, width, ftype);
|
||||
// map from [0, 1] to [zNear, zFar]
|
||||
double scaleNear = (1.0 / zNear);
|
||||
double scaleFar = (1.0 / zFar);
|
||||
for (int y = 0; y < height; y++)
|
||||
{
|
||||
for (int x = 0; x < width; x++)
|
||||
{
|
||||
double z = (double)depth_buf2.at<float>(y, x);
|
||||
convertedDepth.at<float>(y, x) = (float)(1.0 / ((1.0 - z) * scaleNear + z * scaleFar ));
|
||||
}
|
||||
}
|
||||
|
||||
double normL2Diff = cv::norm(depth_buf, convertedDepth, cv::NORM_L2) / (height * width);
|
||||
const double normL2Threshold = 5.53e-10;
|
||||
EXPECT_LE(normL2Diff, normL2Threshold);
|
||||
// add --test_debug to output differences
|
||||
if (debugLevel > 0)
|
||||
{
|
||||
std::cout << "normL2Diff: " << normL2Diff << " vs " << normL2Threshold << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
INSTANTIATE_TEST_CASE_P(Rendering, RenderingTest, ::testing::Values(
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Centered, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Centered, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_WHITE, RASTERIZE_CULLING_NONE, ModelType::Centered, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(640, 480), RASTERIZE_SHADING_FLAT, RASTERIZE_CULLING_NONE, ModelType::Centered, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_FLAT, RASTERIZE_CULLING_CW, ModelType::Color, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Color, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Color, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_WHITE, RASTERIZE_CULLING_NONE, ModelType::Color, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_FLAT, RASTERIZE_CULLING_CW, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_NONE, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_FLAT, RASTERIZE_CULLING_CCW, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(256, 256), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CW, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(640, 480), RASTERIZE_SHADING_WHITE, RASTERIZE_CULLING_NONE, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_FLAT, RASTERIZE_CULLING_CCW, ModelType::Clipping, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(320, 240), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CCW, ModelType::File, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_SHADED, RASTERIZE_CULLING_CW, ModelType::File, CV_32F, CV_32S },
|
||||
RenderTestParamType { std::make_tuple(700, 700), RASTERIZE_SHADING_WHITE, RASTERIZE_CULLING_NONE, ModelType::File, CV_32F, CV_32S }
|
||||
));
|
||||
|
||||
} // namespace ::
|
||||
} // namespace opencv_test
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user