FrameTypedefs.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: Mar 27, 2013
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *    Modified: Andreas Forster (an.forster@gmail.com)
 *********************************************************************************/

#ifndef INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_
#define INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_

#include <map>

#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>

namespace okvis {

struct KeypointIdentifier
{
  KeypointIdentifier(uint64_t fi = 0, size_t ci = 0, size_t ki = 0)
      : frameId(fi),
        cameraIndex(ci),
        keypointIndex(ki)
  {
  }

  uint64_t frameId;     
  size_t cameraIndex;   
  size_t keypointIndex; 

  uint64_t getFrameId()
  {
    return frameId;
  }
  void setFrameId(uint64_t fid)
  {
    frameId = fid;
  }
  bool isBinaryEqual(const KeypointIdentifier & rhs) const
  {
    return frameId == rhs.frameId && cameraIndex == rhs.cameraIndex
        && keypointIndex == rhs.keypointIndex;
  }
  bool operator==(const KeypointIdentifier & rhs) const
  {
    return isBinaryEqual(rhs);
  }
  bool operator<(const KeypointIdentifier & rhs) const
  {

    if (frameId == rhs.frameId) {
      if (cameraIndex == rhs.cameraIndex) {
        return keypointIndex < rhs.keypointIndex;
      } else {
        return cameraIndex < rhs.cameraIndex;
      }
    }
    return frameId < rhs.frameId;
  }

};

struct Match
{
  Match(size_t idxA_, size_t idxB_, float distance_)
      : idxA(idxA_),
        idxB(idxB_),
        distance(distance_)
  {
  }
  size_t idxA;    
  size_t idxB;    
  float distance; 
};
typedef std::vector<Match> Matches;

struct MapPoint
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  MapPoint()
      : id(0),
        quality(0.0),
        distance(0.0)
  {
  }
  MapPoint(uint64_t id, const Eigen::Vector4d & point,
           double quality, double distance)
      : id(id),
        point(point),
        quality(quality),
        distance(distance)
  {
  }
  uint64_t id;            
  Eigen::Vector4d point;  
  double quality;         
  double distance;        
  std::map<okvis::KeypointIdentifier, uint64_t> observations;   
};

typedef std::vector<MapPoint, Eigen::aligned_allocator<MapPoint> > MapPointVector;
typedef std::map<uint64_t, MapPoint, std::less<uint64_t>,
    Eigen::aligned_allocator<MapPoint> > PointMap;
typedef std::map<uint64_t, okvis::kinematics::Transformation, std::less<uint64_t>,
    Eigen::aligned_allocator<okvis::kinematics::Transformation> > TransformationMap;

struct Observation
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Observation(size_t keypointIdx,
              const Eigen::Vector2d& keypointMeasurement,
              double keypointSize,
              size_t cameraIdx,
              uint64_t frameId,
              const Eigen::Vector4d& landmark_W,
              uint64_t landmarkId, bool isInitialized)
      : keypointIdx(keypointIdx),
        cameraIdx(cameraIdx),
        frameId(frameId),
        keypointMeasurement(keypointMeasurement),
        keypointSize(keypointSize),
        landmark_W(landmark_W),
        landmarkId(landmarkId),
        isInitialized(isInitialized)
  {
  }
  Observation()
      : keypointIdx(0),
        cameraIdx(-1),
        frameId(0),
        keypointSize(0),
        landmarkId(0),
        isInitialized(false)
  {
  }
  size_t keypointIdx; 
  size_t cameraIdx;  
  uint64_t frameId;  
  Eigen::Vector2d keypointMeasurement;  
  double keypointSize;  
  Eigen::Vector4d landmark_W;  
  uint64_t landmarkId;  
  bool isInitialized;   
};
typedef std::vector<Observation, Eigen::aligned_allocator<Observation> > ObservationVector;

// todo: find a better place for this
typedef Eigen::Matrix<double, 9, 1> SpeedAndBiases;
typedef Eigen::Matrix<double, 9, 1> SpeedAndBias;

}  // namespace okvis

#endif /* INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_ */