PoseEstimator.cpp
Go to the documentation of this file.
1 // (C) Copyright Renaud Detry 2007-2015.
2 // Distributed under the GNU General Public License and under the
3 // BSD 3-Clause License (See accompanying file LICENSE.txt).
4 
5 /** @file */
6 
7 #include <nuklei/PoseEstimator.h>
8 #include <boost/bind.hpp>
9 #include <nuklei/parallelizer.h>
10 
11 namespace nuklei
12 {
13 
14  PoseEstimator::PoseEstimator(const double locH,
15  const double oriH,
16  const int nChains,
17  const int n,
18  boost::shared_ptr<CustomIntegrandFactor> cif,
19  const bool partialview,
20  const bool progress) :
21  evaluationStrategy_(KernelCollection::MAX_EVAL),
22  loc_h_(locH), ori_h_(oriH),
23  nChains_(nChains), n_(n),
24  cif_(cif), partialview_(partialview),
25  progress_(progress), meshTol_(4)
26  {
27  if (nChains_ <= 0) nChains_ = 8;
28  parallel_ = typeFromName<parallelizer>(PARALLELIZATION);
29  }
30 
31 
32  kernel::se3
33  PoseEstimator::modelToSceneTransformation(const boost::optional<kernel::se3>& gtTransfo) const
34  {
35  NUKLEI_TRACE_BEGIN();
36  int n = -1;
37 
38  if (n_ <= 0)
39  {
40  n = objectModel_.size();
41  if (n > 1000)
42  {
43  NUKLEI_WARN("Warning: Object model has more than 1000 points. "
44  "To keep computational cost low, only 1000 points will be "
45  "used at each inference loop. "
46  "Use -n to force a large number of model points.");
47  n = 1000;
48  }
49  }
50  else
51  n = n_;
52 
53  KernelCollection poses;
54  if (!hasOpenMP())
55  {
56  NUKLEI_WARN("Nuklei has not been compiled with OpenMP support. "
57  "Pose estimation will use a single core.");
58  }
59 
60  if (progress_)
61  pi_->initialize(0, 10*n*(partialview_?4:1)*nChains_ / 10, "Estimating pose", 0);
62 
63  parallelizer p(nChains_, parallel_);
64  std::vector<kernel::se3> retv =
65  p.run<kernel::se3>(boost::bind(&PoseEstimator::mcmc, this, n),
66  kernel::base::WeightAccessor());
67 
68  if (progress_)
69  pi_->forceEnd();
70 
71  for (std::vector<kernel::se3>::const_iterator i = retv.begin();
72  i != retv.end(); ++i)
73  poses.add(*i);
74 
75  int success = 0;
76  if (gtTransfo)
77  {
78  for (KernelCollection::const_sort_iterator i = poses.sortBegin(poses.size()); i != i.end(); ++i)
79  {
80  coord_pair d = i->polyDistanceTo(*gtTransfo);
81  coord_pair t = coord_pair(gtTransfo->getLocH(), gtTransfo->getOriH());
82  bool s = (d.first < t.first && d.second < t.second);
83  if (s)
84  success++;
85  std::cout << "Matching score: " << i->getWeight() << ", ";
86  std::cout << "distance to GT: " << d.first << " " << d.second << ", ";
87  if (s) std::cout << "success";
88  else std::cout << "failure";
89  std::cout << std::endl;
90  }
91  std::cout << "Number of successful chains: " << success << " out of ";
92  std::cout << poses.size() << "." << std::endl;
93  }
94 
95  kernel::se3 pose(*poses.sortBegin(1));
96  pose.setWeight(findMatchingScore(pose));
97 
98  return pose;
99  NUKLEI_TRACE_END();
100  }
101 
102  double
103  PoseEstimator::findMatchingScore(const kernel::se3& pose) const
104  {
105  NUKLEI_TRACE_BEGIN();
106  kernel::se3 t = pose;
107 
108  if (!partialview_)
109  {
110  double w1 = 0, w2 = 0;
111 
112  KernelCollection tmp = objectModel_;
113  tmp.transformWith(t);
114  tmp.computeKernelStatistics();
115  tmp.buildKdTree();
116 
117  for (KernelCollection::const_iterator i = objectModel_.begin();
118  i != objectModel_.end(); ++i)
119  {
120  w1 += sceneModel_.evaluationAt(*i->polyTransformedWith(t),
121  evaluationStrategy_);
122  }
123 
124  for (KernelCollection::const_iterator i = sceneModel_.begin();
125  i != sceneModel_.end(); ++i)
126  {
127  w2 += tmp.evaluationAt(*i, evaluationStrategy_);
128  }
129 
130  t.setWeight(w1/objectModel_.size() * (cif_?cif_->factor(pose):1.));
131  //t.setWeight(std::sqrt(w1/objectModel_.size()*w2/objectModel_.size()));
132  }
133  else
134  {
135  // Use a mesh to compute the partial view of the model and compute the
136  // matching score from that.
137 
139  objectModel_.partialViewBegin(viewpointInFrame(t), meshTol_, false, true);
140  for (KernelCollection::const_partialview_iterator i = viewIterator;
141  i != i.end(); ++i)
142  {
143  weight_t w = sceneModel_.evaluationAt(*i->polyTransformedWith(t),
144  evaluationStrategy_);
145  t.setWeight(t.getWeight() + w);
146  }
147  t.setWeight(t.getWeight()/std::pow(std::distance(viewIterator, viewIterator.end()), .7) * (cif_?cif_->factor(pose):1.));
148 
149  if (cif_ && !cif_->test(t))
150  t.setWeight(0);
151  }
152 
153  return t.getWeight();
154  NUKLEI_TRACE_END();
155  }
156 
157  void PoseEstimator::load(const std::string& objectFilename,
158  const std::string& sceneFilename,
159  const std::string& meshfile,
160  const std::string& viewpointfile,
161  const bool light,
162  const bool computeNormals)
163  {
164  NUKLEI_TRACE_BEGIN();
165  KernelCollection objectModel, sceneModel;
166  readObservations(objectFilename, objectModel);
167  readObservations(sceneFilename, sceneModel);
168  Vector3 viewpoint(0, 0, 0);
169  if (partialview_)
170  {
171  NUKLEI_ASSERT(!viewpointfile.empty());
172  viewpoint = kernel::se3(*readSingleObservation(viewpointfile)).getLoc();
173  }
174  load(objectModel, sceneModel, meshfile, viewpoint,
175  light, computeNormals);
176  NUKLEI_TRACE_END();
177  }
178 
179 
180  void PoseEstimator::load(const KernelCollection& objectModel,
181  const KernelCollection& sceneModel,
182  const std::string& meshfile,
183  const Vector3& viewpoint,
184  const bool light,
185  const bool computeNormals)
186  {
187  NUKLEI_TRACE_BEGIN();
188  objectModel_ = objectModel;
189  sceneModel_ = sceneModel;
190  viewpoint_ = viewpoint;
191 
192  if (objectModel_.size() == 0 || sceneModel_.size() == 0)
193  NUKLEI_THROW("Empty input cloud.");
194 
195  if (objectModel_.front().polyType() == kernel::base::R3)
196  {
197  if (computeNormals)
198  {
199  objectModel_.buildNeighborSearchTree();
200  objectModel_.computeSurfaceNormals();
201  }
202  }
203 
204  if (sceneModel_.front().polyType() == kernel::base::R3)
205  {
206  if (computeNormals)
207  {
208  sceneModel_.buildNeighborSearchTree();
209  sceneModel_.computeSurfaceNormals();
210  }
211  }
212 
213  if (objectModel_.front().polyType() != sceneModel_.front().polyType())
214  NUKLEI_THROW("Input point clouds must be defined on the same domain.");
215 
216  if (light && sceneModel_.size() > 10000)
217  {
218  sceneModel_.computeKernelStatistics();
219  KernelCollection tmp;
221  sceneModel_.sampleBegin(10000);
222  for (; i != i.end(); i++)
223  {
224  tmp.add(*i);
225  }
226  sceneModel_ = tmp;
227  }
228 
229  objectSize_ = objectModel_.moments()->getLocH();
230 
231  if (loc_h_ <= 0)
232  loc_h_ = objectSize_ / 10;
233 
234  sceneModel_.setKernelLocH(loc_h_);
235  sceneModel_.setKernelOriH(ori_h_);
236  objectModel_.setKernelLocH(loc_h_);
237  objectModel_.setKernelOriH(ori_h_);
238 
239  objectModel_.computeKernelStatistics();
240  sceneModel_.computeKernelStatistics();
241  sceneModel_.buildKdTree();
242 
243  if (partialview_)
244  {
245  if (!meshfile.empty())
246  objectModel_.readMeshFromOffFile(meshfile);
247  else
248  objectModel_.buildMesh();
249  objectModel_.buildPartialViewCache(meshTol_, as_const(objectModel_).front().polyType() == kernel::base::R3XS2P);
250  }
251 
252  // Create dummy ProgressIndicator
253  if (progress_)
254  pi_.reset(new ProgressIndicator(1, "", 11));
255  NUKLEI_TRACE_END();
256  }
257 
258 
259  // Temperature function (cooling factor)
260  double PoseEstimator::Ti(const unsigned i, const unsigned F)
261  {
262  {
263  const double T0 = .5;
264  const double TF = .05;
265 
266  return std::max(T0 * std::pow(TF/T0, double(i)/F), TF);
267  }
268  }
269 
270  bool PoseEstimator::recomputeIndices(std::vector<int>& indices,
271  const kernel::se3& nextPose,
272  const int n) const
273  {
274  Vector3 mean = objectModel_.mean()->getLoc();
275  Vector3 v = la::normalized(viewpointInFrame(nextPose) - mean);
276  std::vector<int> pindices = objectModel_.partialView(v, meshTol_, true, true);
277 
278  if (pindices.size() < 20) return false;
279  indices = pindices;
280 
281  // indices = objectModel_.partialView(viewpointInFrame(nextPose),
282  // meshTol_);
283  std::random_shuffle(indices.begin(), indices.end(), Random::uniformInt);
284  if (indices.size() > n)
285  indices.resize(n);
286  return true;
287  }
288 
289  /**
290  * This function implements the algorithm of Fig. 2: Simulated annealing
291  * algorithm of the ACCV paper.
292  * - T_j is given by @c temperature
293  * - u is given by Random::uniform()
294  * - w_j is @c currentPose
295  */
296  void
297  PoseEstimator::metropolisHastings(kernel::se3& currentPose,
298  weight_t &currentWeight,
299  const weight_t temperature,
300  const bool firstRun,
301  const int n) const
302  {
303  NUKLEI_TRACE_BEGIN();
304 
305  // Randomly select particles from the object model
306  std::vector<int> indices;
308  i = objectModel_.sampleBegin(n);
309  i != i.end();
310  i++)
311  {
312  indices.push_back(i.index());
313  }
314  std::random_shuffle(indices.begin(), indices.end(), Random::uniformInt);
315 
316  // Next chain state
317  kernel::se3 nextPose;
318  // Whether we go for a local or independent proposal
319  bool independentProposal = false;
320 
321  if (Random::uniform() < .75 || firstRun)
322  {
323  // Go for independent proposal
324 
325  independentProposal = true;
326 
327  for (int count = 0; ; ++count)
328  {
329  if (count == 100) return;
330  const kernel::base& randomModelPoint = objectModel_.at(indices.at(Random::uniformInt(indices.size())));
331  kernel::se3::ptr k2 = randomModelPoint.polySe3Proj();
332  kernel::se3::ptr k1 = sceneModel_.at(Random::uniformInt(sceneModel_.size())).polySe3Proj();
333 
334  nextPose = k1->transformationFrom(*k2);
335 
336  if (cif_ && !cif_->test(nextPose)) continue;
337 
338  if (partialview_)
339  {
340  bool visible = false;
341 
342  if (randomModelPoint.polyType() == kernel::base::R3XS2P)
343  visible = objectModel_.isVisibleFrom(kernel::r3xs2p(randomModelPoint),
344  viewpointInFrame(nextPose),
345  meshTol_);
346  else
347  visible = objectModel_.isVisibleFrom(randomModelPoint.getLoc(),
348  viewpointInFrame(nextPose),
349  meshTol_);
350  if (!visible) continue;
351 
352  if (! recomputeIndices(indices, nextPose, n))
353  continue;
354  }
355 
356  break;
357  }
358  }
359  else
360  {
361  // Go for local proposal
362 
363  independentProposal = false;
364  NUKLEI_DEBUG_ASSERT(currentPose.loc_h_ > 0 && currentPose.ori_h_ > 0);
365  for (int count = 0; ; ++count)
366  {
367  if (count == 100) return;
368  nextPose = currentPose.sample();
369  if (cif_ && !cif_->test(nextPose)) continue;
370  if (partialview_ && ! recomputeIndices(indices, nextPose, n))
371  continue;
372  break;
373  }
374  }
375 
376  weight_t weight = 0;
377 
378  double threshold = Random::uniform();
379 
380  double factor = (cif_?cif_->factor(nextPose):1.);
381 
382  // Go through the points of the model
383  for (unsigned pi = 0; pi < indices.size(); ++pi)
384  {
385  const kernel::base& objectPoint =
386  objectModel_.at(indices.at(pi));
387 
388  kernel::base::ptr test = objectPoint.polyTransformedWith(nextPose);
389 
390  weight_t w = 0;
391  if (WEIGHTED_SUM_EVIDENCE_EVAL)
392  {
393  w = (sceneModel_.evaluationAt(*test,
394  KernelCollection::WEIGHTED_SUM_EVAL) +
395  WHITE_NOISE_POWER/sceneModel_.size() );
396  }
397  else
398  {
399  w = (sceneModel_.evaluationAt(*test, KernelCollection::MAX_EVAL) +
400  WHITE_NOISE_POWER );
401  }
402 
403  w *= factor;
404 
405  weight += w;
406 
407  // At least consider sqrt(size(model)) points
408  if (pi < std::sqrt(indices.size())) continue;
409 
410 
411  weight_t nextWeight = weight/(pi+1.);
412  if (partialview_) nextWeight = weight/std::sqrt(pi+1.);
413  // For the first run, consider all the points of the model
414  if (firstRun)
415  {
416  if (pi == indices.size()-1)
417  {
418  currentPose = nextPose;
419  currentWeight = nextWeight;
420  return;
421  }
422  else continue;
423  }
424 
425  weight_t dec = std::pow(nextWeight/currentWeight, 1./temperature);
426  if (independentProposal) dec *= currentWeight/nextWeight;
427 
428  // Early abort
429  if (dec < .6*threshold)
430  {
431  return;
432  }
433 
434  // MH decision
435  if (pi == indices.size()-1)
436  {
437  if (dec > threshold)
438  {
439  currentPose = nextPose;
440  currentWeight = nextWeight;
441  }
442  return;
443  }
444  }
445  NUKLEI_THROW("Reached forbidden state.");
446  NUKLEI_TRACE_END();
447  }
448 
449  kernel::se3
450  PoseEstimator::mcmc(const int n) const
451  {
452  NUKLEI_TRACE_BEGIN();
453  kernel::se3 currentPose, bestPose;
454  weight_t currentWeight = 0;
455  bestPose.setWeight(currentWeight);
456  metropolisHastings(currentPose, currentWeight, 1, true, n);
457 
458  //fixme: See if nSteps should be computed as a function of n.
459  int nSteps = 1000;
460  if (partialview_) nSteps = 1000;
461  //fixme:
462  nSteps = 10*n*(partialview_?4:1);
463 
464  for (int i = 0; i < nSteps; i++)
465  {
466  {
467  // begin and end bandwidths for the local proposal
468  coord_t bLocH = objectSize_/10;
469  coord_t eLocH = objectSize_/40;
470  coord_t bOriH = .1;
471  coord_t eOriH = .02;
472 
473  unsigned e = nSteps-1;
474 
475  currentPose.setLocH(double(e-i)/e * bLocH +
476  double(i)/e * eLocH);
477  currentPose.setOriH(double(e-i)/e * bOriH +
478  double(i)/e * eOriH);
479  if (currentPose.loc_h_ <= 0)
480  {
481  NUKLEI_THROW("Unexpected value for currentPose.loc_h_.");
482  }
483  if (progress_ && i%10 == 0) pi_->mtInc();
484  }
485 
486  metropolisHastings(currentPose, currentWeight,
487  Ti(i, nSteps/5), false, n);
488 
489  if (currentWeight > bestPose.getWeight())
490  {
491  bestPose = currentPose;
492  bestPose.setWeight(currentWeight);
493  }
494  }
495 
496  return bestPose;
497  NUKLEI_TRACE_END();
498  }
499 
500  Vector3 PoseEstimator::viewpointInFrame(const kernel::se3& frame) const
501  {
502  kernel::se3 origin;
503  kernel::se3 invt = origin.transformationFrom(frame);
504  kernel::r3 v;
505  v.loc_ = viewpoint_;
506  return v.transformedWith(invt).getLoc();
507  }
508 
509  void
510  PoseEstimator::writeAlignedModel(const std::string& filename,
511  const kernel::se3& t) const
512  {
513  NUKLEI_TRACE_BEGIN();
514  KernelCollection objectModel = objectModel_;
515  if (partialview_)
516  {
517  objectModel = KernelCollection();
518  kernel::se3 tt = t;
519  for (KernelCollection::const_iterator i = objectModel_.begin();
520  i != objectModel_.end(); ++i)
521  {
522  objectModel.add(*i);
523  bool visible = false;
524  const kernel::base& tmp = *i;
525  if (tmp.polyType() == kernel::base::R3XS2P)
526  visible = objectModel_.isVisibleFrom(kernel::r3xs2p(tmp),
527  viewpointInFrame(tt),
528  meshTol_);
529  else
530  visible = objectModel_.isVisibleFrom(tmp.getLoc(),
531  viewpointInFrame(tt),
532  meshTol_);
533  if (visible)
534  {
535  RGBColor c(0, 0, 1);
536  ColorDescriptor d;
537  d.setColor(c);
538  objectModel.back().setDescriptor(d);
539  }
540  }
541  }
542  objectModel.transformWith(t);
543  writeObservations(filename,
544  objectModel,
545  Observation::SERIAL);
546  NUKLEI_TRACE_END();
547  }
548 
549  void PoseEstimator::setCustomIntegrandFactor(boost::shared_ptr<CustomIntegrandFactor> cif)
550  {
551  cif_ = cif;
552  }
553 
554  boost::shared_ptr<CustomIntegrandFactor> PoseEstimator::getCustomIntegrandFactor() const
555  {
556  return cif_;
557  }
558 
559 
560 }
nuklei_trsl::ppfilter_iterator< is_picked, const_iterator > const_sample_iterator
Sample Iterator type.
nuklei_trsl::reorder_iterator< const_iterator > const_sort_iterator
Sort Iterator type.
nuklei_trsl::ppfilter_iterator< is_picked, iterator > sample_iterator
Sample Iterator type.
Public namespace.
Definition: Color.cpp:9
std::pair< coord_t, coord_t > coord_pair
Pair of coord_t.
Definition: Definitions.h:27
Container::const_iterator const_iterator
KernelCollection iterator.
const_partialview_iterator partialViewBegin(const Vector3 &viewpoint, const coord_t &tolerance=FLOATTOL, const bool useViewcache=false, const bool useRayToSurfacenormalAngle=false) const
Assuming that the points in this collection form the surface of an object, this function returns an i...
double weight_t
Type for particle weights.
Definition: Definitions.h:31
void writeObservations(ObservationWriter &w, const KernelCollection &kc)
Writes the content of kc using the provided writer w.
kernel::base::ptr moments() const
Returns a kernel holding the mean and standard deviation in position and orientation of the data.
void buildPartialViewCache(const double meshTol, const bool useRayToSurfacenormalAngle=false)
Builds set of partial views of the object. See Intermediary Results.
r3xs2_base< groupS::s2p > r3xs2p
Definition: Kernel.h:623
void setKernelLocH(coord_t h)
Sets the location bandwidth of all kernels.
Container::reference at(Container::size_type n)
Returns the kernel at index n.
void setKernelOriH(coord_t h)
Sets the orientation bandwidth of all kernels.
#define NUKLEI_ASSERT(expression)
Throws an Error if expression is not true.
Definition: Common.h:113
static unsigned long int uniformInt(unsigned long int n)
This function returns a random integer from 0 to n-1 inclusive.
Definition: Random.cpp:195
Container::reference front()
Returns the kernel at index 0.
NUKLEI_UNIQUE_PTR< kernel::base > ptr
NUKLEI_UNIQUE_PTR for kernel::base.
Definition: Kernel.h:50
double coord_t
Type for point coordinates.
Definition: Definitions.h:25
Container::iterator end()
Returns an iterator pointing to the last kernel.
kernel::base::ptr mean() const
Returns a kernel holding the mean position and orientation of the data.
void transformWith(const kernel::se3 &t)
Transforms the data with t.
Container::iterator begin()
Returns an iterator pointing to the first kernel.
nuklei_trsl::reorder_iterator< const_iterator > const_partialview_iterator
Partial View Iterator type.
sample_iterator sampleBegin(size_t sampleSize)
Returns an iterator that iterates through sampleSize kernels selected randomly.
bool isVisibleFrom(const Vector3 &p, const Vector3 &viewpoint, const coord_t &tolerance=FLOATTOL) const
Assuming that the points in this collection form the surface of an object, this function computes whe...
void buildKdTree()
Builds a kd-tree of the kernel positions and stores the tree internally. See Intermediary Results.
weight_t evaluationAt(const kernel::base &f, const EvaluationStrategy strategy=WEIGHTED_SUM_EVAL) const
Evaluates the density represented by *this at f.
static double uniform()
This function returns a double precision floating point number uniformly distributed in the range .
Definition: Random.cpp:159
Container::size_type size() const
Returns the number of kernels.
std::vector< int > partialView(const Vector3 &viewpoint, const coord_t &tolerance=FLOATTOL, const bool useViewcache=false, const bool useRayToSurfacenormalAngle=false) const
Assuming that the points in this collection form the surface of an object, this function returns the ...
void computeKernelStatistics()
Computes the sum of all kernel weights (total weight), and the maximum kernel cut point.
void readObservations(ObservationReader &r, KernelCollection &kc)
Reads the data available from the reader r and stores it into kc.
kernel::base::ptr readSingleObservation(const std::string &s)
Reads a single observation from file s (with automatic type detection), and returns it.
void buildNeighborSearchTree()
Builds a neighbor search tree of the kernel positions and stores the tree internally....
void buildMesh()
Builds a mesh from kernel positions. See Intermediary Results.
T const & as_const(T const &t)
Provides a const reference to an object.
Definition: Common.h:197
void computeSurfaceNormals()
Computes surface normals at all points. After running this method, all kernels are nuklei::kernel::r3...
#define NUKLEI_THROW(x)
Throws an Error.
Definition: Common.h:94
© Copyright 2007-2013 Renaud Detry.
Distributed under the terms of the GNU General Public License (GPL).
(See accompanying file LICENSE.txt or copy at http://www.gnu.org/copyleft/gpl.html.)
Revised Sun Sep 13 2020 19:10:06.