You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
463 lines
13 KiB
463 lines
13 KiB
/* |
|
* Software License Agreement (BSD License) |
|
* |
|
* Copyright (c) 2012, Willow Garage, Inc. |
|
* All rights reserved. |
|
* |
|
* Redistribution and use in source and binary forms, with or without |
|
* modification, are permitted provided that the following conditions |
|
* are met: |
|
* |
|
* * Redistributions of source code must retain the above copyright |
|
* notice, this list of conditions and the following disclaimer. |
|
* * Redistributions in binary form must reproduce the above |
|
* copyright notice, this list of conditions and the following |
|
* disclaimer in the documentation and/or other materials provided |
|
* with the distribution. |
|
* * Neither the name of Willow Garage, Inc. nor the names of its |
|
* contributors may be used to endorse or promote products derived |
|
* from this software without specific prior written permission. |
|
* |
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
|
* POSSIBILITY OF SUCH DAMAGE. |
|
* |
|
*/ |
|
|
|
#include "test_precomp.hpp" |
|
#include <opencv2/rgbd.hpp> |
|
|
|
namespace cv |
|
{ |
|
namespace rgbd |
|
{ |
|
|
|
class CV_EXPORTS TickMeter |
|
{ |
|
public: |
|
TickMeter(); |
|
void start(); |
|
void stop(); |
|
|
|
int64 getTimeTicks() const; |
|
double getTimeMicro() const; |
|
double getTimeMilli() const; |
|
double getTimeSec() const; |
|
int64 getCounter() const; |
|
|
|
void reset(); |
|
private: |
|
int64 counter; |
|
int64 sumTime; |
|
int64 startTime; |
|
}; |
|
|
|
TickMeter::TickMeter() { reset(); } |
|
int64 TickMeter::getTimeTicks() const { return sumTime; } |
|
double TickMeter::getTimeSec() const { return (double)getTimeTicks()/getTickFrequency(); } |
|
double TickMeter::getTimeMilli() const { return getTimeSec()*1e3; } |
|
double TickMeter::getTimeMicro() const { return getTimeMilli()*1e3; } |
|
int64 TickMeter::getCounter() const { return counter; } |
|
void TickMeter::reset() {startTime = 0; sumTime = 0; counter = 0; } |
|
|
|
void TickMeter::start(){ startTime = getTickCount(); } |
|
void TickMeter::stop() |
|
{ |
|
int64 time = getTickCount(); |
|
if ( startTime == 0 ) |
|
return; |
|
|
|
++counter; |
|
|
|
sumTime += ( time - startTime ); |
|
startTime = 0; |
|
} |
|
|
|
Point3f |
|
rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_<float>& Kinv); |
|
|
|
Vec3f |
|
rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, |
|
const Matx33d& Kinv); |
|
Vec3f |
|
rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, const Matx33d& Kinv) |
|
{ |
|
|
|
Matx31d L = Kinv * uv1; //a ray passing through camera optical center |
|
//and uv. |
|
L = L * (1.0 / norm(L)); |
|
double LdotNormal = L.dot(normal); |
|
double d; |
|
if (std::fabs(LdotNormal) > 1e-9) |
|
{ |
|
d = centroid_dot_normal / LdotNormal; |
|
} |
|
else |
|
{ |
|
d = 1.0; |
|
std::cout << "warning, LdotNormal nearly 0! " << LdotNormal << std::endl; |
|
std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl; |
|
} |
|
Vec3f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2))); |
|
return xyz; |
|
} |
|
|
|
Point3f |
|
rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_<float>& Kinv) |
|
{ |
|
Matx33d dKinv(Kinv); |
|
Vec3d dNormal(normal); |
|
return rayPlaneIntersection(Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv); |
|
} |
|
|
|
const int W = 640; |
|
const int H = 480; |
|
int window_size = 5; |
|
float focal_length = 525; |
|
float cx = W / 2.f + 0.5f; |
|
float cy = H / 2.f + 0.5f; |
|
|
|
Mat K = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1); |
|
Mat Kinv = K.inv(); |
|
|
|
static RNG rng; |
|
struct Plane |
|
{ |
|
|
|
Vec3d n, p; |
|
double p_dot_n; |
|
Plane() |
|
{ |
|
n[0] = rng.uniform(-0.5, 0.5); |
|
n[1] = rng.uniform(-0.5, 0.5); |
|
n[2] = -0.3; //rng.uniform(-1.f, 0.5f); |
|
n = n / norm(n); |
|
set_d((float)rng.uniform(-2.0, 0.6)); |
|
} |
|
|
|
void |
|
set_d(float d) |
|
{ |
|
p = Vec3d(0, 0, d / n[2]); |
|
p_dot_n = p.dot(n); |
|
} |
|
|
|
Vec3f |
|
intersection(float u, float v, const Matx33f& Kinv_in) const |
|
{ |
|
return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in); |
|
} |
|
}; |
|
|
|
void |
|
gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals, |
|
int n_planes); |
|
void |
|
gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals, |
|
int n_planes) |
|
{ |
|
std::vector<Plane> planes; |
|
for (int i = 0; i < n_planes; i++) |
|
{ |
|
Plane px; |
|
for (int j = 0; j < 1; j++) |
|
{ |
|
px.set_d(rng.uniform(-3.f, -0.5f)); |
|
planes.push_back(px); |
|
} |
|
} |
|
Mat_ < Vec3f > outp(H, W); |
|
Mat_ < Vec3f > 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]; |
|
outp(v, u) = plane.intersection((float)u, (float)v, Kinv); |
|
outn(v, u) = plane.n; |
|
plane_mask(v, u) = (uchar)plane_index; |
|
} |
|
} |
|
planes_out = planes; |
|
points3d = outp; |
|
normals = outn; |
|
} |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
class CV_RgbdNormalsTest: public cvtest::BaseTest |
|
{ |
|
public: |
|
CV_RgbdNormalsTest() |
|
{ |
|
} |
|
~CV_RgbdNormalsTest() |
|
{ |
|
} |
|
protected: |
|
void |
|
run(int) |
|
{ |
|
try |
|
{ |
|
Mat_<unsigned char> plane_mask; |
|
for (unsigned char i = 0; i < 3; ++i) |
|
{ |
|
RgbdNormals::RGBD_NORMALS_METHOD method; |
|
// inner vector: whether it's 1 plane or 3 planes |
|
// outer vector: float or double |
|
std::vector<std::vector<float> > errors(2); |
|
errors[0].resize(2); |
|
errors[1].resize(2); |
|
switch (i) |
|
{ |
|
case 0: |
|
method = RgbdNormals::RGBD_NORMALS_METHOD_FALS; |
|
std::cout << std::endl << "*** FALS" << std::endl; |
|
errors[0][0] = 0.006f; |
|
errors[0][1] = 0.03f; |
|
errors[1][0] = 0.00008f; |
|
errors[1][1] = 0.02f; |
|
break; |
|
case 1: |
|
method = RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD; |
|
std::cout << std::endl << "*** LINEMOD" << std::endl; |
|
errors[0][0] = 0.04f; |
|
errors[0][1] = 0.07f; |
|
errors[1][0] = 0.05f; |
|
errors[1][1] = 0.08f; |
|
break; |
|
case 2: |
|
method = RgbdNormals::RGBD_NORMALS_METHOD_SRI; |
|
std::cout << std::endl << "*** SRI" << std::endl; |
|
errors[0][0] = 0.02f; |
|
errors[0][1] = 0.04f; |
|
errors[1][0] = 0.02f; |
|
errors[1][1] = 0.04f; |
|
break; |
|
default: |
|
method = (RgbdNormals::RGBD_NORMALS_METHOD)-1; |
|
CV_Error(0, ""); |
|
} |
|
|
|
for (unsigned char j = 0; j < 2; ++j) |
|
{ |
|
int depth = (j % 2 == 0) ? CV_32F : CV_64F; |
|
if (depth == CV_32F) |
|
std::cout << "* float" << std::endl; |
|
else |
|
std::cout << "* double" << std::endl; |
|
|
|
RgbdNormals normals_computer(H, W, depth, K, 5, method); |
|
normals_computer.initialize(); |
|
|
|
std::vector<Plane> plane_params; |
|
Mat points3d, ground_normals; |
|
// 1 plane, continuous scene, very low error.. |
|
std::cout << "1 plane" << std::endl; |
|
float err_mean = 0; |
|
for (int ii = 0; ii < 5; ++ii) |
|
{ |
|
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1); |
|
err_mean += testit(points3d, ground_normals, normals_computer); |
|
} |
|
std::cout << "mean diff: " << (err_mean / 5) << std::endl; |
|
EXPECT_LE(err_mean/5, errors[j][0])<< " thresh: " << errors[j][0] << std::endl; |
|
|
|
// 3 discontinuities, more error expected. |
|
std::cout << "3 planes" << std::endl; |
|
err_mean = 0; |
|
for (int ii = 0; ii < 5; ++ii) |
|
{ |
|
gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3); |
|
err_mean += testit(points3d, ground_normals, normals_computer); |
|
} |
|
std::cout << "mean diff: " << (err_mean / 5) << std::endl; |
|
EXPECT_LE(err_mean/5, errors[j][1])<< "mean diff: " << (err_mean/5) << " thresh: " << errors[j][1] << std::endl; |
|
} |
|
} |
|
|
|
//TODO test NaNs in data |
|
|
|
} catch (...) |
|
{ |
|
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); |
|
} |
|
ts->set_failed_test_info(cvtest::TS::OK); |
|
} |
|
|
|
float |
|
testit(const Mat & points3d, const Mat & in_ground_normals, const RgbdNormals & normals_computer) |
|
{ |
|
TickMeter tm; |
|
tm.start(); |
|
Mat in_normals; |
|
if (normals_computer.getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD) |
|
{ |
|
std::vector<Mat> channels; |
|
split(points3d, channels); |
|
normals_computer(channels[2], in_normals); |
|
} |
|
else |
|
normals_computer(points3d, in_normals); |
|
tm.stop(); |
|
|
|
Mat_<Vec3f> normals, ground_normals; |
|
in_normals.convertTo(normals, CV_32FC3); |
|
in_ground_normals.convertTo(ground_normals, CV_32FC3); |
|
|
|
float err = 0; |
|
for (int y = 0; y < normals.rows; ++y) |
|
for (int x = 0; x < normals.cols; ++x) |
|
{ |
|
Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x); |
|
vec1 = vec1 / norm(vec1); |
|
vec2 = vec2 / norm(vec2); |
|
|
|
float dot = vec1.dot(vec2); |
|
// Just for rounding errors |
|
if (std::abs(dot) < 1) |
|
err += std::min(std::acos(dot), std::acos(-dot)); |
|
} |
|
|
|
err /= normals.rows * normals.cols; |
|
std::cout << "Average error: " << err << " Speed: " << tm.getTimeMilli() << " ms" << std::endl; |
|
return err; |
|
} |
|
}; |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
class CV_RgbdPlaneTest: public cvtest::BaseTest |
|
{ |
|
public: |
|
CV_RgbdPlaneTest() |
|
{ |
|
} |
|
~CV_RgbdPlaneTest() |
|
{ |
|
} |
|
protected: |
|
void |
|
run(int) |
|
{ |
|
try |
|
{ |
|
RgbdPlane plane_computer; |
|
|
|
std::vector<Plane> planes; |
|
Mat points3d, ground_normals; |
|
Mat_<unsigned char> plane_mask; |
|
gen_points_3d(planes, plane_mask, points3d, ground_normals, 1); |
|
testit(planes, plane_mask, points3d, plane_computer); // 1 plane, continuous scene, very low error.. |
|
for (int ii = 0; ii < 10; ii++) |
|
{ |
|
gen_points_3d(planes, plane_mask, points3d, ground_normals, 3); //three planes |
|
testit(planes, plane_mask, points3d, plane_computer); // 3 discontinuities, more error expected. |
|
} |
|
} catch (...) |
|
{ |
|
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH); |
|
} |
|
ts->set_failed_test_info(cvtest::TS::OK); |
|
} |
|
|
|
void |
|
testit(const std::vector<Plane> & gt_planes, const Mat & gt_plane_mask, const Mat & points3d, |
|
RgbdPlane & plane_computer) |
|
{ |
|
for (char i_test = 0; i_test < 2; ++i_test) |
|
{ |
|
TickMeter tm1, tm2; |
|
Mat plane_mask; |
|
std::vector<Vec4f> plane_coefficients; |
|
|
|
if (i_test == 0) |
|
{ |
|
tm1.start(); |
|
// First, get the normals |
|
int depth = CV_32F; |
|
RgbdNormals normals_computer(H, W, depth, K, 5, RgbdNormals::RGBD_NORMALS_METHOD_FALS); |
|
Mat normals; |
|
normals_computer(points3d, normals); |
|
tm1.stop(); |
|
|
|
tm2.start(); |
|
plane_computer(points3d, normals, plane_mask, plane_coefficients); |
|
tm2.stop(); |
|
} |
|
else |
|
{ |
|
tm2.start(); |
|
plane_computer(points3d, plane_mask, plane_coefficients); |
|
tm2.stop(); |
|
} |
|
|
|
// Compare each found plane to each ground truth plane |
|
int n_planes = (int)plane_coefficients.size(); |
|
int n_gt_planes = (int)gt_planes.size(); |
|
Mat_<int> matching(n_gt_planes, n_planes); |
|
for (int j = 0; j < n_gt_planes; ++j) |
|
{ |
|
Mat gt_mask = gt_plane_mask == j; |
|
int n_gt = countNonZero(gt_mask); |
|
int n_max = 0, i_max = 0; |
|
for (int i = 0; i < n_planes; ++i) |
|
{ |
|
Mat dst; |
|
bitwise_and(gt_mask, plane_mask == i, dst); |
|
matching(j, i) = countNonZero(dst); |
|
if (matching(j, i) > n_max) |
|
{ |
|
n_max = matching(j, i); |
|
i_max = i; |
|
} |
|
} |
|
// Get the best match |
|
ASSERT_LE(float(n_max - n_gt) / n_gt, 0.001); |
|
// Compare the normals |
|
Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]); |
|
ASSERT_GE(std::abs(gt_planes[j].n.dot(normal)), 0.95); |
|
} |
|
|
|
std::cout << " Speed: "; |
|
if (i_test == 0) |
|
std::cout << "normals " << tm1.getTimeMilli() << " ms and "; |
|
std::cout << "plane " << tm2.getTimeMilli() << " ms " << std::endl; |
|
} |
|
} |
|
}; |
|
|
|
} |
|
} |
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|
|
|
TEST(DISABLED_Rgbd_Normals, compute) |
|
{ |
|
cv::rgbd::CV_RgbdNormalsTest test; |
|
test.safe_run(); |
|
} |
|
|
|
TEST(Rgbd_Plane, compute) |
|
{ |
|
cv::rgbd::CV_RgbdPlaneTest test; |
|
test.safe_run(); |
|
}
|
|
|