test_normal.cpp 14.3 KB
Newer Older
Vincent Rabaud's avatar
Vincent Rabaud committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35
/*
 * 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.
 *
 */

36
#include "test_precomp.hpp"
Vincent Rabaud's avatar
Vincent Rabaud committed
37
#include <opencv2/rgbd.hpp>
38
#include <opencv2/calib3d.hpp>
Vincent Rabaud's avatar
Vincent Rabaud committed
39

40
namespace opencv_test { namespace {
41

42 43 44
#if 0
Point3f
rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_<float>& Kinv)
45
{
46 47 48
  Matx33d dKinv(Kinv);
  Vec3d dNormal(normal);
  return rayPlaneIntersection(Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv);
49
}
50
#endif
Vincent Rabaud's avatar
Vincent Rabaud committed
51

52 53
Vec3f
rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, const Matx33d& Kinv)
Vincent Rabaud's avatar
Vincent Rabaud committed
54 55
{

56
  Matx31d L = Kinv * uv1; //a ray passing through camera optical center
Vincent Rabaud's avatar
Vincent Rabaud committed
57
  //and uv.
58
  L = L * (1.0 / cv::norm(L));
Vincent Rabaud's avatar
Vincent Rabaud committed
59 60 61 62 63 64 65 66 67 68
  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;
69
    std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl;
Vincent Rabaud's avatar
Vincent Rabaud committed
70
  }
71
  Vec3f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)));
Vincent Rabaud's avatar
Vincent Rabaud committed
72 73 74 75
  return xyz;
}
const int W = 640;
const int H = 480;
76
//int window_size = 5;
Vincent Rabaud's avatar
Vincent Rabaud committed
77 78 79 80
float focal_length = 525;
float cx = W / 2.f + 0.5f;
float cy = H / 2.f + 0.5f;

81 82
Mat K = (Mat_<double>(3, 3) << focal_length, 0, cx, 0, focal_length, cy, 0, 0, 1);
Mat Kinv = K.inv();
Vincent Rabaud's avatar
Vincent Rabaud committed
83

84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112
void points3dToDepth16U(const Mat_<Vec3f>& points3d, Mat& depthMap);

void points3dToDepth16U(const Mat_<Vec3f>& 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);

  int index = 0;
  for(int i = 0; i < H; i++)
  {

    for(int j = 0; j < W; j++)
    {
      float value = (points3d.at<Vec3f>(i, j))[2]; // value is the z
      depthMap.at<float>(cvRound(img_points[index].y), cvRound(img_points[index].x)) = value;
      index++;
    }
  }
  depthMap.convertTo(depthMap, CV_16U, 1000);
}

113
static RNG rng;
Vincent Rabaud's avatar
Vincent Rabaud committed
114 115 116
struct Plane
{

117
  Vec3d n, p;
Vincent Rabaud's avatar
Vincent Rabaud committed
118 119 120 121 122 123
  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);
124
    n = n / cv::norm(n);
Ilya Lavrenov's avatar
Ilya Lavrenov committed
125
    set_d((float)rng.uniform(-2.0, 0.6));
Vincent Rabaud's avatar
Vincent Rabaud committed
126 127 128 129 130
  }

  void
  set_d(float d)
  {
131
    p = Vec3d(0, 0, d / n[2]);
Vincent Rabaud's avatar
Vincent Rabaud committed
132 133 134
    p_dot_n = p.dot(n);
  }

135 136
  Vec3f
  intersection(float u, float v, const Matx33f& Kinv_in) const
Vincent Rabaud's avatar
Vincent Rabaud committed
137
  {
138
    return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
Vincent Rabaud's avatar
Vincent Rabaud committed
139 140 141 142
  }
};

void
143
gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals,
Vincent Rabaud's avatar
Vincent Rabaud committed
144 145 146 147 148 149 150 151 152 153 154 155
              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);
    }
  }
156 157
  Mat_ < Vec3f > outp(H, W);
  Mat_ < Vec3f > outn(H, W);
Vincent Rabaud's avatar
Vincent Rabaud committed
158 159 160 161 162 163 164 165 166 167 168
  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++)
    {
Ilya Lavrenov's avatar
Ilya Lavrenov committed
169
      unsigned int plane_index = (unsigned int)((u / float(W)) * planes.size());
Vincent Rabaud's avatar
Vincent Rabaud committed
170
      Plane plane = planes[plane_index];
Ilya Lavrenov's avatar
Ilya Lavrenov committed
171
      outp(v, u) = plane.intersection((float)u, (float)v, Kinv);
Vincent Rabaud's avatar
Vincent Rabaud committed
172
      outn(v, u) = plane.n;
Ilya Lavrenov's avatar
Ilya Lavrenov committed
173
      plane_mask(v, u) = (uchar)plane_index;
Vincent Rabaud's avatar
Vincent Rabaud committed
174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197
    }
  }
  planes_out = planes;
  points3d = outp;
  normals = outn;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

class CV_RgbdNormalsTest: public cvtest::BaseTest
{
public:
  CV_RgbdNormalsTest()
  {
  }
  ~CV_RgbdNormalsTest()
  {
  }
protected:
  void
  run(int)
  {
    try
    {
198
      Mat_<unsigned char> plane_mask;
Vincent Rabaud's avatar
Vincent Rabaud committed
199 200
      for (unsigned char i = 0; i < 3; ++i)
      {
201
        RgbdNormals::RGBD_NORMALS_METHOD method;
Vincent Rabaud's avatar
Vincent Rabaud committed
202 203 204
        // inner vector: whether it's 1 plane or 3 planes
        // outer vector: float or double
        std::vector<std::vector<float> > errors(2);
205 206
        errors[0].resize(4);
        errors[1].resize(4);
Vincent Rabaud's avatar
Vincent Rabaud committed
207 208 209
        switch (i)
        {
          case 0:
210
            method = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
Vincent Rabaud's avatar
Vincent Rabaud committed
211
            std::cout << std::endl << "*** FALS" << std::endl;
212 213 214 215
            errors[0][0] = 0.006f;
            errors[0][1] = 0.03f;
            errors[1][0] = 0.00008f;
            errors[1][1] = 0.02f;
Vincent Rabaud's avatar
Vincent Rabaud committed
216 217
            break;
          case 1:
218
            method = RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD;
Vincent Rabaud's avatar
Vincent Rabaud committed
219
            std::cout << std::endl << "*** LINEMOD" << std::endl;
220 221
            errors[0][0] = 0.04f;
            errors[0][1] = 0.07f;
222 223 224
            errors[0][2] = 0.04f; // depth 16U 1 plane
            errors[0][3] = 0.07f; // depth 16U 3 planes

225 226
            errors[1][0] = 0.05f;
            errors[1][1] = 0.08f;
227 228
            errors[1][2] = 0.05f; // depth 16U 1 plane
            errors[1][3] = 0.08f; // depth 16U 3 planes
Vincent Rabaud's avatar
Vincent Rabaud committed
229 230
            break;
          case 2:
231
            method = RgbdNormals::RGBD_NORMALS_METHOD_SRI;
Vincent Rabaud's avatar
Vincent Rabaud committed
232
            std::cout << std::endl << "*** SRI" << std::endl;
233 234 235 236
            errors[0][0] = 0.02f;
            errors[0][1] = 0.04f;
            errors[1][0] = 0.02f;
            errors[1][1] = 0.04f;
Vincent Rabaud's avatar
Vincent Rabaud committed
237
            break;
238 239 240
          default:
            method = (RgbdNormals::RGBD_NORMALS_METHOD)-1;
            CV_Error(0, "");
Vincent Rabaud's avatar
Vincent Rabaud committed
241 242 243 244 245 246 247 248 249 250
        }

        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;

251
          RgbdNormals normals_computer(H, W, depth, K, 5, method);
Vincent Rabaud's avatar
Vincent Rabaud committed
252 253 254
          normals_computer.initialize();

          std::vector<Plane> plane_params;
255
          Mat points3d, ground_normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
256
          // 1 plane, continuous scene, very low error..
257
          std::cout << "1 plane - input 3d points" << std::endl;
Vincent Rabaud's avatar
Vincent Rabaud committed
258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276
          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;
277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304

          if(method == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD)
          {
            // depth 16U test
            std::cout << "** depth 16U - 1 plane" << std::endl;
            err_mean = 0;
            for (int ii = 0; ii < 5; ++ii)
            {
              gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 1);
              Mat depthMap;
              points3dToDepth16U(points3d, depthMap);
              err_mean += testit(depthMap, ground_normals, normals_computer);
            }
            std::cout << "mean diff: " << (err_mean / 5) << std::endl;
            EXPECT_LE(err_mean/5, errors[j][2])<< " thresh: " << errors[j][2] << std::endl;

            std::cout << "** depth 16U - 3 plane" << std::endl;
            err_mean = 0;
            for (int ii = 0; ii < 5; ++ii)
            {
              gen_points_3d(plane_params, plane_mask, points3d, ground_normals, 3);
              Mat depthMap;
              points3dToDepth16U(points3d, depthMap);
              err_mean += testit(depthMap, ground_normals, normals_computer);
            }
            std::cout << "mean diff: " << (err_mean / 5) << std::endl;
            EXPECT_LE(err_mean/5, errors[j][3])<< "mean diff: " << (err_mean/5) << " thresh: " << errors[j][3] << std::endl;
          }
Vincent Rabaud's avatar
Vincent Rabaud committed
305 306 307 308 309 310 311 312 313 314 315 316 317
        }
      }

      //TODO test NaNs in data

    } catch (...)
    {
      ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
    }
    ts->set_failed_test_info(cvtest::TS::OK);
  }

  float
318
  testit(const Mat & points3d, const Mat & in_ground_normals, const RgbdNormals & normals_computer)
Vincent Rabaud's avatar
Vincent Rabaud committed
319
  {
320
    TickMeter tm;
Vincent Rabaud's avatar
Vincent Rabaud committed
321
    tm.start();
322
    Mat in_normals;
323
    if (normals_computer.getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && points3d.channels() == 3)
Vincent Rabaud's avatar
Vincent Rabaud committed
324
    {
325 326
      std::vector<Mat> channels;
      split(points3d, channels);
Vincent Rabaud's avatar
Vincent Rabaud committed
327 328 329 330 331 332
      normals_computer(channels[2], in_normals);
    }
    else
      normals_computer(points3d, in_normals);
    tm.stop();

333
    Mat_<Vec3f> normals, ground_normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
334 335 336 337 338 339 340
    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)
      {
341
        Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x);
342 343
        vec1 = vec1 / cv::norm(vec1);
        vec2 = vec2 / cv::norm(vec2);
Vincent Rabaud's avatar
Vincent Rabaud committed
344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373

        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
    {
374
      RgbdPlane plane_computer;
Vincent Rabaud's avatar
Vincent Rabaud committed
375 376

      std::vector<Plane> planes;
377 378
      Mat points3d, ground_normals;
      Mat_<unsigned char> plane_mask;
Vincent Rabaud's avatar
Vincent Rabaud committed
379 380 381 382 383 384 385 386 387 388 389 390 391 392 393
      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
394 395
  testit(const std::vector<Plane> & gt_planes, const Mat & gt_plane_mask, const Mat & points3d,
         RgbdPlane & plane_computer)
Vincent Rabaud's avatar
Vincent Rabaud committed
396 397 398
  {
    for (char i_test = 0; i_test < 2; ++i_test)
    {
399 400 401
      TickMeter tm1, tm2;
      Mat plane_mask;
      std::vector<Vec4f> plane_coefficients;
Vincent Rabaud's avatar
Vincent Rabaud committed
402 403 404 405 406 407

      if (i_test == 0)
      {
        tm1.start();
        // First, get the normals
        int depth = CV_32F;
408 409
        RgbdNormals normals_computer(H, W, depth, K, 5, RgbdNormals::RGBD_NORMALS_METHOD_FALS);
        Mat normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
410 411 412 413 414 415 416 417 418 419 420 421 422 423 424
        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
425 426
      int n_planes = (int)plane_coefficients.size();
      int n_gt_planes = (int)gt_planes.size();
427
      Mat_<int> matching(n_gt_planes, n_planes);
428
      for (int j = 0; j < n_gt_planes; ++j)
Vincent Rabaud's avatar
Vincent Rabaud committed
429
      {
430 431
        Mat gt_mask = gt_plane_mask == j;
        int n_gt = countNonZero(gt_mask);
Vincent Rabaud's avatar
Vincent Rabaud committed
432
        int n_max = 0, i_max = 0;
433
        for (int i = 0; i < n_planes; ++i)
Vincent Rabaud's avatar
Vincent Rabaud committed
434
        {
435 436 437
          Mat dst;
          bitwise_and(gt_mask, plane_mask == i, dst);
          matching(j, i) = countNonZero(dst);
Vincent Rabaud's avatar
Vincent Rabaud committed
438 439 440 441 442 443 444 445 446
          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
447
        Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
Vincent Rabaud's avatar
Vincent Rabaud committed
448 449 450 451 452 453 454 455 456 457 458 459 460
        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;
    }
  }
};

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

461
TEST(Rgbd_Normals, compute)
Vincent Rabaud's avatar
Vincent Rabaud committed
462
{
463
  CV_RgbdNormalsTest test;
Vincent Rabaud's avatar
Vincent Rabaud committed
464 465 466 467 468
  test.safe_run();
}

TEST(Rgbd_Plane, compute)
{
469
  CV_RgbdPlaneTest test;
Vincent Rabaud's avatar
Vincent Rabaud committed
470 471
  test.safe_run();
}
472 473

}} // namespace