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  return (features.rows() - 1);
168 }
169 
171 template<typename T>
173 {
174  return features.rows();
175 }
176 
178 template<typename T>
180 {
181  return descriptorLabels.size();
182 }
183 
185 template<typename T>
187 {
188  return descriptors.rows();
189 }
190 
192 template<typename T>
194 {
195  return times.rows();
196 }
197 
199 template<typename T>
201 {
202  // Note comparing matrix without the same dimensions trigger an assert
203  bool isEqual = false;
204  if((features.rows() == that.features.rows()) &&
205  (features.cols() == that.features.cols()) &&
206  (descriptors.rows() == that.descriptors.rows()) &&
207  (descriptors.cols() == that.descriptors.cols()) &&
208  (times.rows() == that.times.rows()) &&
209  (times.cols() == that.times.cols()))
210  {
211  isEqual = (features == that.features) &&
212  (featureLabels == that.featureLabels) &&
213  (descriptors == that.descriptors) &&
214  (descriptorLabels == that.descriptorLabels)&&
215  (times == that.times) &&
216  (timeLabels == that.timeLabels);
217  }
218 
219  return isEqual;
220 
221 }
222 
224 template<typename T>
226 {
227  const int nbPoints1 = this->features.cols();
228  const int nbPoints2 = dp.features.cols();
229  const int nbPointsTotal = nbPoints1 + nbPoints2;
230 
231  const int dimFeat = this->features.rows();
232  if(dimFeat != dp.features.rows())
233  {
234  stringstream errorMsg;
235  errorMsg << "Cannot concatenate DataPoints because the dimension of the features are not the same. Actual dimension: " << dimFeat << " New dimension: " << dp.features.rows();
236  throw InvalidField(errorMsg.str());
237  }
238 
239  // concatenate features
240  this->features.conservativeResize(Eigen::NoChange, nbPointsTotal);
241  this->features.rightCols(nbPoints2) = dp.features;
242 
243  // concatenate descriptors
244  concatenateLabelledMatrix(&descriptorLabels, &descriptors, dp.descriptorLabels, dp.descriptors);
245  assertDescriptorConsistency();
246 
247  // concatenate time
248  concatenateLabelledMatrix(&timeLabels, &times, dp.timeLabels, dp.times);
249  assertTimesConsistency();
250 
251 }
252 
253 
255 template<typename T>
256 template<typename MatrixType>
257 void PointMatcher<T>::DataPoints::concatenateLabelledMatrix(Labels* labels, MatrixType* data, const Labels extraLabels, const MatrixType extraData)
258 {
259  const int nbPoints1 = data->cols();
260  const int nbPoints2 = extraData.cols();
261  const int nbPointsTotal = nbPoints1 + nbPoints2;
262 
263 
264  if (*labels == extraLabels)
265  {
266  if (labels->size() > 0)
267  {
268  // same descriptors, fast merge
269  data->conservativeResize(Eigen::NoChange, nbPointsTotal);
270  data->rightCols(nbPoints2) = extraData;
271  }
272  }
273  else
274  {
275  // different descriptors, slow merge
276 
277  // collect labels to be kept
278  Labels newLabels;
279  for(BOOST_AUTO(it, labels->begin()); it != labels->end(); ++it)
280  {
281  for(BOOST_AUTO(jt, extraLabels.begin()); jt != extraLabels.end(); ++jt)
282  {
283  if (it->text == jt->text)
284  {
285  if (it->span != jt->span)
286  throw InvalidField(
287  (boost::format("The field %1% has dimension %2% in this, different than dimension %3% in that") % it->text % it->span % jt->span).str()
288  );
289  newLabels.push_back(*it);
290  break;
291  }
292  }
293  }
294 
295  // allocate new descriptors
296  if (newLabels.size() > 0)
297  {
298  MatrixType newData;
299  Labels filledLabels;
300  this->allocateFields(newLabels, filledLabels, newData);
301  assert(newLabels == filledLabels);
302 
303  // fill
304  unsigned row(0);
305  for(BOOST_AUTO(it, newLabels.begin()); it != newLabels.end(); ++it)
306  {
307  Eigen::Block<MatrixType> view(newData.block(row, 0, it->span, newData.cols()));
308  view.leftCols(nbPoints1) = getViewByName(it->text, *labels, *data);
309  view.rightCols(nbPoints2) = getViewByName(it->text, extraLabels, extraData);
310  row += it->span;
311  }
312 
313  // swap descriptors
314  data->swap(newData);
315  *labels = newLabels;
316  }
317  else
318  {
319  *data = MatrixType();
320  *labels = Labels();
321  }
322  }
323 }
324 
326 template<typename T>
328 {
329  features.conservativeResize(Eigen::NoChange, pointCount);
330  if (descriptors.cols() > 0)
331  descriptors.conservativeResize(Eigen::NoChange, pointCount);
332 
333  if (times.cols() > 0)
334  times.conservativeResize(Eigen::NoChange, pointCount);
335 }
336 
338 template<typename T>
340 {
341  const int nbPoints(features.cols());
342  DataPoints output(
343  Matrix(features.rows(), nbPoints),
344  featureLabels
345  );
346 
347  assertDescriptorConsistency();
348  if (descriptors.cols() > 0)
349  {
350  output.descriptors = Matrix(descriptors.rows(), nbPoints);
351  output.descriptorLabels = descriptorLabels;
352  }
353 
354  assertTimesConsistency();
355  if (times.cols() > 0)
356  {
357  output.times = Int64Matrix(times.rows(), nbPoints);
358  output.timeLabels = timeLabels;
359  }
360 
361  return output;
362 }
363 
365 template<typename T>
367 {
368  DataPoints output(
369  Matrix(features.rows(), pointCount),
370  featureLabels
371  );
372 
373  assertDescriptorConsistency();
374  if (descriptors.cols() > 0)
375  {
376  output.descriptors = Matrix(descriptors.rows(), pointCount);
377  output.descriptorLabels = descriptorLabels;
378  }
379 
380  assertTimesConsistency();
381  if (times.cols() > 0)
382  {
383  output.times = Int64Matrix(times.rows(), pointCount);
384  output.timeLabels = timeLabels;
385  }
386 
387  return output;
388 }
389 
391 template<typename T>
393 {
394  features.col(thisCol) = that.features.col(thatCol);
395 
396  if (descriptors.cols() > 0)
397  descriptors.col(thisCol) = that.descriptors.col(thatCol);
398 
399  if (times.cols() > 0)
400  times.col(thisCol) = that.times.col(thatCol);
401 
402 }
404 template<typename T>
406 {
407  features.col(iCol).swap(features.col(jCol));
408  if (descriptors.cols() > 0)
409  descriptors.col(iCol).swap(descriptors.col(jCol));
410  if (times.cols() > 0)
411  times.col(iCol).swap(times.col(jCol));
412 }
413 
414 //------------------------------------
415 // Methods related to features
416 //------------------------------------
417 
418 
420 template<typename T>
421 void PointMatcher<T>::DataPoints::allocateFeature(const std::string& name, const unsigned dim)
422 {
423  allocateField(name, dim, featureLabels, features);
424 }
425 
427 template<typename T>
429 {
430  allocateFields(newLabels, featureLabels, features);
431 }
432 
434 template<typename T>
435 void PointMatcher<T>::DataPoints::addFeature(const std::string& name, const Matrix& newFeature)
436 {
437  removeFeature("pad");
438  addField<Matrix>(name, newFeature, featureLabels, features);
439  addField<Matrix>("pad", Matrix::Ones(1, features.cols()), featureLabels, features);
440 }
441 
443 template<typename T>
445 {
446  removeField(name, featureLabels, features);
447 }
448 
450 template<typename T>
452 {
453  return Matrix(getFeatureViewByName(name));
454 }
455 
457 template<typename T>
459 {
460  return getConstViewByName(name, featureLabels, features);
461 }
462 
464 template<typename T>
466 {
467  return getViewByName(name, featureLabels, features);
468 }
469 
471 template<typename T>
473 {
474  return getConstViewByName(name, featureLabels, features, int(row));
475 }
476 
478 template<typename T>
480 {
481  return getViewByName(name, featureLabels, features, int(row));
482 }
483 
485 template<typename T>
487 {
488  return fieldExists(name, 0, featureLabels);
489 }
490 
492 template<typename T>
493 bool PointMatcher<T>::DataPoints::featureExists(const std::string& name, const unsigned dim) const
494 {
495  return fieldExists(name, dim, featureLabels);
496 }
497 
499 template<typename T>
501 {
502  return getFieldDimension(name, featureLabels);
503 }
504 
506 template<typename T>
508 {
509  return getFieldStartingRow(name, featureLabels);
510 }
511 
512 //------------------------------------
513 // Methods related to descriptors
514 //------------------------------------
515 
517 template<typename T>
519 {
520  allocateField(name, dim, descriptorLabels, descriptors);
521 }
522 
524 template<typename T>
526 {
527  allocateFields(newLabels, descriptorLabels, descriptors);
528 }
529 
531 template<typename T>
532 void PointMatcher<T>::DataPoints::addDescriptor(const std::string& name, const Matrix& newDescriptor)
533 {
534  addField(name, newDescriptor, descriptorLabels, descriptors);
535 }
536 
538 template<typename T>
540 {
541  removeField(name, descriptorLabels, descriptors);
542 }
543 
544 
546 template<typename T>
548 {
549  return Matrix(getDescriptorViewByName(name));
550 }
551 
553 template<typename T>
555 {
556  return getConstViewByName(name, descriptorLabels, descriptors);
557 }
558 
560 template<typename T>
562 {
563  return getViewByName(name, descriptorLabels, descriptors);
564 }
565 
567 template<typename T>
569 {
570  return getConstViewByName(name, descriptorLabels, descriptors, int(row));
571 }
572 
574 template<typename T>
576 {
577  return getViewByName(name, descriptorLabels, descriptors, int(row));
578 }
579 
581 template<typename T>
583 {
584  return fieldExists(name, 0, descriptorLabels);
585 }
586 
588 template<typename T>
589 bool PointMatcher<T>::DataPoints::descriptorExists(const std::string& name, const unsigned dim) const
590 {
591  return fieldExists(name, dim, descriptorLabels);
592 }
593 
595 template<typename T>
597 {
598  return getFieldDimension(name, descriptorLabels);
599 }
600 
602 template<typename T>
604 {
605  return getFieldStartingRow(name, descriptorLabels);
606 }
607 
609 template<typename T>
611 {
612  assertConsistency("descriptors", descriptors.rows(), descriptors.cols(), descriptorLabels);
613 }
614 
615 //------------------------------------
616 // Methods related to time
617 //------------------------------------
618 
620 template<typename T>
621 void PointMatcher<T>::DataPoints::allocateTime(const std::string& name, const unsigned dim)
622 {
623  allocateField(name, dim, timeLabels, times);
624 }
625 
627 template<typename T>
629 {
630  allocateFields(newLabels, timeLabels, times);
631 }
632 
633 
635 template<typename T>
637 {
638  addField(name, newTime, timeLabels, times);
639 }
640 
642 template<typename T>
644 {
645  removeField(name, timeLabels, times);
646 }
647 
649 template<typename T>
651 {
652  return Int64Matrix(getTimeViewByName(name));
653 }
654 
655 
657 template<typename T>
659 {
660  return getConstViewByName(name, timeLabels, times);
661 }
662 
664 template<typename T>
666 {
667  return getViewByName(name, timeLabels, times);
668 }
669 
671 template<typename T>
673 {
674  return getConstViewByName(name, timeLabels, times, int(row));
675 }
676 
678 template<typename T>
680 {
681  return getViewByName(name, timeLabels, times, int(row));
682 }
683 
684 
686 template<typename T>
688 {
689  return fieldExists(name, 0, timeLabels);
690 }
691 
693 template<typename T>
694 bool PointMatcher<T>::DataPoints::timeExists(const std::string& name, const unsigned dim) const
695 {
696  return fieldExists(name, dim, timeLabels);
697 }
698 
700 template<typename T>
702 {
703  return getFieldDimension(name, timeLabels);
704 }
705 
707 template<typename T>
709 {
710  return getFieldStartingRow(name, timeLabels);
711 }
712 
714 template<typename T>
716 {
717  assertConsistency("times", times.rows(), times.cols(), timeLabels);
718 }
719 
721 template<typename T>
722 void PointMatcher<T>::DataPoints::assertConsistency(const std::string& dataName, const int dataRows, const int dataCols, const Labels& labels) const
723 {
724  if (dataRows == 0)
725  {
726  if (dataCols != 0)
727  throw InvalidField(
728  (boost::format("Point cloud has degenerate %2% dimensions of rows=0, cols=%1%") % dataCols % dataName).str()
729  );
730  if (labels.size() > 0)
731  throw InvalidField(
732  (boost::format("Point cloud has no %2% data but %1% descriptor labels") % labels.size() % dataName).str()
733  );
734  }
735  else
736  {
737  if (dataCols != features.cols())
738  {
739  throw InvalidField(
740  (boost::format("Point cloud has %1% points in features but %2% points in %3%") % features.cols() % dataCols % dataName).str()
741  );
742  }
743 
744  int descDim(0);
745  for (BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
746  descDim += it->span;
747 
748  if (dataRows != descDim)
749  throw InvalidField(
750  (boost::format("Labels from %3% return %1% total dimensions but there are %2% in the %3% matrix") % descDim % dataRows % dataName).str()
751  );
752 
753  }
754 
755 }
756 
758 template<typename T>
759 template<typename MatrixType>
760 void PointMatcher<T>::DataPoints::allocateField(const std::string& name, const unsigned dim, Labels& labels, MatrixType& data) const
761 {
762  if (fieldExists(name, 0, labels))
763  {
764  const unsigned descDim(getFieldDimension(name, labels));
765  if (descDim != dim)
766  {
767  throw InvalidField(
768  (boost::format("The existing field %1% has dimension %2%, different than requested dimension %3%") % name % descDim % dim).str()
769  );
770  }
771  }
772  else
773  {
774  const int oldDim(data.rows());
775  const int totalDim(oldDim + dim);
776  const int pointCount(features.cols());
777  data.conservativeResize(totalDim, pointCount);
778  labels.push_back(Label(name, dim));
779  }
780 }
781 
783 template<typename T>
784 template<typename MatrixType>
785 void PointMatcher<T>::DataPoints::allocateFields(const Labels& newLabels, Labels& labels, MatrixType& data) const
786 {
787  typedef vector<bool> BoolVector;
788  BoolVector present(newLabels.size(), false);
789 
790  // for fields that exist, take note and check dimension
791  size_t additionalDim(0);
792  for (size_t i = 0; i < newLabels.size(); ++i)
793  {
794  const string& newName(newLabels[i].text);
795  const size_t newSpan(newLabels[i].span);
796  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
797  {
798  if (it->text == newName)
799  {
800  if (it->span != newSpan)
801  throw InvalidField(
802  (boost::format("The existing field %1% has dimension %2%, different than requested dimension %3%") % newName % it->span % newSpan).str()
803  );
804  present[i] = true;
805  break;
806  }
807  }
808  if (!present[i])
809  additionalDim += newSpan;
810  }
811 
812  // for new fields allocate
813  const int oldDim(data.rows());
814  const int totalDim(oldDim + additionalDim);
815  const int pointCount(features.cols());
816  data.conservativeResize(totalDim, pointCount);
817  for (size_t i = 0; i < newLabels.size(); ++i)
818  {
819  if (!present[i])
820  labels.push_back(newLabels[i]);
821  }
822 }
823 
825 template<typename T>
826 template<typename MatrixType>
827 void PointMatcher<T>::DataPoints::addField(const std::string& name, const MatrixType& newField, Labels& labels, MatrixType& data) const
828 {
829  const int newFieldDim = newField.rows();
830  const int newPointCount = newField.cols();
831  const int pointCount = features.cols();
832 
833  if (newField.rows() == 0)
834  return;
835 
836  // Replace if the field exists
837  if (fieldExists(name, 0, labels))
838  {
839  const int fieldDim = getFieldDimension(name, labels);
840 
841  if(fieldDim == newFieldDim)
842  {
843  // Ensure that the number of points in the point cloud and in the field are the same
844  if(pointCount == newPointCount)
845  {
846  const int row = getFieldStartingRow(name, labels);
847  data.block(row, 0, fieldDim, pointCount) = newField;
848  }
849  else
850  {
851  stringstream errorMsg;
852  errorMsg << "The field " << name << " cannot be added because the number of points is not the same. Old point count: " << pointCount << "new: " << newPointCount;
853  throw InvalidField(errorMsg.str());
854  }
855  }
856  else
857  {
858  stringstream errorMsg;
859  errorMsg << "The field " << name << " already exists but could not be added because the dimension is not the same. Old dim: " << fieldDim << " new: " << newFieldDim;
860  throw InvalidField(errorMsg.str());
861  }
862  }
863  else // Add at the end if it is a new field
864  {
865  if(pointCount == newPointCount || pointCount == 0)
866  {
867  const int oldFieldDim(data.rows());
868  const int totalDim = oldFieldDim + newFieldDim;
869  data.conservativeResize(totalDim, newPointCount);
870  data.bottomRows(newFieldDim) = newField;
871  labels.push_back(Label(name, newFieldDim));
872  }
873  else
874  {
875  stringstream errorMsg;
876  errorMsg << "The field " << name << " cannot be added because the number of points is not the same. Old point count: " << pointCount << " new: " << newPointCount;
877  throw InvalidField(errorMsg.str());
878  }
879  }
880 }
882 template<typename T>
883 template<typename MatrixType>
884 void PointMatcher<T>::DataPoints::removeField(const std::string& name, Labels& labels, MatrixType& data) const
885 {
886 
887  const unsigned deleteId = getFieldStartingRow(name, labels);
888  const unsigned span = getFieldDimension(name, labels);
889  const unsigned keepAfterId = deleteId + span;
890  const unsigned lastId = data.rows() - 1;
891  const unsigned sizeKeep = data.rows() - keepAfterId;
892  const unsigned nbPoints = data.cols();
893 
894 
895  // check if the data to be removed at the end
896  if(keepAfterId <= lastId)
897  {
898  data.block(deleteId, 0, sizeKeep, nbPoints) = data.block(keepAfterId, 0, sizeKeep, nbPoints);
899  }
900 
901  //Remove the last rows
902  data.conservativeResize(data.rows()-span, nbPoints);
903 
904  // remove label from the label list
905  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
906  {
907  if (it->text == name)
908  {
909  labels.erase(it);
910  break;
911  }
912  }
913 
914 }
915 
916 
919 template<typename T>
920 template<typename MatrixType>
921 const typename Eigen::Block<const MatrixType> PointMatcher<T>::DataPoints::getConstViewByName(const std::string& name, const Labels& labels, const MatrixType& data, const int viewRow) const
922 {
923  unsigned row(0);
924  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
925  {
926  if (it->text == name)
927  {
928  if (viewRow >= 0)
929  {
930  if (viewRow >= int(it->span))
931  throw InvalidField(
932  (boost::format("Requesting row %1% of field %2% that only has %3% rows") % viewRow % name % it->span).str()
933  );
934  return data.block(row + viewRow, 0, 1, data.cols());
935  }
936  else
937  return data.block(row, 0, it->span, data.cols());
938  }
939  row += it->span;
940  }
941  throw InvalidField("Field " + name + " not found");
942 }
943 
946 template<typename T>
947 template<typename MatrixType>
948 typename Eigen::Block<MatrixType> PointMatcher<T>::DataPoints::getViewByName(const std::string& name, const Labels& labels, MatrixType& data, const int viewRow) const
949 {
950  unsigned row(0);
951  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
952  {
953  if (it->text == name)
954  {
955  if (viewRow >= 0)
956  {
957  if (viewRow >= int(it->span))
958  throw InvalidField(
959  (boost::format("Requesting row %1% of field %2% that only has %3% rows") % viewRow % name % it->span).str()
960  );
961  return data.block(row + viewRow, 0, 1, data.cols());
962  }
963  else
964  return data.block(row, 0, it->span, data.cols());
965  }
966  row += it->span;
967  }
968  throw InvalidField("Field " + name + " not found");
969 }
970 
972 template<typename T>
973 bool PointMatcher<T>::DataPoints::fieldExists(const std::string& name, const unsigned dim, const Labels& labels) const
974 {
975  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
976  {
977  if (it->text == name)
978  {
979  if (dim == 0 || it->span == dim)
980  return true;
981  else
982  return false;
983  }
984  }
985  return false;
986 }
987 
988 
990 template<typename T>
991 unsigned PointMatcher<T>::DataPoints::getFieldDimension(const std::string& name, const Labels& labels) const
992 {
993  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
994  {
995  if (it->text == name)
996  return it->span;
997  }
998  return 0;
999 }
1000 
1001 
1003 template<typename T>
1004 unsigned PointMatcher<T>::DataPoints::getFieldStartingRow(const std::string& name, const Labels& labels) const
1005 {
1006  unsigned row(0);
1007  for(BOOST_AUTO(it, labels.begin()); it != labels.end(); ++it)
1008  {
1009  if (it->text == name)
1010  return row;
1011  row += it->span;
1012  }
1013  return 0;
1014 }
1015 
1016 template struct PointMatcher<float>::DataPoints;
1017 template struct PointMatcher<double>::DataPoints;
1018 
1019 
1021 template<typename T>
1022 void PointMatcher<T>::swapDataPoints(DataPoints& a, DataPoints& b)
1023 {
1024  a.features.swap(b.features);
1025  swap(a.featureLabels, b.featureLabels);
1026  a.descriptors.swap(b.descriptors);
1027  swap(a.descriptorLabels, b.descriptorLabels);
1028  a.times.swap(b.times);
1029  swap(a.timeLabels, b.timeLabels);
1030 }
1031 
1032 template
1033 void PointMatcher<float>::swapDataPoints(DataPoints& a, DataPoints& b);
1034 template
1035 void PointMatcher<double>::swapDataPoints(DataPoints& a, DataPoints& b);
PointMatcher::DataPoints::timeExists
bool timeExists(const std::string &name) const
Look if a time with a given name exist.
Definition: pointmatcher/DataPoints.cpp:687
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:179
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:405
PointMatcher::DataPoints::Label::text
std::string text
name of the label
Definition: PointMatcher.h:223
std::swap
void swap(linb::any &lhs, linb::any &rhs) noexcept
Definition: any.hpp:452
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:636
PointMatcher::DataPoints::assertDescriptorConsistency
void assertDescriptorConsistency() const
Assert if descriptors are not consistent with features.
Definition: pointmatcher/DataPoints.cpp:610
PointMatcher::DataPoints::Labels
A vector of Label.
Definition: PointMatcher.h:229
PointMatcher::DataPoints::removeFeature
void removeFeature(const std::string &name)
Remove a feature by name, the whole matrix will be copied.
Definition: pointmatcher/DataPoints.cpp:444
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:225
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:435
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:518
PointMatcher::DataPoints::descriptorExists
bool descriptorExists(const std::string &name) const
Look if a descriptor with a given name exist.
Definition: pointmatcher/DataPoints.cpp:582
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:948
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:532
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:921
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:785
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:547
PointMatcher::DataPoints::getTimeDim
unsigned getTimeDim() const
Return the total number of times.
Definition: pointmatcher/DataPoints.cpp:193
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:650
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:428
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:603
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:172
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:200
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:701
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:500
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:458
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:486
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:884
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:621
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:421
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:1004
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:392
PointMatcher::DataPoints::createSimilarEmpty
DataPoints createSimilarEmpty() const
Create an empty DataPoints of similar dimensions and labels for features, descriptors and times.
Definition: pointmatcher/DataPoints.cpp:339
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:257
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
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:472
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:658
PointMatcher::DataPoints::assertTimesConsistency
void assertTimesConsistency() const
Assert if times are not consistent with features.
Definition: pointmatcher/DataPoints.cpp:715
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:760
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:1022
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:628
PointMatcher::DataPoints::removeTime
void removeTime(const std::string &name)
Remove a descriptor by name, the whole matrix will be copied.
Definition: pointmatcher/DataPoints.cpp:643
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:722
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:539
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:708
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:596
PointMatcher::DataPoints::getDescriptorDim
unsigned getDescriptorDim() const
Return the total number of descriptors.
Definition: pointmatcher/DataPoints.cpp:186
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:327
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:568
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:991
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:672
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:973
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:827
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:554
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:507
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:451
PointMatcher::DataPoints::allocateDescriptors
void allocateDescriptors(const Labels &newLabels)
Make sure a vector of descriptors of given names exist.
Definition: pointmatcher/DataPoints.cpp:525
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


mp2p_icp
Author(s):
autogenerated on Wed Oct 23 2024 02:45:38