Graph.h

Go to the documentation of this file.

/*
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/RtabmapExp.h" // DLL export/import defines

#include <map>
#include <list>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/GeodeticCoords.h>

namespace rtabmap {
class Memory;

namespace graph {

// Graph utilities

bool RTABMAP_EXP exportPoses(
                const std::string & filePath,
                int format, // 0=Raw (*.txt), 1=RGBD-SLAM (*.txt), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
                const std::map<int, Transform> & poses,
                const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
                const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
                bool g2oRobust = false); // optional for format 4

bool RTABMAP_EXP importPoses(
                const std::string & filePath,
                int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
                std::map<int, Transform> & poses,
                std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
                std::map<int, double> * stamps = 0); // optional for format 1

bool RTABMAP_EXP exportGPS(
                const std::string & filePath,
                const std::map<int, GPS> & gpsValues,
                unsigned int rgba = 0xFFFFFFFF);

void RTABMAP_EXP calcKittiSequenceErrors(
                const std::vector<Transform> &poses_gt,
                const std::vector<Transform> &poses_result,
                float & t_err,
                float & r_err);

Transform RTABMAP_EXP 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);

std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
                std::multimap<int, Link> & links,
                int from,
                int to,
                bool checkBothWays = true);
std::multimap<int, int>::iterator RTABMAP_EXP findLink(
                std::multimap<int, int> & links,
                int from,
                int to,
                bool checkBothWays = true);
std::multimap<int, Link>::const_iterator RTABMAP_EXP findLink(
                const std::multimap<int, Link> & links,
                int from,
                int to,
                bool checkBothWays = true);
std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
                const std::multimap<int, int> & links,
                int from,
                int to,
                bool checkBothWays = true);

std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
                const std::multimap<int, Link> & links);
std::multimap<int, Link> RTABMAP_EXP filterLinks(
                const std::multimap<int, Link> & links,
                Link::Type filteredType);
std::map<int, Link> RTABMAP_EXP filterLinks(
                const std::map<int, Link> & links,
                Link::Type filteredType);

//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
                const std::map<int, Transform> & poses,
                const Transform & cameraPose,
                float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
                float verticalFOV = 45.0f,   // in degrees, yfov = atan((image_height/2)/fy)*2
                float nearClipPlaneDistance = 0.1f,
                float farClipPlaneDistance = 100.0f,
                bool negative = false);

std::map<int, Transform> RTABMAP_EXP radiusPosesFiltering(
                const std::map<int, Transform> & poses,
                float radius,
                float angle,
                bool keepLatest = true);

std::multimap<int, int> RTABMAP_EXP 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, //<parent ID, child ID>
                std::multimap<int, Link> & hyperLinks);

std::list<std::pair<int, Transform> > RTABMAP_EXP 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_EXP computePath(
                        const std::multimap<int, Link> & links,
                        int from,
                        int to,
                        bool updateNewCosts = false,
                        bool useSameCostForAllLinks = false);

std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
                int fromId,
                int toId,
                const Memory * memory,
                bool lookInDatabase = true,
                bool updateNewCosts = false,
                float linearVelocity = 0.0f,   // m/sec
                float angularVelocity = 0.0f); // rad/sec

int RTABMAP_EXP findNearestNode(
                const std::map<int, rtabmap::Transform> & nodes,
                const rtabmap::Transform & targetPose);

std::vector<int> RTABMAP_EXP findNearestNodes(
                const std::map<int, rtabmap::Transform> & nodes,
                const rtabmap::Transform & targetPose,
                int k);

std::map<int, float> RTABMAP_EXP getNodesInRadius(
                int nodeId,
                const std::map<int, Transform> & nodes,
                float radius);
std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
                int nodeId,
                const std::map<int, Transform> & nodes,
                float radius,
                float angle = 0.0f);

float RTABMAP_EXP computePathLength(
                const std::vector<std::pair<int, Transform> > & path,
                unsigned int fromIndex = 0,
                unsigned int toIndex = 0);

// assuming they are all linked in map order
float RTABMAP_EXP computePathLength(
                const std::map<int, Transform> & path);

std::list<std::map<int, Transform> > RTABMAP_EXP getPaths(
                std::map<int, Transform> poses,
                const std::multimap<int, Link> & links);


} /* namespace graph */

} /* namespace rtabmap */
#endif /* GRAPH_H_ */