Graph.h

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
 
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 the Universite de Sherbrooke 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 HOLDER 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.
*/
 
#ifndef GRAPH_H_
#define GRAPH_H_
 
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
#include <map>
#include <list>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/GPS.h>
#include <rtabmap/core/CameraModel.h>
 
namespace rtabmap {
class Memory;
 
namespace graph {
 
bool RTABMAP_CORE_EXPORT exportPoses(
    const std::string & filePath,
    int format,
    const std::map<int, Transform> & poses,
    const std::multimap<int, Link> & constraints = std::multimap<int, Link>(),
    const std::map<int, double> & stamps = std::map<int, double>(),
    const ParametersMap & parameters = ParametersMap());
 
bool RTABMAP_CORE_EXPORT importPoses(
    const std::string & filePath,
    int format,
    std::map<int, Transform> & poses,
    std::multimap<int, Link> * constraints = 0,
    std::map<int, double> * stamps = 0);
 
bool RTABMAP_CORE_EXPORT exportGPS(
    const std::string & filePath,
    const std::map<int, GPS> & gpsValues,
    unsigned int rgba = 0xFFFFFFFF);
 
void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
    const std::vector<Transform> &poses_gt,
    const std::vector<Transform> &poses_result,
    float & t_err,
    float & r_err);
 
void RTABMAP_CORE_EXPORT calcRelativeErrors (
    const std::vector<Transform> &poses_gt,
    const std::vector<Transform> &poses_result,
    float & t_err,
    float & r_err);
 
Transform RTABMAP_CORE_EXPORT calcRMSE(
    const std::map<int, Transform> &groundTruth,
    const std::map<int, Transform> &poses,
    float & translational_rmse,
    float & translational_mean,
    float & translational_median,
    float & translational_std,
    float & translational_min,
    float & translational_max,
    float & rotational_rmse,
    float & rotational_mean,
    float & rotational_median,
    float & rotational_std,
    float & rotational_min,
    float & rotational_max,
    bool align2D = false);
 
struct MaxGraphErrors
{
  float linear=-1.0f;         
  float angular=-1.0f;        
  float linearRatio=-1.0f;    
  float angularRatio=-1.0f;   
  Link linearLink;            
  Link angularLink;           
};
 
MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
    const std::map<int, Transform> & poses,
    const std::multimap<int, Link> & links,
    bool for3DoF = false);
 
std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
 
std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
    std::multimap<int, Link> & links,
    int from,
    int to,
    bool checkBothWays = true,
    Link::Type type = Link::kUndef);
std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
    std::multimap<int, std::pair<int, Link::Type> > & links,
    int from,
    int to,
    bool checkBothWays = true,
    Link::Type type = Link::kUndef);
std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
    std::multimap<int, int> & links,
    int from,
    int to,
    bool checkBothWays = true);
std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
    const std::multimap<int, Link> & links,
    int from,
    int to,
    bool checkBothWays = true,
    Link::Type type = Link::kUndef);
std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
    const std::multimap<int, std::pair<int, Link::Type> > & links,
    int from,
    int to,
    bool checkBothWays = true,
    Link::Type type = Link::kUndef);
std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
    const std::multimap<int, int> & links,
    int from,
    int to,
    bool checkBothWays = true);
 
std::list<Link> RTABMAP_CORE_EXPORT findLinks(
    const std::multimap<int, Link> & links,
    int from);
 
std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
    const std::multimap<int, Link> & links);
 
std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
    const std::multimap<int, Link> & links,
    Link::Type filteredType,
    bool inverted = false);
std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
    const std::map<int, Link> & links,
    Link::Type filteredType,
    bool inverted = false);
 
std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
    const std::map<int, Transform> & poses,
    const Transform & cameraPose,
    float horizontalFOV = 45.0f,
    float verticalFOV = 45.0f,
    float nearClipPlaneDistance = 0.1f,
    float farClipPlaneDistance = 100.0f,
    bool negative = false);
 
std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
    const std::map<int, Transform> & poses,
    float radius,
    float angle,
    bool keepLatest = true);
 
std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
    const std::map<int, Transform> & poses,
    float radius,
    float angle);
 
void reduceGraph(
    const std::map<int, Transform> & poses,
    const std::multimap<int, Link> & links,
    std::multimap<int, int> & hyperNodes,
    std::multimap<int, Link> & hyperLinks);
 
std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
      const std::map<int, rtabmap::Transform> & poses,
      const std::multimap<int, int> & links,
      int from,
      int to,
      bool updateNewCosts = false);
 
std::list<int> RTABMAP_CORE_EXPORT computePath(
      const std::multimap<int, Link> & links,
      int from,
      int to,
      bool updateNewCosts = false,
      bool useSameCostForAllLinks = false);
 
std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
    int fromId,
    int toId,
    const Memory * memory,
    bool lookInDatabase = true,
    bool updateNewCosts = false,
    float linearVelocity = 0.0f,
    float angularVelocity = 0.0f,
    bool ignoreDirectLinks = false);
 
int RTABMAP_CORE_EXPORT findNearestNode(
    const std::map<int, rtabmap::Transform> & poses,
    const rtabmap::Transform & targetPose,
    float * distance = 0);
 
std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
    int nodeId,
    const std::map<int, Transform> & poses,
    float radius,
    float angle = 0.0f,
    int k=0);
std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
    const Transform & targetPose,
    const std::map<int, Transform> & poses,
    float radius,
    float angle = 0.0f,
    int k=0);
std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
    int nodeId,
    const std::map<int, Transform> & poses,
    float radius,
    float angle = 0.0f,
    int k=0);
std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
    const Transform & targetPose,
    const std::map<int, Transform> & poses,
    float radius,
    float angle = 0.0f,
    int k=0);
 
RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
 
float RTABMAP_CORE_EXPORT computePathLength(
    const std::vector<std::pair<int, Transform> > & path);
 
float RTABMAP_CORE_EXPORT computePathLength(
    const std::map<int, Transform> & path);
 
std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
    std::map<int, Transform> poses,
    const std::multimap<int, Link> & links);
 
void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
    cv::Vec3f & min,
    cv::Vec3f & max);
 
} /* namespace graph */
 
} /* namespace rtabmap */
#endif /* GRAPH_H_ */