VioKeyframeWindowMatchingAlgorithm.hpp

Go to the documentation of this file.

/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 * 
 *   * Redistributions of source code must retain the above copyright notice,
 *     this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright notice,
 *     this list of conditions and the following disclaimer in the documentation
 *     and/or other materials provided with the distribution.
 *   * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
 *     its contributors may be used to endorse or promote products derived from
 *     this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Created on: Oct 17, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_
#define INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_

#include <memory>

#include <okvis/DenseMatcher.hpp>
#include <okvis/MatchingAlgorithm.hpp>
#include <okvis/Estimator.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/triangulation/ProbabilisticStereoTriangulator.hpp>
#include <okvis/MultiFrame.hpp>
#include <brisk/internal/hamming.h>

namespace okvis {

template<class CAMERA_GEOMETRY_T>
class VioKeyframeWindowMatchingAlgorithm : public okvis::MatchingAlgorithm {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)

  typedef CAMERA_GEOMETRY_T camera_geometry_t;

  enum MatchingTypes {
    Match3D2D = 1,  
    Match2D2D = 2   
  };

  VioKeyframeWindowMatchingAlgorithm(okvis::Estimator& estimator,
                                     int matchingType, float distanceThreshold,
                                     bool usePoseUncertainty = true);

  virtual ~VioKeyframeWindowMatchingAlgorithm();

  void setFrames(uint64_t mfIdA, uint64_t mfIdB, size_t camIdA, size_t camIdB);

  void setMatchingType(int matchingType);

  virtual void doSetup();

  virtual size_t sizeA() const;
  virtual size_t sizeB() const;

  virtual float distanceThreshold() const;
  void setDistanceThreshold(float distanceThreshold);

  virtual bool skipA(size_t indexA) const {
    return skipA_[indexA];
  }

  virtual bool skipB(size_t indexB) const {
    return skipB_[indexB];
  }

  virtual float distance(size_t indexA, size_t indexB) const {
    OKVIS_ASSERT_LT_DBG(MatchingAlgorithm::Exception, indexA, sizeA(), "index A out of bounds");
    OKVIS_ASSERT_LT_DBG(MatchingAlgorithm::Exception, indexB, sizeB(), "index B out of bounds");
    const float dist = static_cast<float>(specificDescriptorDistance(
        frameA_->keypointDescriptor(camIdA_, indexA),
        frameB_->keypointDescriptor(camIdB_, indexB)));

    if (dist < distanceThreshold_) {
      if (verifyMatch(indexA, indexB))
        return dist;
    }
    return std::numeric_limits<float>::max();
  }

  bool verifyMatch(size_t indexA, size_t indexB) const;

  virtual void reserveMatches(size_t numMatches);

  virtual void setBestMatch(size_t indexA, size_t indexB, double distance);

  size_t numMatches();
  size_t numUncertainMatches();

  const okvis::Matches & getMatches() const;

  bool isRelativeUncertaintyValid() {
    return validRelativeUncertainty_;
  }

 private:
  okvis::Estimator* estimator_;

  uint64_t mfIdA_ = 0;
  uint64_t mfIdB_ = 0;
  size_t camIdA_ = 0;
  size_t camIdB_ = 0;

  std::shared_ptr<okvis::MultiFrame> frameA_;
  std::shared_ptr<okvis::MultiFrame> frameB_;

  float distanceThreshold_;

  okvis::kinematics::Transformation T_CaCb_;
  okvis::kinematics::Transformation T_CbCa_;
  okvis::kinematics::Transformation T_SaCa_;
  okvis::kinematics::Transformation T_SbCb_;
  okvis::kinematics::Transformation T_WSa_;
  okvis::kinematics::Transformation T_WSb_;
  okvis::kinematics::Transformation T_SaW_;
  okvis::kinematics::Transformation T_SbW_;
  okvis::kinematics::Transformation T_WCa_;
  okvis::kinematics::Transformation T_WCb_;
  okvis::kinematics::Transformation T_CaW_;
  okvis::kinematics::Transformation T_CbW_;

  size_t numMatches_ = 0;
  size_t numUncertainMatches_ = 0;

  double fA_ = 0;
  double fB_ = 0;

  int matchingType_;

  Eigen::Matrix<double, Eigen::Dynamic, 2> projectionsIntoB_;
  Eigen::Matrix<double, Eigen::Dynamic, 2> projectionsIntoBUncertainties_;

  std::vector<bool> skipA_;
  std::vector<bool> skipB_;

  Eigen::Vector3d pA_W_;
  Eigen::Vector3d pB_W_;

  std::vector<double> raySigmasA_;
  std::vector<double> raySigmasB_;

  okvis::triangulation::ProbabilisticStereoTriangulator<camera_geometry_t> probabilisticStereoTriangulator_;

  bool validRelativeUncertainty_ = false;
  bool usePoseUncertainty_ = false;

  // copy from BriskDescriptor.hpp
  uint32_t specificDescriptorDistance(
      const unsigned char * descriptorA,
      const unsigned char * descriptorB) const {
    OKVIS_ASSERT_TRUE_DBG(
        Exception, descriptorA != NULL && descriptorB != NULL,
        "Trying to compare a descriptor with a null description vector");

    return brisk::Hamming::PopcntofXORed(descriptorA, descriptorB, 3/*48 / 16*/);
  }
};

}

#endif /* INCLUDE_OKVIS_VIOKEYFRAMEWINDOWMATCHINGALGORITHM_HPP_ */