ppf_match_3d.cpp 16.8 KB
Newer Older
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 36 37 38 39 40 41 42 43
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                          License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2014, OpenCV Foundation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
// Author: Tolga Birdal <tbirdal AT gmail.com>

#include "precomp.hpp"
#include "hash_murmur.hpp"

44
namespace cv
45 46 47
{
namespace ppf_match_3d
{
48 49 50

static const size_t PPF_LENGTH = 5;

51
// routines for assisting sort
52
static bool pose3DPtrCompare(const Pose3DPtr& a, const Pose3DPtr& b)
53
{
54 55
  CV_Assert(!a.empty() && !b.empty());
  return ( a->numVotes > b->numVotes );
56 57
}

58
static int sortPoseClusters(const PoseCluster3DPtr& a, const PoseCluster3DPtr& b)
59
{
60
  CV_Assert(!a.empty() && !b.empty());
Bence Magyar's avatar
Bence Magyar committed
61
  return ( a->numVotes > b->numVotes );
62 63 64
}

// simple hashing
65
/*static int hashPPFSimple(const Vec4d& f, const double AngleStep, const double DistanceStep)
66
{
67 68 69 70 71
  Vec4i key(
      (int)(f[0] / AngleStep),
      (int)(f[1] / AngleStep),
      (int)(f[2] / AngleStep),
      (int)(f[3] / DistanceStep));
Bence Magyar's avatar
Bence Magyar committed
72

73
  int hashKey = d.val[0] | (d.val[1] << 8) | (d.val[2] << 16) | (d.val[3] << 24);
Bence Magyar's avatar
Bence Magyar committed
74
  return hashKey;
75 76 77
}*/

// quantize ppf and hash it for proper indexing
78
static KeyType hashPPF(const Vec4d& f, const double AngleStep, const double DistanceStep)
79
{
80 81 82 83 84 85 86 87
  Vec4i key(
      (int)(f[0] / AngleStep),
      (int)(f[1] / AngleStep),
      (int)(f[2] / AngleStep),
      (int)(f[3] / DistanceStep));
  KeyType hashKey = 0;

  murmurHash(key.val, 4*sizeof(int), 42, &hashKey);
Bence Magyar's avatar
Bence Magyar committed
88
  return hashKey;
89 90
}

91
/*static size_t hashMurmur(uint key)
92
{
Bence Magyar's avatar
Bence Magyar committed
93 94 95
  size_t hashKey=0;
  hashMurmurx86((void*)&key, 4, 42, &hashKey);
  return hashKey;
96 97
}*/

98
static double computeAlpha(const Vec3d& p1, const Vec3d& n1, const Vec3d& p2)
99
{
100 101 102
  Vec3d Tmg, mpt;
  Matx33d R;
  double alpha;
Bence Magyar's avatar
Bence Magyar committed
103

104 105
  computeTransformRT(p1, n1, R, Tmg);
  mpt = Tmg + R * p2;
Bence Magyar's avatar
Bence Magyar committed
106 107 108 109 110 111 112 113 114 115 116
  alpha=atan2(-mpt[2], mpt[1]);

  if ( alpha != alpha)
  {
    return 0;
  }

  if (sin(alpha)*mpt[2]<0.0)
    alpha=-alpha;

  return (-alpha);
117 118 119 120
}

PPF3DDetector::PPF3DDetector()
{
121 122 123 124 125 126
  sampling_step_relative = 0.05;
  distance_step_relative = 0.05;
  scene_sample_step = (int)(1/0.04);
  angle_step_relative = 30;
  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
  angle_step = angle_step_radians;
Bence Magyar's avatar
Bence Magyar committed
127 128
  trained = false;

129 130 131
  hash_table = NULL;
  hash_nodes = NULL;

Bence Magyar's avatar
Bence Magyar committed
132
  setSearchParams();
133 134 135 136
}

PPF3DDetector::PPF3DDetector(const double RelativeSamplingStep, const double RelativeDistanceStep, const double NumAngles)
{
137 138 139 140
  sampling_step_relative = RelativeSamplingStep;
  distance_step_relative = RelativeDistanceStep;
  angle_step_relative = NumAngles;
  angle_step_radians = (360.0/angle_step_relative)*M_PI/180.0;
Bence Magyar's avatar
Bence Magyar committed
141
  //SceneSampleStep = 1.0/RelativeSceneSampleStep;
142
  angle_step = angle_step_radians;
Bence Magyar's avatar
Bence Magyar committed
143 144
  trained = false;

145 146 147
  hash_table = NULL;
  hash_nodes = NULL;

Bence Magyar's avatar
Bence Magyar committed
148
  setSearchParams();
149 150
}

151
void PPF3DDetector::setSearchParams(const double positionThreshold, const double rotationThreshold, const bool useWeightedClustering)
152
{
Bence Magyar's avatar
Bence Magyar committed
153
  if (positionThreshold<0)
154
    position_threshold = sampling_step_relative;
Bence Magyar's avatar
Bence Magyar committed
155
  else
156
    position_threshold = positionThreshold;
Bence Magyar's avatar
Bence Magyar committed
157 158

  if (rotationThreshold<0)
159
    rotation_threshold = ((360/angle_step) / 180.0 * M_PI);
Bence Magyar's avatar
Bence Magyar committed
160
  else
161
    rotation_threshold = rotationThreshold;
Bence Magyar's avatar
Bence Magyar committed
162

163
  use_weighted_avg = useWeightedClustering;
164 165 166
}

// compute per point PPF as in paper
167 168 169
void PPF3DDetector::computePPFFeatures(const Vec3d& p1, const Vec3d& n1,
                                       const Vec3d& p2, const Vec3d& n2,
                                       Vec4d& f)
170
{
171 172 173 174 175
  Vec3d d(p2 - p1);
  f[3] = cv::norm(d);
  if (f[3] <= EPS)
    return;
  d *= 1.0 / f[3];
Bence Magyar's avatar
Bence Magyar committed
176

177 178 179
  f[0] = TAngle3Normalized(n1, d);
  f[1] = TAngle3Normalized(n2, d);
  f[2] = TAngle3Normalized(n1, n2);
180 181 182 183
}

void PPF3DDetector::clearTrainingModels()
{
Bence Magyar's avatar
Bence Magyar committed
184 185 186 187 188 189 190 191 192 193 194
  if (this->hash_nodes)
  {
    free(this->hash_nodes);
    this->hash_nodes=0;
  }

  if (this->hash_table)
  {
    hashtableDestroy(this->hash_table);
    this->hash_table=0;
  }
195 196 197 198
}

PPF3DDetector::~PPF3DDetector()
{
Bence Magyar's avatar
Bence Magyar committed
199
  clearTrainingModels();
200 201 202 203 204
}

// TODO: Check all step sizes to be positive
void PPF3DDetector::trainModel(const Mat &PC)
{
Bence Magyar's avatar
Bence Magyar committed
205 206 207
  CV_Assert(PC.type() == CV_32F || PC.type() == CV_32FC1);

  // compute bbox
208
  Vec2f xRange, yRange, zRange;
Bence Magyar's avatar
Bence Magyar committed
209 210 211 212 213 214 215 216
  computeBboxStd(PC, xRange, yRange, zRange);

  // compute sampling step from diameter of bbox
  float dx = xRange[1] - xRange[0];
  float dy = yRange[1] - yRange[0];
  float dz = zRange[1] - zRange[0];
  float diameter = sqrt ( dx * dx + dy * dy + dz * dz );

217
  float distanceStep = (float)(diameter * sampling_step_relative);
Bence Magyar's avatar
Bence Magyar committed
218

219
  Mat sampled = samplePCByQuantization(PC, xRange, yRange, zRange, (float)sampling_step_relative,0);
Bence Magyar's avatar
Bence Magyar committed
220 221 222 223 224 225

  int size = sampled.rows*sampled.rows;

  hashtable_int* hashTable = hashtableCreate(size, NULL);

  int numPPF = sampled.rows*sampled.rows;
226
  ppf = Mat(numPPF, PPF_LENGTH, CV_32FC1);
Bence Magyar's avatar
Bence Magyar committed
227 228 229 230 231 232 233 234 235 236 237 238 239 240

  // TODO: Maybe I could sample 1/5th of them here. Check the performance later.
  int numRefPoints = sampled.rows;

  // pre-allocate the hash nodes
  hash_nodes = (THash*)calloc(numRefPoints*numRefPoints, sizeof(THash));

  // TODO : This can easily be parallelized. But we have to lock hashtable_insert.
  // I realized that performance drops when this loop is parallelized (unordered
  // inserts into the hashtable
  // But it is still there to be investigated. For now, I leave this unparallelized
  // since this is just a training part.
  for (int i=0; i<numRefPoints; i++)
  {
241 242
    const Vec3f p1(sampled.ptr<float>(i));
    const Vec3f n1(sampled.ptr<float>(i) + 3);
Bence Magyar's avatar
Bence Magyar committed
243 244 245

    //printf("///////////////////// NEW REFERENCE ////////////////////////\n");
    for (int j=0; j<numRefPoints; j++)
246
    {
Bence Magyar's avatar
Bence Magyar committed
247 248 249
      // cannnot compute the ppf with myself
      if (i!=j)
      {
250 251
        const Vec3f p2(sampled.ptr<float>(j));
        const Vec3f n2(sampled.ptr<float>(j) + 3);
Bence Magyar's avatar
Bence Magyar committed
252

253
        Vec4d f = Vec4d::all(0);
Bence Magyar's avatar
Bence Magyar committed
254
        computePPFFeatures(p1, n1, p2, n2, f);
255
        KeyType hashValue = hashPPF(f, angle_step_radians, distanceStep);
Bence Magyar's avatar
Bence Magyar committed
256
        double alpha = computeAlpha(p1, n1, p2);
257
        uint ppfInd = i*numRefPoints+j;
Bence Magyar's avatar
Bence Magyar committed
258 259 260 261 262 263 264 265

        THash* hashNode = &hash_nodes[i*numRefPoints+j];
        hashNode->id = hashValue;
        hashNode->i = i;
        hashNode->ppfInd = ppfInd;

        hashtableInsertHashed(hashTable, hashValue, (void*)hashNode);

266 267
        Mat(f).reshape(1, 1).convertTo(ppf.row(ppfInd).colRange(0, 4), CV_32F);
        ppf.ptr<float>(ppfInd)[4] = (float)alpha;
Bence Magyar's avatar
Bence Magyar committed
268
      }
269
    }
Bence Magyar's avatar
Bence Magyar committed
270 271
  }

272
  angle_step = angle_step_radians;
Bence Magyar's avatar
Bence Magyar committed
273 274 275
  distance_step = distanceStep;
  hash_table = hashTable;
  num_ref_points = numRefPoints;
276
  sampled_pc = sampled;
Bence Magyar's avatar
Bence Magyar committed
277
  trained = true;
278 279 280 281 282 283 284 285 286
}



///////////////////////// MATCHING ////////////////////////////////////////


bool PPF3DDetector::matchPose(const Pose3D& sourcePose, const Pose3D& targetPose)
{
Bence Magyar's avatar
Bence Magyar committed
287
  // translational difference
288 289
  Vec3d dv = targetPose.t - sourcePose.t;
  double dNorm = cv::norm(dv);
Bence Magyar's avatar
Bence Magyar committed
290 291 292

  const double phi = fabs ( sourcePose.angle - targetPose.angle );

293
  return (phi<this->rotation_threshold && dNorm < this->position_threshold);
294 295
}

296
void PPF3DDetector::clusterPoses(std::vector<Pose3DPtr>& poseList, int numPoses, std::vector<Pose3DPtr> &finalPoses)
297
{
298
  std::vector<PoseCluster3DPtr> poseClusters;
Bence Magyar's avatar
Bence Magyar committed
299 300 301 302

  finalPoses.clear();

  // sort the poses for stability
303
  std::sort(poseList.begin(), poseList.end(), pose3DPtrCompare);
Bence Magyar's avatar
Bence Magyar committed
304 305 306

  for (int i=0; i<numPoses; i++)
  {
307
    Pose3DPtr pose = poseList[i];
Bence Magyar's avatar
Bence Magyar committed
308 309 310 311
    bool assigned = false;

    // search all clusters
    for (size_t j=0; j<poseClusters.size() && !assigned; j++)
312
    {
313
      const Pose3DPtr poseCenter = poseClusters[j]->poseList[0];
Bence Magyar's avatar
Bence Magyar committed
314 315 316 317 318
      if (matchPose(*pose, *poseCenter))
      {
        poseClusters[j]->addPose(pose);
        assigned = true;
      }
319
    }
Bence Magyar's avatar
Bence Magyar committed
320 321

    if (!assigned)
322
    {
323
      poseClusters.push_back(PoseCluster3DPtr(new PoseCluster3D(pose)));
Bence Magyar's avatar
Bence Magyar committed
324 325 326 327
    }
  }

  // sort the clusters so that we could output multiple hypothesis
328
  std::sort(poseClusters.begin(), poseClusters.end(), sortPoseClusters);
Bence Magyar's avatar
Bence Magyar committed
329 330 331 332 333

  finalPoses.resize(poseClusters.size());

  // TODO: Use MinMatchScore

334
  if (use_weighted_avg)
Bence Magyar's avatar
Bence Magyar committed
335
  {
336 337 338
#if defined _OPENMP
#pragma omp parallel for
#endif
Bence Magyar's avatar
Bence Magyar committed
339
    // uses weighting by the number of votes
340
    for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
341
    {
Bence Magyar's avatar
Bence Magyar committed
342
      // We could only average the quaternions. So I will make use of them here
343 344
      Vec4d qAvg = Vec4d::all(0);
      Vec3d tAvg = Vec3d::all(0);
Bence Magyar's avatar
Bence Magyar committed
345 346

      // Perform the final averaging
347 348
      PoseCluster3DPtr curCluster = poseClusters[i];
      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
Bence Magyar's avatar
Bence Magyar committed
349
      int curSize = (int)curPoses.size();
350
      size_t numTotalVotes = 0;
Bence Magyar's avatar
Bence Magyar committed
351 352 353 354 355 356 357 358 359 360

      for (int j=0; j<curSize; j++)
        numTotalVotes += curPoses[j]->numVotes;

      double wSum=0;

      for (int j=0; j<curSize; j++)
      {
        const double w = (double)curPoses[j]->numVotes / (double)numTotalVotes;

361 362 363
        qAvg += w * curPoses[j]->q;
        tAvg += w * curPoses[j]->t;
        wSum += w;
Bence Magyar's avatar
Bence Magyar committed
364 365
      }

366 367
      tAvg *= 1.0 / wSum;
      qAvg *= 1.0 / wSum;
Bence Magyar's avatar
Bence Magyar committed
368 369 370 371 372 373 374 375 376

      curPoses[0]->updatePoseQuat(qAvg, tAvg);
      curPoses[0]->numVotes=curCluster->numVotes;

      finalPoses[i]=curPoses[0]->clone();
    }
  }
  else
  {
377 378 379
#if defined _OPENMP
#pragma omp parallel for
#endif
380
    for (int i=0; i<static_cast<int>(poseClusters.size()); i++)
Bence Magyar's avatar
Bence Magyar committed
381 382
    {
      // We could only average the quaternions. So I will make use of them here
383 384
      Vec4d qAvg = Vec4d::all(0);
      Vec3d tAvg = Vec3d::all(0);
Bence Magyar's avatar
Bence Magyar committed
385 386

      // Perform the final averaging
387 388 389
      PoseCluster3DPtr curCluster = poseClusters[i];
      std::vector<Pose3DPtr> curPoses = curCluster->poseList;
      const int curSize = (int)curPoses.size();
Bence Magyar's avatar
Bence Magyar committed
390 391 392

      for (int j=0; j<curSize; j++)
      {
393 394
        qAvg += curPoses[j]->q;
        tAvg += curPoses[j]->t;
Bence Magyar's avatar
Bence Magyar committed
395 396
      }

397 398
      tAvg *= 1.0 / curSize;
      qAvg *= 1.0 / curSize;
Bence Magyar's avatar
Bence Magyar committed
399 400 401 402 403

      curPoses[0]->updatePoseQuat(qAvg, tAvg);
      curPoses[0]->numVotes=curCluster->numVotes;

      finalPoses[i]=curPoses[0]->clone();
404
    }
Bence Magyar's avatar
Bence Magyar committed
405 406 407
  }

  poseClusters.clear();
408 409
}

410
void PPF3DDetector::match(const Mat& pc, std::vector<Pose3DPtr>& results, const double relativeSceneSampleStep, const double relativeSceneDistance)
411
{
Bence Magyar's avatar
Bence Magyar committed
412 413 414 415 416 417 418 419
  if (!trained)
  {
    throw cv::Exception(cv::Error::StsError, "The model is not trained. Cannot match without training", __FUNCTION__, __FILE__, __LINE__);
  }

  CV_Assert(pc.type() == CV_32F || pc.type() == CV_32FC1);
  CV_Assert(relativeSceneSampleStep<=1 && relativeSceneSampleStep>0);

420
  scene_sample_step = (int)(1.0/relativeSceneSampleStep);
Bence Magyar's avatar
Bence Magyar committed
421 422 423 424

  //int numNeighbors = 10;
  int numAngles = (int) (floor (2 * M_PI / angle_step));
  float distanceStep = (float)distance_step;
425
  uint n = num_ref_points;
426
  std::vector<Pose3DPtr> poseList;
427
  int sceneSamplingStep = scene_sample_step;
Bence Magyar's avatar
Bence Magyar committed
428 429

  // compute bbox
430
  Vec2f xRange, yRange, zRange;
Bence Magyar's avatar
Bence Magyar committed
431 432 433 434 435 436 437 438 439 440 441 442
  computeBboxStd(pc, xRange, yRange, zRange);

  // sample the point cloud
  /*float dx = xRange[1] - xRange[0];
  float dy = yRange[1] - yRange[0];
  float dz = zRange[1] - zRange[0];
  float diameter = sqrt ( dx * dx + dy * dy + dz * dz );
  float distanceSampleStep = diameter * RelativeSceneDistance;*/
  Mat sampled = samplePCByQuantization(pc, xRange, yRange, zRange, (float)relativeSceneDistance, 0);

  // allocate the accumulator : Moved this to the inside of the loop
  /*#if !defined (_OPENMP)
443
     uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
Bence Magyar's avatar
Bence Magyar committed
444 445
  #endif*/

446
  poseList.reserve((sampled.rows/sceneSamplingStep)+4);
Bence Magyar's avatar
Bence Magyar committed
447

448 449 450
#if defined _OPENMP
#pragma omp parallel for
#endif
Bence Magyar's avatar
Bence Magyar committed
451 452
  for (int i = 0; i < sampled.rows; i += sceneSamplingStep)
  {
453 454
    uint refIndMax = 0, alphaIndMax = 0;
    uint maxVotes = 0;
Bence Magyar's avatar
Bence Magyar committed
455

456 457 458 459
    const Vec3f p1(sampled.ptr<float>(i));
    const Vec3f n1(sampled.ptr<float>(i) + 3);
    Vec3d tsg = Vec3d::all(0);
    Matx33d Rsg = Matx33d::all(0), RInv = Matx33d::all(0);
Bence Magyar's avatar
Bence Magyar committed
460

461
    uint* accumulator = (uint*)calloc(numAngles*n, sizeof(uint));
Bence Magyar's avatar
Bence Magyar committed
462 463 464 465 466 467 468 469
    computeTransformRT(p1, n1, Rsg, tsg);

    // Tolga Birdal's notice:
    // As a later update, we might want to look into a local neighborhood only
    // To do this, simply search the local neighborhood by radius look up
    // and collect the neighbors to compute the relative pose

    for (int j = 0; j < sampled.rows; j ++)
470
    {
Bence Magyar's avatar
Bence Magyar committed
471 472
      if (i!=j)
      {
473 474 475 476
        const Vec3f p2(sampled.ptr<float>(j));
        const Vec3f n2(sampled.ptr<float>(j) + 3);
        Vec3d p2t;
        double alpha_scene;
Bence Magyar's avatar
Bence Magyar committed
477

478
        Vec4d f = Vec4d::all(0);
Bence Magyar's avatar
Bence Magyar committed
479 480 481
        computePPFFeatures(p1, n1, p2, n2, f);
        KeyType hashValue = hashPPF(f, angle_step, distanceStep);

482
        p2t = tsg + Rsg * Vec3d(p2);
Bence Magyar's avatar
Bence Magyar committed
483 484 485 486

        alpha_scene=atan2(-p2t[2], p2t[1]);

        if ( alpha_scene != alpha_scene)
487
        {
Bence Magyar's avatar
Bence Magyar committed
488
          continue;
489
        }
Bence Magyar's avatar
Bence Magyar committed
490 491 492 493 494 495 496 497 498

        if (sin(alpha_scene)*p2t[2]<0.0)
          alpha_scene=-alpha_scene;

        alpha_scene=-alpha_scene;

        hashnode_i* node = hashtableGetBucketHashed(hash_table, (hashValue));

        while (node)
499
        {
Bence Magyar's avatar
Bence Magyar committed
500 501 502
          THash* tData = (THash*) node->data;
          int corrI = (int)tData->i;
          int ppfInd = (int)tData->ppfInd;
503
          float* ppfCorrScene = ppf.ptr<float>(ppfInd);
504
          double alpha_model = (double)ppfCorrScene[PPF_LENGTH-1];
Bence Magyar's avatar
Bence Magyar committed
505 506 507 508 509 510 511 512 513 514 515 516
          double alpha = alpha_model - alpha_scene;

          /*  Tolga Birdal's note: Map alpha to the indices:
                  atan2 generates results in (-pi pi]
                  That's why alpha should be in range [-2pi 2pi]
                  So the quantization would be :
                  numAngles * (alpha+2pi)/(4pi)
                  */

          //printf("%f\n", alpha);
          int alpha_index = (int)(numAngles*(alpha + 2*M_PI) / (4*M_PI));

517
          uint accIndex = corrI * numAngles + alpha_index;
Bence Magyar's avatar
Bence Magyar committed
518 519 520

          accumulator[accIndex]++;
          node = node->next;
521
        }
Bence Magyar's avatar
Bence Magyar committed
522
      }
523
    }
Bence Magyar's avatar
Bence Magyar committed
524 525

    // Maximize the accumulator
526
    for (uint k = 0; k < n; k++)
527
    {
Bence Magyar's avatar
Bence Magyar committed
528 529
      for (int j = 0; j < numAngles; j++)
      {
530 531
        const uint accInd = k*numAngles + j;
        const uint accVal = accumulator[ accInd ];
Bence Magyar's avatar
Bence Magyar committed
532 533 534 535 536 537 538 539 540 541 542
        if (accVal > maxVotes)
        {
          maxVotes = accVal;
          refIndMax = k;
          alphaIndMax = j;
        }

#if !defined (_OPENMP)
        accumulator[accInd ] = 0;
#endif
      }
543
    }
Bence Magyar's avatar
Bence Magyar committed
544 545 546

    // invert Tsg : Luckily rotation is orthogonal: Inverse = Transpose.
    // We are not required to invert.
547 548 549 550
    Vec3d tInv, tmg;
    Matx33d Rmg;
    RInv = Rsg.t();
    tInv = -RInv * tsg;
Bence Magyar's avatar
Bence Magyar committed
551

552 553
    Matx44d TsgInv;
    rtToPose(RInv, tInv, TsgInv);
Bence Magyar's avatar
Bence Magyar committed
554 555

    // TODO : Compute pose
556 557
    const Vec3f pMax(sampled_pc.ptr<float>(refIndMax));
    const Vec3f nMax(sampled_pc.ptr<float>(refIndMax) + 3);
Bence Magyar's avatar
Bence Magyar committed
558 559 560

    computeTransformRT(pMax, nMax, Rmg, tmg);

561 562
    Matx44d Tmg;
    rtToPose(Rmg, tmg, Tmg);
Bence Magyar's avatar
Bence Magyar committed
563 564 565 566 567 568

    // convert alpha_index to alpha
    int alpha_index = alphaIndMax;
    double alpha = (alpha_index*(4*M_PI))/numAngles-2*M_PI;

    // Equation 2:
569 570 571 572 573
    Matx44d Talpha;
    Matx33d R;
    Vec3d t = Vec3d::all(0);
    getUnitXRotation(alpha, R);
    rtToPose(R, t, Talpha);
Bence Magyar's avatar
Bence Magyar committed
574

575
    Matx44d rawPose = TsgInv * (Talpha * Tmg);
Bence Magyar's avatar
Bence Magyar committed
576

577 578
    Pose3DPtr pose(new Pose3D(alpha, refIndMax, maxVotes));
    pose->updatePose(rawPose);
579 580 581 582 583 584
    #if defined (_OPENMP)
    #pragma omp critical
    #endif
    {
      poseList.push_back(pose);
    }
Bence Magyar's avatar
Bence Magyar committed
585 586 587 588 589 590 591 592 593 594 595 596

#if defined (_OPENMP)
    free(accumulator);
#endif
  }

  // TODO : Make the parameters relative if not arguments.
  //double MinMatchScore = 0.5;

  int numPosesAdded = sampled.rows/sceneSamplingStep;

  clusterPoses(poseList, numPosesAdded, results);
597 598 599 600 601
}

} // namespace ppf_match_3d

} // namespace cv