pointmatcher/DataPoints.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 Fran├žois Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
36 #include "PointMatcher.h"
37 #include "PointMatcherPrivate.h"
38 #include <iostream>
39 
40 using namespace std;
41 
43 template<typename T>
45  text(text),
46  span(span)
47 {}
48 
50 template<typename T>
52  runtime_error(reason)
53 {}
54 
56 template<typename T>
58 {
59  return (this->text == that.text) && (this->span == that.span);
60 }
61 
63 template<typename T>
65 {}
66 
68 template<typename T>
70  std::vector<Label>(1, label)
71 {}
72 
74 template<typename T>
76 {
77  for (const_iterator it(this->begin()); it != this->end(); ++it)
78  {
79  if (it->text == text)
80  return true;
81  }
82  return false;
83 }
84 
86 template<typename T>
88 {
89  size_t dim(0);
90  for (const_iterator it(this->begin()); it != this->end(); ++it)
91  dim += it->span;
92  return dim;
93 }
94 
96 template<typename T>
98 {}
99 
101 template<typename T>
105 {
106  features.resize(featureLabels.totalDim(), pointCount);
108  descriptors.resize(descriptorLabels.totalDim(), pointCount);
109 }
110 
112 template<typename T>
114  const Labels& descriptorLabels,
115  const Labels& timeLabels,
116  const size_t pointCount):
117  featureLabels(featureLabels),
118  descriptorLabels(descriptorLabels),
119  timeLabels(timeLabels)
120 {
121  features.resize(featureLabels.totalDim(), pointCount);
122 
124  descriptors.resize(descriptorLabels.totalDim(), pointCount);
125  if(timeLabels.totalDim())
126  times.resize(timeLabels.totalDim(), pointCount);
127 }
128 
130 template<typename T>
131 PointMatcher<T>::DataPoints::DataPoints(const Matrix& features, const Labels& featureLabels):
132  features(features),
133  featureLabels(featureLabels)
134 {}
135 
137 template<typename T>
138 PointMatcher<T>::DataPoints::DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels):
139  features(features),
140  featureLabels(featureLabels),
141  descriptors(descriptors),
142  descriptorLabels(descriptorLabels)
143 {}
144 
146 template<typename T>
147 PointMatcher<T>::DataPoints::DataPoints(const Matrix& features, const Labels& featureLabels, const Matrix& descriptors, const Labels& descriptorLabels, const Int64Matrix& times, const Labels& timeLabels):
148  features(features),
149  featureLabels(featureLabels),
150  descriptors(descriptors),
151  descriptorLabels(descriptorLabels),
152  times(times),
153  timeLabels(timeLabels)
154 {}
155 
157 template<typename T>
159 {
160  return features.cols();
161 }
162 
164 template<typename T>
166 {
167  const size_t nbRows = features.rows();
168  return (nbRows == 0) ? 0 : (nbRows - 1);
169 }
170 
172 template<typename T>
174 {
175  return features.rows();
176 }
177 
179 template<typename T>
181 {
182  return descriptorLabels.size();
183 }
184 
186 template<typename T>
188 {
189  return descriptors.rows();
190 }
191 
193 template<typename T>
195 {
196  return times.rows();
197 }
198 
200 template<typename T>
202 {
203  // Note comparing matrix without the same dimensions trigger an assert
204  bool isEqual = false;
205  if((features.rows() == that.features.rows()) &&
206  (features.cols() == that.features.cols()) &&
207  (descriptors.rows() == that.descriptors.rows()) &&
208  (descriptors.cols() == that.descriptors.cols()) &&
209  (times.rows() == that.times.rows()) &&
210  (times.cols() == that.times.cols()))
211  {
212  isEqual = (features == that.features) &&
213  (featureLabels == that.featureLabels) &&
214  (descriptors == that.descriptors) &&
215  (descriptorLabels == that.descriptorLabels)&&
216  (times == that.times) &&
217  (timeLabels == that.timeLabels);
218  }
219 
220  return isEqual;
221 
222 }
223 
225 template<typename T>
227 {
228  const int nbPoints1 = this->features.cols();
229  const int nbPoints2 = dp.features.cols();
230  const int nbPointsTotal = nbPoints1 + nbPoints2;
231 
232  const int dimFeat = this->features.rows();
233  if(dimFeat != dp.features.rows())
234  {
235  stringstream errorMsg;
236  errorMsg << "Cannot concatenate DataPoints because the dimension of the features are not the same. Actual dimension: " << dimFeat << " New dimension: " << dp.features.rows();
237  throw InvalidField(errorMsg.str());
238  }
239 
240  // concatenate features
241  this->features.conservativeResize(Eigen::NoChange, nbPointsTotal);
242  this->features.rightCols(nbPoints2) = dp.features;
243 
244  // concatenate descriptors
245  concatenateLabelledMatrix(&descriptorLabels, &descriptors, dp.descriptorLabels, dp.descriptors);
246  assertDescriptorConsistency();
247 
248  // concatenate time
249  concatenateLabelledMatrix(&timeLabels, &times, dp.timeLabels, dp.times);
250  assertTimesConsistency();
251 
252 }
253 
254 
256 template<typename T>
257 template<typename MatrixType>
258 void PointMatcher<T>::DataPoints::concatenateLabelledMatrix(Labels* labels, MatrixType* data, const Labels extraLabels, const MatrixType extraData)
259 {
260  const int nbPoints1 = data->cols();
261  const int nbPoints2 = extraData.cols();
262  const int nbPointsTotal = nbPoints1 + nbPoints2;
263 
264 
265  if (*labels == extraLabels)
266  {
267  if (labels->size() > 0)
268  {
269  // same descriptors, fast merge
270  data->conservativeResize(Eigen::NoChange, nbPointsTotal);
271  data->rightCols(nbPoints2) = extraData;
272  }
273  }
274  else
275  {
276  // different descriptors, slow merge
277 
278  // collect labels to be kept
279  Labels newLabels;
280  for(BOOST_AUTO(it, labels->begin()); it != labels->end(); ++it)
281  {
282  for(BOOST_AUTO(jt, extraLabels.begin()); jt != extraLabels.end(); ++jt)
283  {
284  if (it->text == jt->text)
285  {
286  if (it->span != jt->span)
287  throw InvalidField(
288  (boost::format("The field %1% has dimension %2% in this, different than dimension %3% in that") % it->text % it->span % jt->span).str()
289  );
290  newLabels.push_back(*it);
291  break;
292  }
293  }
294  }
295 
296  // allocate new descriptors
297  if (newLabels.size() > 0)
298  {
299  MatrixType newData;
300  Labels filledLabels;
301  this->allocateFields(newLabels, filledLabels, newData);
302  assert(newLabels == filledLabels);
303 
304  // fill
305  unsigned row(0);
306  for(BOOST_AUTO(it, newLabels.begin()); it != newLabels.end(); ++it)
307  {
308  Eigen::Block<MatrixType> view(newData.block(row, 0, it->span, newData.cols()));
309  view.leftCols(nbPoints1) = getViewByName(it->text, *labels, *data);
310  view.rightCols(nbPoints2) = getViewByName(it->text, extraLabels, extraData);
311  row += it->span;
312  }
313 
314  // swap descriptors
315  data->swap(newData);
316  *labels = newLabels;
317  }
318  else
319  {
320  *data = MatrixType();
321  *labels = Labels();
322  }
323  }
324 }
325 
327 template<typename T>
329 {
330  features.conservativeResize(Eigen::NoChange, pointCount);
331  if (descriptors.cols() > 0)
332  descriptors.conservativeResize(Eigen::NoChange, pointCount);
333 
334  if (times.cols() > 0)
335  times.conservativeResize(Eigen::NoChange, pointCount);
336 }
337 
339 template<typename T>
341 {
342  const int nbPoints(features.cols());
343  DataPoints output(
344  Matrix(features.rows(), nbPoints),
345  featureLabels
346  );
347 
348  assertDescriptorConsistency();
349  if (descriptors.cols() > 0)
350  {
351  output.descriptors = Matrix(descriptors.rows(), nbPoints);
352  output.descriptorLabels = descriptorLabels;
353  }
354 
355  assertTimesConsistency();
356  if (times.cols() > 0)
357  {
358  output.times = Int64Matrix(times.rows(), nbPoints);
359  output.timeLabels = timeLabels;
360  }
361 
362  return output;
363 }
364 
366 template<typename T>
368 {
369  DataPoints output(
370  Matrix(features.rows(), pointCount),
371  featureLabels
372  );
373 
374  assertDescriptorConsistency();
375  if (descriptors.cols() > 0)
376  {
377  output.descriptors = Matrix(descriptors.rows(), pointCount);
378  output.descriptorLabels = descriptorLabels;
379  }
380 
381  assertTimesConsistency();
382  if (times.cols() > 0)
383  {
384  output.times = Int64Matrix(times.rows(), pointCount);
385  output.timeLabels = timeLabels;
386  }
387 
388  return output;
389 }
390 
392 template<typename T>
394 {
395  features.col(thisCol) = that.features.col(thatCol);
396 
397  if (descriptors.cols() > 0)
398  descriptors.col(thisCol) = that.descriptors.col(thatCol);
399 
400  if (times.cols() > 0)
401  times.col(thisCol) = that.times.col(thatCol);
402 
403 }
405 template<typename T>
407 {
408  features.col(iCol).swap(features.col(jCol));
409  if (descriptors.cols() > 0)
410  descriptors.col(iCol).swap(descriptors.col(jCol));
411  if (times.cols() > 0)
412  times.col(iCol).swap(times.col(jCol));
413 }
414 
415 //------------------------------------
416 // Methods related to features
417 //------------------------------------
418 
419 
421 template<typename T>
423 {
424  allocateField(name, dim, featureLabels, features);
425 }
426 
428 template<typename T>
430 {
431  allocateFields(newLabels, featureLabels, features);
432 }
433 
435 template<typename T>
437 {
438  removeFeature("pad");
439  addField<Matrix>(name, newFeature, featureLabels, features);
440  addField<Matrix>("pad", Matrix::Ones(1, features.cols()), featureLabels, features);
441 }
442 
444 template<typename T>
446 {
447  removeField(name, featureLabels, features);
448 }
449 
451 template<typename T>
453 {
454  return Matrix(getFeatureViewByName(name));
455 }
456 
458 template<typename T>
460 {
461  return getConstViewByName(name, featureLabels, features);
462 }
463 
465 template<typename T>
467 {
468  return getViewByName(name, featureLabels, features);
469 }
470 
472 template<typename T>
474 {
475  return getConstViewByName(name, featureLabels, features, int(row));
476 }
477 
479 template<typename T>
481 {
482  return getViewByName(name, featureLabels, features, int(row));
483 }
484 
486 template<typename T>
488 {
489  return fieldExists(name, 0, featureLabels);
490 }
491 
493 template<typename T>
495 {
496  return fieldExists(name, dim, featureLabels);
497 }
498 
500 template<typename T>
502 {
503  return getFieldDimension(name, featureLabels);
504 }
505 
507 template<typename T>
509 {
510  return getFieldStartingRow(name, featureLabels);
511 }
512 
513 //------------------------------------
514 // Methods related to descriptors
515 //------------------------------------
516 
518 template<typename T>
520 {
521  allocateField(name, dim, descriptorLabels, descriptors);
522 }
523 
525 template<typename T>
527 {
528  allocateFields(newLabels, descriptorLabels, descriptors);
529 }
530 
532 template<typename T>
534 {
535  addField(name, newDescriptor, descriptorLabels, descriptors);
536 }
537 
539 template<typename T>
541 {
542  removeField(name, descriptorLabels, descriptors);
543 }
544 
545 
547 template<typename T>
549 {
550  return Matrix(getDescriptorViewByName(name));
551 }
552 
554 template<typename T>
556 {
557  return getConstViewByName(name, descriptorLabels, descriptors);
558 }
559 
561 template<typename T>
563 {
564  return getViewByName(name, descriptorLabels, descriptors);
565 }
566 
568 template<typename T>
570 {
571  return getConstViewByName(name, descriptorLabels, descriptors, int(row));
572 }
573 
575 template<typename T>
577 {
578  return getViewByName(name, descriptorLabels, descriptors, int(row));
579 }
580 
582 template<typename T>
584 {
585  return fieldExists(name, 0, descriptorLabels);
586 }
587 
589 template<typename T>
591 {
592  return fieldExists(name, dim, descriptorLabels);
593 }
594 
596 template<typename T>
598 {
599  return getFieldDimension(name, descriptorLabels);
600 }
601 
603 template<typename T>
605 {
606  return getFieldStartingRow(name, descriptorLabels);
607 }
608 
610 template<typename T>
612 {
613  assertConsistency("descriptors", descriptors.rows(), descriptors.cols(), descriptorLabels);
614 }
615 
616 //------------------------------------
617 // Methods related to time
618 //------------------------------------
619 
621 template<typename T>
623 {
624  allocateField(name, dim, timeLabels, times);
625 }
626 
628 template<typename T>
630 {
631  allocateFields(newLabels, timeLabels, times);
632 }
633 
634 
636 template<typename T>
638 {
639  addField(name, newTime, timeLabels, times);
640 }
641 
643 template<typename T>
645 {
646  removeField(name, timeLabels, times);
647 }
648 
650 template<typename T>
652 {
653  return Int64Matrix(getTimeViewByName(name));
654 }
655 
656 
658 template<typename T>
660 {
661  return getConstViewByName(name, timeLabels, times);
662 }
663 
665 template<typename T>
667 {
668  return getViewByName(name, timeLabels, times);
669 }
670 
672 template<typename T>
674 {
675  return getConstViewByName(name, timeLabels, times, int(row));
676 }
677 
679 template<typename T>
681 {
682  return getViewByName(name, timeLabels, times, int(row));
683 }
684 
685 
687 template<typename T>
689 {
690  return fieldExists(name, 0, timeLabels);
691 }
692 
694 template<typename T>
696 {
697  return fieldExists(name, dim, timeLabels);
698 }
699 
701 template<typename T>
703 {
704  return getFieldDimension(name, timeLabels);
705 }
706 
708 template<typename T>
710 {
711  return getFieldStartingRow(name, timeLabels);
712 }
713 
715 template<typename T>
717 {
718  assertConsistency("times", times.rows(), times.cols(), timeLabels);
719 }
720 
722 template<typename T>
723 void PointMatcher<T>::DataPoints::assertConsistency(const std::string& dataName, const int dataRows, const int dataCols, const Labels& labels) const
724 {
725  if (dataRows == 0)
726  {
727  if (dataCols != 0)
728  throw InvalidField(
729  (boost::format("Point cloud has degenerate %2% dimensions of rows=0, cols=%1%") % dataCols % dataName).str()
730  );
731  if (labels.size() > 0)
732  throw InvalidField(
733  (boost::format("Point cloud has no %2% data but %1% descriptor labels") % labels.size() % dataName).str()
734  );
735  }
736  else
737  {
738  if (dataCols != features.cols())
739  {
740  throw InvalidField(
741  (boost::format("Point cloud has %1% points in features but %2% points in %3%") % features.cols() % dataCols % dataName).str()
742  );
743  }
744 
745  int descDim(0);
746  for (BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
747  descDim += it->span;
748 
749  if (dataRows != descDim)
750  throw InvalidField(
751  (boost::format("Labels from %3% return %1% total dimensions but there are %2% in the %3% matrix") % descDim % dataRows % dataName).str()
752  );
753 
754  }
755 
756 }
757 
759 template<typename T>
760 template<typename MatrixType>
761 void PointMatcher<T>::DataPoints::allocateField(const std::string& name, const unsigned dim, Labels& labels, MatrixType& data) const
762 {
763  if (fieldExists(name, 0, labels))
764  {
765  const unsigned descDim(getFieldDimension(name, labels));
766  if (descDim != dim)
767  {
768  throw InvalidField(
769  (boost::format("The existing field %1% has dimension %2%, different than requested dimension %3%") % name % descDim % dim).str()
770  );
771  }
772  }
773  else
774  {
775  const int oldDim(data.rows());
776  const int totalDim(oldDim + dim);
777  const int pointCount(features.cols());
778  data.conservativeResize(totalDim, pointCount);
779  labels.push_back(Label(name, dim));
780  }
781 }
782 
784 template<typename T>
785 template<typename MatrixType>
786 void PointMatcher<T>::DataPoints::allocateFields(const Labels& newLabels, Labels& labels, MatrixType& data) const
787 {
788  typedef vector<bool> BoolVector;
789  BoolVector present(newLabels.size(), false);
790 
791  // for fields that exist, take note and check dimension
792  size_t additionalDim(0);
793  for (size_t i = 0; i < newLabels.size(); ++i)
794  {
795  const string& newName(newLabels[i].text);
796  const size_t newSpan(newLabels[i].span);
797  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
798  {
799  if (it->text == newName)
800  {
801  if (it->span != newSpan)
802  throw InvalidField(
803  (boost::format("The existing field %1% has dimension %2%, different than requested dimension %3%") % newName % it->span % newSpan).str()
804  );
805  present[i] = true;
806  break;
807  }
808  }
809  if (!present[i])
810  additionalDim += newSpan;
811  }
812 
813  // for new fields allocate
814  const int oldDim(data.rows());
815  const int totalDim(oldDim + additionalDim);
816  const int pointCount(features.cols());
817  data.conservativeResize(totalDim, pointCount);
818  for (size_t i = 0; i < newLabels.size(); ++i)
819  {
820  if (!present[i])
821  labels.push_back(newLabels[i]);
822  }
823 }
824 
826 template<typename T>
827 template<typename MatrixType>
828 void PointMatcher<T>::DataPoints::addField(const std::string& name, const MatrixType& newField, Labels& labels, MatrixType& data) const
829 {
830  const int newFieldDim = newField.rows();
831  const int newPointCount = newField.cols();
832  const int pointCount = features.cols();
833 
834  if (newField.rows() == 0)
835  return;
836 
837  // Replace if the field exists
838  if (fieldExists(name, 0, labels))
839  {
840  const int fieldDim = getFieldDimension(name, labels);
841 
842  if(fieldDim == newFieldDim)
843  {
844  // Ensure that the number of points in the point cloud and in the field are the same
845  if(pointCount == newPointCount)
846  {
847  const int row = getFieldStartingRow(name, labels);
848  data.block(row, 0, fieldDim, pointCount) = newField;
849  }
850  else
851  {
852  stringstream errorMsg;
853  errorMsg << "The field " << name << " cannot be added because the number of points is not the same. Old point count: " << pointCount << "new: " << newPointCount;
854  throw InvalidField(errorMsg.str());
855  }
856  }
857  else
858  {
859  stringstream errorMsg;
860  errorMsg << "The field " << name << " already exists but could not be added because the dimension is not the same. Old dim: " << fieldDim << " new: " << newFieldDim;
861  throw InvalidField(errorMsg.str());
862  }
863  }
864  else // Add at the end if it is a new field
865  {
866  if(pointCount == newPointCount || pointCount == 0)
867  {
868  const int oldFieldDim(data.rows());
869  const int totalDim = oldFieldDim + newFieldDim;
870  data.conservativeResize(totalDim, newPointCount);
871  data.bottomRows(newFieldDim) = newField;
872  labels.push_back(Label(name, newFieldDim));
873  }
874  else
875  {
876  stringstream errorMsg;
877  errorMsg << "The field " << name << " cannot be added because the number of points is not the same. Old point count: " << pointCount << " new: " << newPointCount;
878  throw InvalidField(errorMsg.str());
879  }
880  }
881 }
883 template<typename T>
884 template<typename MatrixType>
885 void PointMatcher<T>::DataPoints::removeField(const std::string& name, Labels& labels, MatrixType& data) const
886 {
887 
888  const unsigned deleteId = getFieldStartingRow(name, labels);
889  const unsigned span = getFieldDimension(name, labels);
890  const unsigned keepAfterId = deleteId + span;
891  const unsigned lastId = data.rows() - 1;
892  const unsigned sizeKeep = data.rows() - keepAfterId;
893  const unsigned nbPoints = data.cols();
894 
895 
896  // check if the data to be removed at the end
897  if(keepAfterId <= lastId)
898  {
899  data.block(deleteId, 0, sizeKeep, nbPoints) = data.block(keepAfterId, 0, sizeKeep, nbPoints);
900  }
901 
902  //Remove the last rows
903  data.conservativeResize(data.rows()-span, nbPoints);
904 
905  // remove label from the label list
906  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
907  {
908  if (it->text == name)
909  {
910  labels.erase(it);
911  break;
912  }
913  }
914 
915 }
916 
917 
920 template<typename T>
921 template<typename MatrixType>
922 const typename Eigen::Block<const MatrixType> PointMatcher<T>::DataPoints::getConstViewByName(const std::string& name, const Labels& labels, const MatrixType& data, const int viewRow) const
923 {
924  unsigned row(0);
925  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
926  {
927  if (it->text == name)
928  {
929  if (viewRow >= 0)
930  {
931  if (viewRow >= int(it->span))
932  throw InvalidField(
933  (boost::format("Requesting row %1% of field %2% that only has %3% rows") % viewRow % name % it->span).str()
934  );
935  return data.block(row + viewRow, 0, 1, data.cols());
936  }
937  else
938  return data.block(row, 0, it->span, data.cols());
939  }
940  row += it->span;
941  }
942  throw InvalidField("Field " + name + " not found");
943 }
944 
947 template<typename T>
948 template<typename MatrixType>
949 typename Eigen::Block<MatrixType> PointMatcher<T>::DataPoints::getViewByName(const std::string& name, const Labels& labels, MatrixType& data, const int viewRow) const
950 {
951  unsigned row(0);
952  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
953  {
954  if (it->text == name)
955  {
956  if (viewRow >= 0)
957  {
958  if (viewRow >= int(it->span))
959  throw InvalidField(
960  (boost::format("Requesting row %1% of field %2% that only has %3% rows") % viewRow % name % it->span).str()
961  );
962  return data.block(row + viewRow, 0, 1, data.cols());
963  }
964  else
965  return data.block(row, 0, it->span, data.cols());
966  }
967  row += it->span;
968  }
969  throw InvalidField("Field " + name + " not found");
970 }
971 
973 template<typename T>
974 bool PointMatcher<T>::DataPoints::fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const
975 {
976  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
977  {
978  if (it->text == name)
979  {
980  if (dim == 0 || it->span == dim)
981  return true;
982  else
983  return false;
984  }
985  }
986  return false;
987 }
988 
989 
991 template<typename T>
993 {
994  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
995  {
996  if (it->text == name)
997  return it->span;
998  }
999  return 0;
1000 }
1001 
1002 
1004 template<typename T>
1006 {
1007  unsigned row(0);
1008  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
1009  {
1010  if (it->text == name)
1011  return row;
1012  row += it->span;
1013  }
1014  return 0;
1015 }
1016 
1017 template struct PointMatcher<float>::DataPoints;
1018 template struct PointMatcher<double>::DataPoints;
1019 
1020 
1022 template<typename T>
1024 {
1025  a.features.swap(b.features);
1027  a.descriptors.swap(b.descriptors);
1029  a.times.swap(b.times);
1030  swap(a.timeLabels, b.timeLabels);
1031 }
1032 
1033 template
1035 template
Label
DataPoints::Label Label
Definition: pypoint_matcher_helper.h:17
PointMatcher::DataPoints::timeExists
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
Definition: pointmatcher/DataPoints.cpp:688
PointMatcher::DataPoints::getNbGroupedDescriptors
unsigned getNbGroupedDescriptors() const
Return the number of grouped descriptors (e.g., normals can have 3 components but would count as only...
Definition: pointmatcher/DataPoints.cpp:180
PointMatcher::DataPoints::Label::operator==
bool operator==(const Label &that) const
Return whether two labels are equals.
Definition: pointmatcher/DataPoints.cpp:57
PointMatcher::DataPoints::descriptorLabels
Labels descriptorLabels
labels of descriptors
Definition: PointMatcher.h:334
PointMatcher::DataPoints::swapCols
void swapCols(Index iCol, Index jCol)
Swap column i and j in the point cloud, swap also features and descriptors if any....
Definition: pointmatcher/DataPoints.cpp:406
PointMatcher::DataPoints::Label::text
std::string text
name of the label
Definition: PointMatcher.h:223
swap
void swap(linb::any &lhs, linb::any &rhs) noexcept
PointMatcher::DataPoints::getEuclideanDim
unsigned getEuclideanDim() const
Return the dimension of the point cloud.
Definition: pointmatcher/DataPoints.cpp:165
PointMatcher::DataPoints::addTime
void addTime(const std::string &name, const Int64Matrix &newTime)
Add a time by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:637
PointMatcher::DataPoints::assertDescriptorConsistency
void assertDescriptorConsistency() const
Assert if descriptors are not consistent with features.
Definition: pointmatcher/DataPoints.cpp:611
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
icp_customized.name
string name
Definition: icp_customized.py:45
PointMatcher::DataPoints::removeFeature
void removeFeature(const std::string &name)
Remove a feature by name, the whole matrix will be copied.
Definition: pointmatcher/DataPoints.cpp:445
PointMatcher::DataPoints::concatenate
void concatenate(const DataPoints &dp)
Add another point cloud after the current one. Faster merge will be achieved if all descriptor and ti...
Definition: pointmatcher/DataPoints.cpp:226
PointMatcher::DataPoints::addFeature
void addFeature(const std::string &name, const Matrix &newFeature)
Add a feature by name, remove first if already exists. The 'pad' field will stay at the end for homog...
Definition: pointmatcher/DataPoints.cpp:436
PointMatcher::DataPoints::allocateDescriptor
void allocateDescriptor(const std::string &name, const unsigned dim)
Makes sure a descriptor of a given name exists, if present, check its dimensions.
Definition: pointmatcher/DataPoints.cpp:519
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:583
PointMatcher::DataPoints::getViewByName
Eigen::Block< MatrixType > getViewByName(const std::string &name, const Labels &labels, MatrixType &data, const int viewRow=-1) const
Definition: pointmatcher/DataPoints.cpp:949
PointMatcher::DataPoints::addDescriptor
void addDescriptor(const std::string &name, const Matrix &newDescriptor)
Add a descriptor by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:533
PointMatcher::DataPoints::getConstViewByName
const Eigen::Block< const MatrixType > getConstViewByName(const std::string &name, const Labels &labels, const MatrixType &data, const int viewRow=-1) const
Definition: pointmatcher/DataPoints.cpp:922
PointMatcher::DataPoints::allocateFields
void allocateFields(const Labels &newLabels, Labels &labels, MatrixType &data) const
Make sure a vector of fields of given names exist.
Definition: pointmatcher/DataPoints.cpp:786
PointMatcher
Functions and classes that are dependant on scalar type are defined in this templatized class.
Definition: PointMatcher.h:130
PointMatcherPrivate.h
PointMatcher::DataPoints::getDescriptorCopyByName
Matrix getDescriptorCopyByName(const std::string &name) const
Get descriptor by name, return a matrix containing a copy of the requested descriptor.
Definition: pointmatcher/DataPoints.cpp:548
PointMatcher::DataPoints::getTimeDim
unsigned getTimeDim() const
Return the total number of times.
Definition: pointmatcher/DataPoints.cpp:194
PointMatcher::DataPoints::getTimeCopyByName
Int64Matrix getTimeCopyByName(const std::string &name) const
Get time by name, return a matrix containing a copy of the requested time.
Definition: pointmatcher/DataPoints.cpp:651
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
PointMatcher::DataPoints::allocateFeatures
void allocateFeatures(const Labels &newLabels)
Make sure a vector of features of given names exist.
Definition: pointmatcher/DataPoints.cpp:429
PointMatcher::DataPoints::getDescriptorStartingRow
unsigned getDescriptorStartingRow(const std::string &name) const
Return the starting row of a descriptor with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:604
PointMatcher::DataPoints::getHomogeneousDim
unsigned getHomogeneousDim() const
Return the dimension of the point cloud in homogeneous coordinates (one more than Euclidean dimension...
Definition: pointmatcher/DataPoints.cpp:173
testing::internal::string
::std::string string
Definition: gtest.h:1979
PointMatcher::DataPoints::timeLabels
Labels timeLabels
labels of times.
Definition: PointMatcher.h:336
PointMatcher::DataPoints::operator==
bool operator==(const DataPoints &that) const
Return whether two point-clouds are identical (same data and same labels)
Definition: pointmatcher/DataPoints.cpp:201
PointMatcher::DataPoints::getTimeDimension
unsigned getTimeDimension(const std::string &name) const
Return the dimension of a time with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:702
PointMatcher::DataPoints::getFeatureDimension
unsigned getFeatureDimension(const std::string &name) const
Return the dimension of a feature with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:501
PointMatcher::DataPoints::InvalidField
An exception thrown when one tries to access features or descriptors unexisting or of wrong dimension...
Definition: PointMatcher.h:250
PointMatcher::DataPoints::getFeatureViewByName
ConstView getFeatureViewByName(const std::string &name) const
Get a const view on a feature by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:459
PointMatcher::Int64Matrix
Eigen::Matrix< std::int64_t, Eigen::Dynamic, Eigen::Dynamic > Int64Matrix
A dense signed 64-bits matrix.
Definition: PointMatcher.h:173
PointMatcher::DataPoints::featureExists
bool featureExists(const std::string &name) const
Look if a feature with a given name exist.
Definition: pointmatcher/DataPoints.cpp:487
PointMatcher::DataPoints::removeField
void removeField(const std::string &name, Labels &labels, MatrixType &data) const
Remove a descriptor or feature by name, no copy is done.
Definition: pointmatcher/DataPoints.cpp:885
PointMatcher::DataPoints::allocateTime
void allocateTime(const std::string &name, const unsigned dim)
Makes sure a time of a given name exists, if present, check its dimensions.
Definition: pointmatcher/DataPoints.cpp:622
PointMatcher::DataPoints::allocateFeature
void allocateFeature(const std::string &name, const unsigned dim)
Makes sure a feature of a given name exists, if present, check its dimensions.
Definition: pointmatcher/DataPoints.cpp:422
PointMatcher::DataPoints::getFieldStartingRow
unsigned getFieldStartingRow(const std::string &name, const Labels &labels) const
Return the starting row of a feature or a descriptor with a given name. Return 0 if the name is not f...
Definition: pointmatcher/DataPoints.cpp:1005
PointMatcher::DataPoints::featureLabels
Labels featureLabels
labels of features
Definition: PointMatcher.h:332
PointMatcher::DataPoints::setColFrom
void setColFrom(Index thisCol, const DataPoints &that, Index thatCol)
Set column thisCol equal to column thatCol of that, copy features and descriptors if any....
Definition: pointmatcher/DataPoints.cpp:393
PointMatcher::DataPoints::createSimilarEmpty
DataPoints createSimilarEmpty() const
Create an empty DataPoints of similar dimensions and labels for features, descriptors and times.
Definition: pointmatcher/DataPoints.cpp:340
PointMatcher::DataPoints::concatenateLabelledMatrix
void concatenateLabelledMatrix(Labels *labels, MatrixType *data, const Labels extraLabels, const MatrixType extraData)
Add another matrix after the current one. Faster merge will be achieved if all labels are the same....
Definition: pointmatcher/DataPoints.cpp:258
PointMatcher::DataPoints::descriptors
Matrix descriptors
descriptors of points in the cloud, might be empty
Definition: PointMatcher.h:333
PointMatcher::DataPoints::TimeView
Eigen::Block< Int64Matrix > TimeView
A view on a time.
Definition: PointMatcher.h:212
icp.data
data
Definition: icp.py:50
PointMatcher::Matrix
Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
PointMatcher::DataPoints::getFeatureRowViewByName
ConstView getFeatureRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a feature row by name and number, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:473
PointMatcher::DataPoints::getTimeViewByName
TimeConstView getTimeViewByName(const std::string &name) const
Get a const view on a time by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:659
Labels
DataPoints::Labels Labels
Definition: pypoint_matcher_helper.h:18
PointMatcher::DataPoints::assertTimesConsistency
void assertTimesConsistency() const
Assert if times are not consistent with features.
Definition: pointmatcher/DataPoints.cpp:716
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
PointMatcher::DataPoints::allocateField
void allocateField(const std::string &name, const unsigned dim, Labels &labels, MatrixType &data) const
Make sure a field of a given name exists, if present, check its dimensions.
Definition: pointmatcher/DataPoints.cpp:761
PointMatcher::DataPoints::Label::Label
Label(const std::string &text="", const size_t span=0)
Construct a label from a given name and number of data dimensions it spans.
Definition: pointmatcher/DataPoints.cpp:44
PointMatcher::swapDataPoints
static void swapDataPoints(DataPoints &a, DataPoints &b)
Exchange in place point clouds a and b, with no data copy.
Definition: pointmatcher/DataPoints.cpp:1023
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
PointMatcher::DataPoints::Index
Matrix::Index Index
An index to a row or a column.
Definition: PointMatcher.h:218
PointMatcher::DataPoints::Labels::contains
bool contains(const std::string &text) const
Return whether there is a label named text.
Definition: pointmatcher/DataPoints.cpp:75
PointMatcher::DataPoints::times
Int64Matrix times
time associated to each points, might be empty
Definition: PointMatcher.h:335
PointMatcher::DataPoints::allocateTimes
void allocateTimes(const Labels &newLabels)
Make sure a vector of time of given names exist.
Definition: pointmatcher/DataPoints.cpp:629
PointMatcher::DataPoints::removeTime
void removeTime(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
Definition: pointmatcher/DataPoints.cpp:644
PointMatcher::DataPoints::assertConsistency
void assertConsistency(const std::string &dataName, const int dataRows, const int dataCols, const Labels &labels) const
Assert if a matrix is not consistent with features.
Definition: pointmatcher/DataPoints.cpp:723
std
PointMatcher::DataPoints::removeDescriptor
void removeDescriptor(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
Definition: pointmatcher/DataPoints.cpp:540
PointMatcher::DataPoints::getTimeStartingRow
unsigned getTimeStartingRow(const std::string &name) const
Return the starting row of a time with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:709
icp_advance_api.dim
dim
Definition: icp_advance_api.py:152
PointMatcher::DataPoints::Label::span
size_t span
number of data dimensions the label spans
Definition: PointMatcher.h:224
PointMatcher::DataPoints::Labels::Labels
Labels()
Construct empty Labels.
Definition: pointmatcher/DataPoints.cpp:64
PointMatcher::DataPoints::getDescriptorDimension
unsigned getDescriptorDimension(const std::string &name) const
Return the dimension of a descriptor with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:597
PointMatcher::DataPoints::getDescriptorDim
unsigned getDescriptorDim() const
Return the total number of descriptors.
Definition: pointmatcher/DataPoints.cpp:187
PointMatcher::DataPoints::View
Eigen::Block< Matrix > View
A view on a feature or descriptor.
Definition: PointMatcher.h:210
PointMatcher::DataPoints::conservativeResize
void conservativeResize(Index pointCount)
Resize the cloud to pointCount points, conserving existing ones.
Definition: pointmatcher/DataPoints.cpp:328
PointMatcher::DataPoints::getDescriptorRowViewByName
ConstView getDescriptorRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a descriptor row by name and number, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:569
PointMatcher::DataPoints::Labels::totalDim
size_t totalDim() const
Return the sum of the spans of each label.
Definition: pointmatcher/DataPoints.cpp:87
PointMatcher::DataPoints::DataPoints
DataPoints()
Construct an empty point cloud.
Definition: pointmatcher/DataPoints.cpp:97
PointMatcher::DataPoints::getFieldDimension
unsigned getFieldDimension(const std::string &name, const Labels &labels) const
Return the dimension of a feature or a descriptor with a given name. Return 0 if the name is not foun...
Definition: pointmatcher/DataPoints.cpp:992
PointMatcher::DataPoints::getTimeRowViewByName
TimeConstView getTimeRowViewByName(const std::string &name, const unsigned row) const
Get a const view on a time row by name and number, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:673
PointMatcher::DataPoints::fieldExists
bool fieldExists(const std::string &name, const unsigned dim, const Labels &labels) const
Look if a descriptor or a feature with a given name and dimension exist.
Definition: pointmatcher/DataPoints.cpp:974
build_map.end
end
Definition: build_map.py:79
PointMatcher.h
public interface
PointMatcher::DataPoints::addField
void addField(const std::string &name, const MatrixType &newField, Labels &labels, MatrixType &data) const
Add a descriptor or feature by name, remove first if already exists.
Definition: pointmatcher/DataPoints.cpp:828
PointMatcher::DataPoints::getDescriptorViewByName
ConstView getDescriptorViewByName(const std::string &name) const
Get a const view on a descriptor by name, throw an exception if it does not exist.
Definition: pointmatcher/DataPoints.cpp:555
PointMatcher::DataPoints::InvalidField::InvalidField
InvalidField(const std::string &reason)
Construct the exception with an error message.
Definition: pointmatcher/DataPoints.cpp:51
PointMatcher::DataPoints::getFeatureStartingRow
unsigned getFeatureStartingRow(const std::string &name) const
Return the starting row of a feature with a given name. Return 0 if the name is not found.
Definition: pointmatcher/DataPoints.cpp:508
PointMatcher::DataPoints::getFeatureCopyByName
Matrix getFeatureCopyByName(const std::string &name) const
Get feature by name, return a matrix containing a copy of the requested feature.
Definition: pointmatcher/DataPoints.cpp:452
PointMatcher::DataPoints::allocateDescriptors
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Definition: pointmatcher/DataPoints.cpp:526
PointMatcher::DataPoints::Labels::const_iterator
std::vector< Label >::const_iterator const_iterator
alias
Definition: PointMatcher.h:231
PointMatcher::DataPoints::Label
The name for a certain number of dim.
Definition: PointMatcher.h:221


libpointmatcher
Author(s):
autogenerated on Mon Jan 1 2024 03:24:42