test_normal.cpp 13 KB
Newer Older
1 2 3 4 5
// 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
Vincent Rabaud's avatar
Vincent Rabaud committed
6

7
#include "test_precomp.hpp"
Vincent Rabaud's avatar
Vincent Rabaud committed
8
#include <opencv2/rgbd.hpp>
9
#include <opencv2/calib3d.hpp>
Vincent Rabaud's avatar
Vincent Rabaud committed
10

11
namespace opencv_test { namespace {
12

13 14 15
#if 0
Point3f
rayPlaneIntersection(Point2f uv, const Mat& centroid, const Mat& normal, const Mat_<float>& Kinv)
16
{
17 18 19
  Matx33d dKinv(Kinv);
  Vec3d dNormal(normal);
  return rayPlaneIntersection(Vec3d(uv.x, uv.y, 1), centroid.dot(normal), dNormal, dKinv);
20
}
21
#endif
Vincent Rabaud's avatar
Vincent Rabaud committed
22

23 24
Vec3f
rayPlaneIntersection(const Vec3d& uv1, double centroid_dot_normal, const Vec3d& normal, const Matx33d& Kinv)
Vincent Rabaud's avatar
Vincent Rabaud committed
25 26
{

27
  Matx31d L = Kinv * uv1; //a ray passing through camera optical center
Vincent Rabaud's avatar
Vincent Rabaud committed
28
  //and uv.
29
  L = L * (1.0 / cv::norm(L));
Vincent Rabaud's avatar
Vincent Rabaud committed
30 31 32 33 34 35 36 37 38 39
  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;
40
    std::cout << "contents of L, Normal: " << Mat(L) << ", " << Mat(normal) << std::endl;
Vincent Rabaud's avatar
Vincent Rabaud committed
41
  }
42
  Vec3f xyz((float)(d * L(0)), (float)(d * L(1)), (float)(d * L(2)));
Vincent Rabaud's avatar
Vincent Rabaud committed
43 44 45 46
  return xyz;
}
const int W = 640;
const int H = 480;
47
//int window_size = 5;
Vincent Rabaud's avatar
Vincent Rabaud committed
48 49 50 51
float focal_length = 525;
float cx = W / 2.f + 0.5f;
float cy = H / 2.f + 0.5f;

52 53
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
54

55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
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);
}

84
static RNG rng;
Vincent Rabaud's avatar
Vincent Rabaud committed
85 86 87
struct Plane
{

88
  Vec3d n, p;
Vincent Rabaud's avatar
Vincent Rabaud committed
89 90 91 92 93 94
  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);
95
    n = n / cv::norm(n);
Ilya Lavrenov's avatar
Ilya Lavrenov committed
96
    set_d((float)rng.uniform(-2.0, 0.6));
Vincent Rabaud's avatar
Vincent Rabaud committed
97 98 99 100 101
  }

  void
  set_d(float d)
  {
102
    p = Vec3d(0, 0, d / n[2]);
Vincent Rabaud's avatar
Vincent Rabaud committed
103 104 105
    p_dot_n = p.dot(n);
  }

106 107
  Vec3f
  intersection(float u, float v, const Matx33f& Kinv_in) const
Vincent Rabaud's avatar
Vincent Rabaud committed
108
  {
109
    return rayPlaneIntersection(Vec3d(u, v, 1), p_dot_n, n, Kinv_in);
Vincent Rabaud's avatar
Vincent Rabaud committed
110 111 112 113
  }
};

void
114
gen_points_3d(std::vector<Plane>& planes_out, Mat_<unsigned char> &plane_mask, Mat& points3d, Mat& normals,
Vincent Rabaud's avatar
Vincent Rabaud committed
115 116 117 118 119 120 121 122 123 124 125 126
              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);
    }
  }
127 128
  Mat_ < Vec3f > outp(H, W);
  Mat_ < Vec3f > outn(H, W);
Vincent Rabaud's avatar
Vincent Rabaud committed
129 130 131 132 133 134 135 136 137 138 139
  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
140
      unsigned int plane_index = (unsigned int)((u / float(W)) * planes.size());
Vincent Rabaud's avatar
Vincent Rabaud committed
141
      Plane plane = planes[plane_index];
Ilya Lavrenov's avatar
Ilya Lavrenov committed
142
      outp(v, u) = plane.intersection((float)u, (float)v, Kinv);
Vincent Rabaud's avatar
Vincent Rabaud committed
143
      outn(v, u) = plane.n;
Ilya Lavrenov's avatar
Ilya Lavrenov committed
144
      plane_mask(v, u) = (uchar)plane_index;
Vincent Rabaud's avatar
Vincent Rabaud committed
145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168
    }
  }
  planes_out = planes;
  points3d = outp;
  normals = outn;
}

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

class CV_RgbdNormalsTest: public cvtest::BaseTest
{
public:
  CV_RgbdNormalsTest()
  {
  }
  ~CV_RgbdNormalsTest()
  {
  }
protected:
  void
  run(int)
  {
    try
    {
169
      Mat_<unsigned char> plane_mask;
Vincent Rabaud's avatar
Vincent Rabaud committed
170 171
      for (unsigned char i = 0; i < 3; ++i)
      {
172
        RgbdNormals::RGBD_NORMALS_METHOD method;
Vincent Rabaud's avatar
Vincent Rabaud committed
173 174 175
        // inner vector: whether it's 1 plane or 3 planes
        // outer vector: float or double
        std::vector<std::vector<float> > errors(2);
176 177
        errors[0].resize(4);
        errors[1].resize(4);
Vincent Rabaud's avatar
Vincent Rabaud committed
178 179 180
        switch (i)
        {
          case 0:
181
            method = RgbdNormals::RGBD_NORMALS_METHOD_FALS;
Vincent Rabaud's avatar
Vincent Rabaud committed
182
            std::cout << std::endl << "*** FALS" << std::endl;
183 184
            errors[0][0] = 0.006f;
            errors[0][1] = 0.03f;
185
            errors[1][0] = 0.0001f;
186
            errors[1][1] = 0.02f;
Vincent Rabaud's avatar
Vincent Rabaud committed
187 188
            break;
          case 1:
189
            method = RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD;
Vincent Rabaud's avatar
Vincent Rabaud committed
190
            std::cout << std::endl << "*** LINEMOD" << std::endl;
191 192
            errors[0][0] = 0.04f;
            errors[0][1] = 0.07f;
193 194 195
            errors[0][2] = 0.04f; // depth 16U 1 plane
            errors[0][3] = 0.07f; // depth 16U 3 planes

196 197
            errors[1][0] = 0.05f;
            errors[1][1] = 0.08f;
198 199
            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
200 201
            break;
          case 2:
202
            method = RgbdNormals::RGBD_NORMALS_METHOD_SRI;
Vincent Rabaud's avatar
Vincent Rabaud committed
203
            std::cout << std::endl << "*** SRI" << std::endl;
204 205 206 207
            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
208
            break;
209 210 211
          default:
            method = (RgbdNormals::RGBD_NORMALS_METHOD)-1;
            CV_Error(0, "");
Vincent Rabaud's avatar
Vincent Rabaud committed
212 213 214 215 216 217 218 219 220 221
        }

        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;

222
          RgbdNormals normals_computer(H, W, depth, K, 5, method);
Vincent Rabaud's avatar
Vincent Rabaud committed
223 224 225
          normals_computer.initialize();

          std::vector<Plane> plane_params;
226
          Mat points3d, ground_normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
227
          // 1 plane, continuous scene, very low error..
228
          std::cout << "1 plane - input 3d points" << std::endl;
Vincent Rabaud's avatar
Vincent Rabaud committed
229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247
          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;
248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275

          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
276 277 278 279 280 281 282 283 284 285 286 287 288
        }
      }

      //TODO test NaNs in data

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

  float
289
  testit(const Mat & points3d, const Mat & in_ground_normals, const RgbdNormals & normals_computer)
Vincent Rabaud's avatar
Vincent Rabaud committed
290
  {
291
    TickMeter tm;
Vincent Rabaud's avatar
Vincent Rabaud committed
292
    tm.start();
293
    Mat in_normals;
294
    if (normals_computer.getMethod() == RgbdNormals::RGBD_NORMALS_METHOD_LINEMOD && points3d.channels() == 3)
Vincent Rabaud's avatar
Vincent Rabaud committed
295
    {
296 297
      std::vector<Mat> channels;
      split(points3d, channels);
Vincent Rabaud's avatar
Vincent Rabaud committed
298 299 300 301 302 303
      normals_computer(channels[2], in_normals);
    }
    else
      normals_computer(points3d, in_normals);
    tm.stop();

304
    Mat_<Vec3f> normals, ground_normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
305 306 307 308 309 310 311
    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)
      {
312
        Vec3f vec1 = normals(y, x), vec2 = ground_normals(y, x);
313 314
        vec1 = vec1 / cv::norm(vec1);
        vec2 = vec2 / cv::norm(vec2);
Vincent Rabaud's avatar
Vincent Rabaud committed
315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344

        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
    {
345
      RgbdPlane plane_computer;
Vincent Rabaud's avatar
Vincent Rabaud committed
346 347

      std::vector<Plane> planes;
348 349
      Mat points3d, ground_normals;
      Mat_<unsigned char> plane_mask;
Vincent Rabaud's avatar
Vincent Rabaud committed
350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
      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
365 366
  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
367 368 369
  {
    for (char i_test = 0; i_test < 2; ++i_test)
    {
370 371 372
      TickMeter tm1, tm2;
      Mat plane_mask;
      std::vector<Vec4f> plane_coefficients;
Vincent Rabaud's avatar
Vincent Rabaud committed
373 374 375 376 377 378

      if (i_test == 0)
      {
        tm1.start();
        // First, get the normals
        int depth = CV_32F;
379 380
        RgbdNormals normals_computer(H, W, depth, K, 5, RgbdNormals::RGBD_NORMALS_METHOD_FALS);
        Mat normals;
Vincent Rabaud's avatar
Vincent Rabaud committed
381 382 383 384 385 386 387 388 389 390 391 392 393 394 395
        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
396 397
      int n_planes = (int)plane_coefficients.size();
      int n_gt_planes = (int)gt_planes.size();
398
      Mat_<int> matching(n_gt_planes, n_planes);
399
      for (int j = 0; j < n_gt_planes; ++j)
Vincent Rabaud's avatar
Vincent Rabaud committed
400
      {
401 402
        Mat gt_mask = gt_plane_mask == j;
        int n_gt = countNonZero(gt_mask);
Vincent Rabaud's avatar
Vincent Rabaud committed
403
        int n_max = 0, i_max = 0;
404
        for (int i = 0; i < n_planes; ++i)
Vincent Rabaud's avatar
Vincent Rabaud committed
405
        {
406 407 408
          Mat dst;
          bitwise_and(gt_mask, plane_mask == i, dst);
          matching(j, i) = countNonZero(dst);
Vincent Rabaud's avatar
Vincent Rabaud committed
409 410 411 412 413 414 415 416 417
          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
418
        Vec3d normal(plane_coefficients[i_max][0], plane_coefficients[i_max][1], plane_coefficients[i_max][2]);
Vincent Rabaud's avatar
Vincent Rabaud committed
419 420 421 422 423 424 425 426 427 428 429 430 431
        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;
    }
  }
};

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

432
TEST(Rgbd_Normals, compute)
Vincent Rabaud's avatar
Vincent Rabaud committed
433
{
434
  CV_RgbdNormalsTest test;
Vincent Rabaud's avatar
Vincent Rabaud committed
435 436 437 438 439
  test.safe_run();
}

TEST(Rgbd_Plane, compute)
{
440
  CV_RgbdPlaneTest test;
Vincent Rabaud's avatar
Vincent Rabaud committed
441 442
  test.safe_run();
}
443 444

}} // namespace