SfmTrack.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#include <gtsam/sfm/SfmTrack.h>

#include <iostream>

namespace gtsam {

void SfmTrack::print(const std::string& s) const {
  std::cout << "Track with " << measurements.size();
  std::cout << " measurements of point " << p << std::endl;
}

bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const {
  // check the 3D point
  if (!p.isApprox(sfmTrack.p)) {
    return false;
  }

  // check the RGB values
  if (r != sfmTrack.r || g != sfmTrack.g || b != sfmTrack.b) {
    return false;
  }

  // compare size of vectors for measurements and siftIndices
  if (numberMeasurements() != sfmTrack.numberMeasurements() ||
      siftIndices.size() != sfmTrack.siftIndices.size()) {
    return false;
  }

  // compare measurements (order sensitive)
  for (size_t idx = 0; idx < numberMeasurements(); ++idx) {
    SfmMeasurement measurement = measurements[idx];
    SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];

    if (measurement.first != otherMeasurement.first ||
        !measurement.second.isApprox(otherMeasurement.second)) {
      return false;
    }
  }

  // compare sift indices (order sensitive)
  for (size_t idx = 0; idx < siftIndices.size(); ++idx) {
    SiftIndex index = siftIndices[idx];
    SiftIndex otherIndex = sfmTrack.siftIndices[idx];

    if (index.first != otherIndex.first || index.second != otherIndex.second) {
      return false;
    }
  }

  return true;
}

}  // namespace gtsam