mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 10:00:23 +08:00
9c1e117384
* Sparse Bayes * updated perf test * improved tests with real data * Making sparse works in incremental mapping * bookkeeping optimization * small opt * refactoring * splitting dense and sparse in different classes to make the code more lisible * cleanup comments * fixing CI * Making all Bayes tests testing both dense and sparse * Added multisession_3it integration test (test memory management, multisession and dense/sparse bayes in that settings) * optimized sparse when transfer/retrieval happens (was slower than dense for that case) * Testing retrieval param variants * Updated multisession_3it integration tests to compare loop closure hypotheses * bump version * Fixed ui sum of prediction * adding g2o gtsam to linux ci * cleanup * added debug crash log for ci * Simplified Bayes/SparsePrediction description * Dont show too dense for sparse on small maps (e.g., when we just started a new map) * fixing amd64v3 issue with gtsam on ci ubuntu 26 * Dot not auto switch to dense based on map size. * updating test range * added coverage tests * Adressing coverage * ignore one line in coverage for purpose
1516 lines
51 KiB
C++
1516 lines
51 KiB
C++
#include <gtest/gtest.h>
|
||
#include <rtabmap/core/Graph.h>
|
||
#include <rtabmap/core/Link.h>
|
||
#include <rtabmap/core/Optimizer.h>
|
||
#include <rtabmap/core/Parameters.h>
|
||
#include <rtabmap/utilite/UFile.h>
|
||
#include "TestUtils.h"
|
||
#include <cmath>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <fstream>
|
||
#include <sstream>
|
||
|
||
using namespace rtabmap;
|
||
|
||
namespace {
|
||
|
||
static cv::Mat infMatrixDiagonal(double x, double y, double z, double roll, double pitch, double yaw)
|
||
{
|
||
cv::Mat inf = cv::Mat::zeros(6, 6, CV_64FC1);
|
||
inf.at<double>(0, 0) = x;
|
||
inf.at<double>(1, 1) = y;
|
||
inf.at<double>(2, 2) = z;
|
||
inf.at<double>(3, 3) = roll;
|
||
inf.at<double>(4, 4) = pitch;
|
||
inf.at<double>(5, 5) = yaw;
|
||
return inf;
|
||
}
|
||
|
||
static Link neighborLink(int from, int to, float dx = 1.0f)
|
||
{
|
||
return Link(from, to, Link::kNeighbor, Transform(dx, 0, 0, 0, 0, 0));
|
||
}
|
||
|
||
static void insertLink(std::multimap<int, Link> & links, const Link & link)
|
||
{
|
||
links.insert(std::make_pair(link.from(), link));
|
||
}
|
||
|
||
static std::map<int, Transform> linePoses(unsigned int count, float step = 1.0f)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
for(unsigned int i = 0; i < count; ++i)
|
||
{
|
||
poses.insert(std::make_pair(static_cast<int>(i + 1), Transform(step * i, 0, 0, 0, 0, 0)));
|
||
}
|
||
return poses;
|
||
}
|
||
|
||
// KITTI metrics use 100–800 m segments; need ~800 m of trajectory at 1 m/frame.
|
||
static std::vector<Transform> lineTrajectory(unsigned int count, float step = 1.0f)
|
||
{
|
||
std::vector<Transform> traj;
|
||
traj.reserve(count);
|
||
for(unsigned int i = 0; i < count; ++i)
|
||
{
|
||
traj.push_back(Transform(step * i, 0, 0, 0, 0, 0));
|
||
}
|
||
return traj;
|
||
}
|
||
|
||
static std::map<int, Transform> transformPoses(
|
||
const std::map<int, Transform> & poses,
|
||
const Transform & t)
|
||
{
|
||
std::map<int, Transform> out;
|
||
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter != poses.end(); ++iter)
|
||
{
|
||
out.insert(std::make_pair(iter->first, t * iter->second));
|
||
}
|
||
return out;
|
||
}
|
||
|
||
static float calcTranslationalRmse(
|
||
const std::map<int, Transform> & groundTruth,
|
||
const std::map<int, Transform> & poses,
|
||
bool align2D = true)
|
||
{
|
||
float tRmse = 0.0f;
|
||
float tMean = 0.0f;
|
||
float tMedian = 0.0f;
|
||
float tStd = 0.0f;
|
||
float tMin = 0.0f;
|
||
float tMax = 0.0f;
|
||
float rRmse = 0.0f;
|
||
float rMean = 0.0f;
|
||
float rMedian = 0.0f;
|
||
float rStd = 0.0f;
|
||
float rMin = 0.0f;
|
||
float rMax = 0.0f;
|
||
graph::calcRMSE(
|
||
groundTruth,
|
||
poses,
|
||
tRmse,
|
||
tMean,
|
||
tMedian,
|
||
tStd,
|
||
tMin,
|
||
tMax,
|
||
rRmse,
|
||
rMean,
|
||
rMedian,
|
||
rStd,
|
||
rMin,
|
||
rMax,
|
||
align2D);
|
||
return tRmse;
|
||
}
|
||
|
||
} // namespace
|
||
|
||
TEST(GraphTest, FindLinkForwardAndReverse)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
|
||
EXPECT_NE(graph::findLink(links, 1, 2), links.end());
|
||
EXPECT_EQ(graph::findLink(links, 1, 2)->second.to(), 2);
|
||
|
||
EXPECT_EQ(graph::findLink(links, 2, 1, false), links.end());
|
||
EXPECT_NE(graph::findLink(links, 2, 1, true), links.end());
|
||
|
||
EXPECT_EQ(graph::findLink(links, 1, 2, true, Link::kGlobalClosure), links.end());
|
||
EXPECT_NE(graph::findLink(links, 1, 2, true, Link::kNeighbor), links.end());
|
||
}
|
||
|
||
TEST(GraphTest, FindLinkIntMultimap)
|
||
{
|
||
std::multimap<int, int> links;
|
||
links.insert(std::make_pair(1, 2));
|
||
links.insert(std::make_pair(2, 3));
|
||
|
||
EXPECT_NE(graph::findLink(links, 1, 2), links.end());
|
||
EXPECT_NE(graph::findLink(links, 3, 2, true), links.end());
|
||
EXPECT_EQ(graph::findLink(links, 1, 3), links.end());
|
||
}
|
||
|
||
TEST(GraphTest, FindLinksIncludesIncomingAsInverse)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
|
||
const std::list<Link> from1 = graph::findLinks(links, 1);
|
||
ASSERT_EQ(from1.size(), 1u);
|
||
EXPECT_EQ(from1.front().from(), 1);
|
||
EXPECT_EQ(from1.front().to(), 2);
|
||
|
||
const std::list<Link> from2 = graph::findLinks(links, 2);
|
||
ASSERT_EQ(from2.size(), 1u);
|
||
EXPECT_EQ(from2.front().from(), 2);
|
||
EXPECT_EQ(from2.front().to(), 1);
|
||
}
|
||
|
||
TEST(GraphTest, FilterDuplicateLinks)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, neighborLink(2, 1));
|
||
|
||
const std::multimap<int, Link> filtered = graph::filterDuplicateLinks(links);
|
||
EXPECT_EQ(filtered.size(), 1u);
|
||
}
|
||
|
||
TEST(GraphTest, FilterLinksByType)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, Link(2, 3, Link::kGlobalClosure, Transform::getIdentity()));
|
||
|
||
const std::multimap<int, Link> noClosure = graph::filterLinks(links, Link::kGlobalClosure, false);
|
||
EXPECT_EQ(noClosure.size(), 1u);
|
||
EXPECT_EQ(noClosure.begin()->second.type(), Link::kNeighbor);
|
||
|
||
const std::multimap<int, Link> onlyClosure = graph::filterLinks(links, Link::kGlobalClosure, true);
|
||
ASSERT_EQ(onlyClosure.size(), 1u);
|
||
EXPECT_EQ(onlyClosure.begin()->second.type(), Link::kGlobalClosure);
|
||
}
|
||
|
||
TEST(GraphTest, FilterSelfReferenceLinks)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, Link(3, 3, Link::kPosePrior, Transform::getIdentity()));
|
||
|
||
const std::multimap<int, Link> nonSelf = graph::filterLinks(links, Link::kSelfRefLink, false);
|
||
EXPECT_EQ(nonSelf.size(), 1u);
|
||
EXPECT_NE(nonSelf.begin()->second.from(), nonSelf.begin()->second.to());
|
||
|
||
const std::multimap<int, Link> selfOnly = graph::filterLinks(links, Link::kSelfRefLink, true);
|
||
ASSERT_EQ(selfOnly.size(), 1u);
|
||
EXPECT_EQ(selfOnly.begin()->second.from(), selfOnly.begin()->second.to());
|
||
}
|
||
|
||
static std::list<int> computeDijkstraPath(bool updateNewCosts, bool useSameCostForAllLinks)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, neighborLink(2, 3));
|
||
insertLink(links, neighborLink(1, 3, 5.0f));
|
||
return graph::computePath(links, 1, 3, updateNewCosts, useSameCostForAllLinks);
|
||
}
|
||
|
||
static std::string dijkstraPathToString(const std::list<int> & path)
|
||
{
|
||
std::string s;
|
||
for(std::list<int>::const_iterator it = path.begin(); it != path.end(); ++it)
|
||
{
|
||
if(!s.empty())
|
||
{
|
||
s += "->";
|
||
}
|
||
s += std::to_string(*it);
|
||
}
|
||
return s;
|
||
}
|
||
|
||
static void expectDijkstraPath(
|
||
const std::list<int> & path,
|
||
const std::initializer_list<int> expected)
|
||
{
|
||
ASSERT_EQ(path.size(), expected.size());
|
||
auto it = path.begin();
|
||
for(int id : expected)
|
||
{
|
||
ASSERT_NE(it, path.end());
|
||
EXPECT_EQ(*it, id);
|
||
++it;
|
||
}
|
||
}
|
||
|
||
TEST(GraphTest, ComputePathDijkstraWeighted)
|
||
{
|
||
// Same graph as computeDijkstraPath(); edge cost = link length (m).
|
||
//
|
||
// 1 -------- 5 m -------- 3 cost 5 -> {1, 3} when updateNewCosts=false
|
||
// | ^
|
||
// +-- 1 m -- 2 -- 1 m ----+ cost 2 -> {1, 2, 3} when updateNewCosts=true
|
||
//
|
||
expectDijkstraPath(computeDijkstraPath(false, false), {1, 3}); // updateNewCosts=false: 3 stays on direct link
|
||
expectDijkstraPath(computeDijkstraPath(true, false), {1, 2, 3});
|
||
}
|
||
|
||
TEST(GraphTest, ComputePathDijkstraUnitCostUpdateNewCostsEquivalent)
|
||
{
|
||
// Same topology; useSameCostForAllLinks=true (1 hop per edge).
|
||
//
|
||
// 1 --------------------- 3 1 hop -> {1, 3} (both updateNewCosts values)
|
||
// |
|
||
// +-- 1 hop -- 2 -- 1 hop -- 3 2 hops (never chosen)
|
||
//
|
||
// Relaxation only applies on a strictly lower hop count, so updateNewCosts cannot
|
||
// change the result.
|
||
expectDijkstraPath(computeDijkstraPath(false, true), {1, 3});
|
||
expectDijkstraPath(computeDijkstraPath(true, true), {1, 3});
|
||
EXPECT_EQ(
|
||
dijkstraPathToString(computeDijkstraPath(false, true)),
|
||
dijkstraPathToString(computeDijkstraPath(true, true)));
|
||
}
|
||
|
||
TEST(GraphTest, ComputePathAStar)
|
||
{
|
||
std::map<int, Transform> poses = linePoses(3);
|
||
std::multimap<int, int> links;
|
||
links.insert(std::make_pair(1, 2));
|
||
links.insert(std::make_pair(2, 3));
|
||
|
||
const std::list<std::pair<int, Transform> > path = graph::computePath(poses, links, 1, 3, false);
|
||
ASSERT_EQ(path.size(), 3u);
|
||
EXPECT_EQ(path.front().first, 1);
|
||
EXPECT_EQ(path.back().first, 3);
|
||
}
|
||
|
||
static std::string aStarPathToString(const std::list<std::pair<int, Transform> > & path)
|
||
{
|
||
std::string s;
|
||
for(std::list<std::pair<int, Transform> >::const_iterator it = path.begin(); it != path.end(); ++it)
|
||
{
|
||
if(it != path.begin())
|
||
{
|
||
s += "->";
|
||
}
|
||
s += std::to_string(it->first);
|
||
}
|
||
return s;
|
||
}
|
||
|
||
static void expectAStarPath(
|
||
const std::list<std::pair<int, Transform> > & path,
|
||
const std::initializer_list<int> expectedIds)
|
||
{
|
||
ASSERT_EQ(path.size(), expectedIds.size()) << "path=" << aStarPathToString(path);
|
||
auto it = path.begin();
|
||
for(int id : expectedIds)
|
||
{
|
||
ASSERT_NE(it, path.end());
|
||
EXPECT_EQ(it->first, id);
|
||
++it;
|
||
}
|
||
}
|
||
|
||
static std::list<std::pair<int, Transform> > computeAStarPath(
|
||
const std::map<int, Transform> & poses,
|
||
const std::multimap<int, int> & links,
|
||
bool updateNewCosts)
|
||
{
|
||
return graph::computePath(poses, links, 1, 3, updateNewCosts);
|
||
}
|
||
|
||
TEST(GraphTest, ComputePathAStarUpdateNewCostsChangesPath)
|
||
{
|
||
/* Detour 1→2→5→6→3 vs shortcut 1→2→4→6→3. No 5→3 edge so h(5,3) < cost(5→6→3).
|
||
Node 5 is expanded before 4 (lower f-score). Node 6 is first reached from 5;
|
||
expanding 4 relaxes the parent of 6 when updateNewCosts=true.
|
||
|
||
3 goal (10, 0)
|
||
|
|
||
6 (2, -5)
|
||
/ \
|
||
5 4 (1,-2) (1.5,-4)
|
||
\ /
|
||
2 (1, 0)
|
||
|
|
||
1 start (0, 0)
|
||
|
||
Links: 1—2, 2—5, 2—4, 5—6, 4—6, 6—3 (no 5—3)
|
||
*/
|
||
const std::map<int, Transform> poses = {
|
||
{1, Transform(0, 0, 0, 0, 0, 0)},
|
||
{2, Transform(1, 0, 0, 0, 0, 0)},
|
||
{3, Transform(10, 0, 0, 0, 0, 0)},
|
||
{4, Transform(1.5f, -4, 0, 0, 0, 0)},
|
||
{5, Transform(1, -2, 0, 0, 0, 0)},
|
||
{6, Transform(2, -5, 0, 0, 0, 0)}};
|
||
std::multimap<int, int> links;
|
||
links.insert(std::make_pair(1, 2));
|
||
links.insert(std::make_pair(2, 5)); // before 2→4
|
||
links.insert(std::make_pair(2, 4));
|
||
links.insert(std::make_pair(5, 6));
|
||
links.insert(std::make_pair(4, 6));
|
||
links.insert(std::make_pair(6, 3));
|
||
|
||
const std::list<std::pair<int, Transform> > pathNoUpdate =
|
||
computeAStarPath(poses, links, false);
|
||
const std::list<std::pair<int, Transform> > pathUpdate =
|
||
computeAStarPath(poses, links, true);
|
||
|
||
expectAStarPath(pathNoUpdate, {1, 2, 5, 6, 3});
|
||
expectAStarPath(pathUpdate, {1, 2, 4, 6, 3});
|
||
EXPECT_NE(aStarPathToString(pathNoUpdate), aStarPathToString(pathUpdate));
|
||
}
|
||
|
||
TEST(GraphTest, FindNearestNode)
|
||
{
|
||
const std::map<int, Transform> poses = linePoses(3, 2.0f);
|
||
const Transform query(2.1f, 0.1f, 0, 0, 0, 0);
|
||
|
||
float sqDist = -1.0f;
|
||
const int id = graph::findNearestNode(poses, query, &sqDist);
|
||
EXPECT_EQ(id, 2);
|
||
EXPECT_NEAR(sqDist, 0.1f * 0.1f + 0.1f * 0.1f, 1e-4f);
|
||
}
|
||
|
||
TEST(GraphTest, FindNearestNodesKnn)
|
||
{
|
||
const std::map<int, Transform> poses = linePoses(4);
|
||
const std::map<int, float> nearest = graph::findNearestNodes(poses.at(2), poses, 0.0f, 0.0f, 2);
|
||
ASSERT_EQ(nearest.size(), 2u);
|
||
ASSERT_TRUE(nearest.find(2) != nearest.end());
|
||
EXPECT_NEAR(nearest.at(2), 0.0f, 1e-6f);
|
||
|
||
// nodeId overload excludes the query node from results
|
||
const std::map<int, float> excludingSelf = graph::findNearestNodes(2, poses, 0.0f, 0.0f, 2);
|
||
ASSERT_EQ(excludingSelf.size(), 2u);
|
||
EXPECT_TRUE(excludingSelf.find(2) == excludingSelf.end());
|
||
EXPECT_NEAR(excludingSelf.at(1), 1.0f, 1e-6f);
|
||
EXPECT_NEAR(excludingSelf.at(3), 1.0f, 1e-6f);
|
||
}
|
||
|
||
TEST(GraphTest, FindNearestNodesRadius)
|
||
{
|
||
const std::map<int, Transform> poses = linePoses(4);
|
||
const std::map<int, float> inRadius = graph::findNearestNodes(poses.at(2), poses, 1.5f);
|
||
EXPECT_EQ(inRadius.size(), 3u);
|
||
EXPECT_TRUE(inRadius.find(2) != inRadius.end());
|
||
EXPECT_NEAR(inRadius.at(2), 0.0f, 1e-6f);
|
||
EXPECT_TRUE(inRadius.find(4) == inRadius.end());
|
||
}
|
||
|
||
TEST(GraphTest, ComputePathLength)
|
||
{
|
||
const std::vector<std::pair<int, Transform> > vecPath = {
|
||
{1, Transform(0, 0, 0, 0, 0, 0)},
|
||
{2, Transform(3, 4, 0, 0, 0, 0)},
|
||
{3, Transform(3, 9, 0, 0, 0, 0)}};
|
||
EXPECT_NEAR(graph::computePathLength(vecPath), 10.0f, 1e-4f);
|
||
|
||
const std::map<int, Transform> mapPath = linePoses(3);
|
||
EXPECT_NEAR(graph::computePathLength(mapPath), 2.0f, 1e-4f);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMinMax)
|
||
{
|
||
const std::map<int, Transform> poses = {
|
||
{1, Transform(-1, 2, 3, 0, 0, 0)},
|
||
{2, Transform(4, -5, 0, 0, 0, 0)}};
|
||
|
||
cv::Vec3f min, max;
|
||
graph::computeMinMax(poses, min, max);
|
||
EXPECT_FLOAT_EQ(min[0], -1.0f);
|
||
EXPECT_FLOAT_EQ(min[1], -5.0f);
|
||
EXPECT_FLOAT_EQ(min[2], 0.0f);
|
||
EXPECT_FLOAT_EQ(max[0], 4.0f);
|
||
EXPECT_FLOAT_EQ(max[1], 2.0f);
|
||
EXPECT_FLOAT_EQ(max[2], 3.0f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcRelativeErrorsIdenticalTrajectories)
|
||
{
|
||
const std::vector<Transform> traj = {
|
||
Transform(0, 0, 0, 0, 0, 0),
|
||
Transform(1, 0, 0, 0, 0, 0),
|
||
Transform(2, 0, 0, 0, 0, 0)};
|
||
|
||
float tErr = -1.0f;
|
||
float rErr = -1.0f;
|
||
graph::calcRelativeErrors(traj, traj, tErr, rErr);
|
||
EXPECT_NEAR(tErr, 0.0f, 1e-5f);
|
||
EXPECT_NEAR(rErr, 0.0f, 1e-5f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcRelativeErrorsWithNoise)
|
||
{
|
||
const std::vector<Transform> gt = {
|
||
Transform(0, 0, 0, 0, 0, 0),
|
||
Transform(1, 0, 0, 0, 0, 0),
|
||
Transform(2, 0, 0, 0, 0, 0),
|
||
Transform(3, 0, 0, 0, 0, 0),
|
||
Transform(4, 0, 0, 0, 0, 0)};
|
||
|
||
// Small position and orientation noise on the estimate.
|
||
const std::vector<Transform> est = {
|
||
Transform(0.01f, -0.02f, 0.005f, 0, 0, 0.01f),
|
||
Transform(1.03f, 0.01f, -0.01f, 0, 0, -0.02f),
|
||
Transform(2.02f, -0.03f, 0.02f, 0, 0, 0.015f),
|
||
Transform(2.98f, 0.02f, 0.01f, 0, 0, -0.01f),
|
||
Transform(4.01f, -0.01f, -0.02f, 0, 0, 0.005f)};
|
||
|
||
float tErr = 0.0f;
|
||
float rErr = 0.0f;
|
||
graph::calcRelativeErrors(gt, est, tErr, rErr);
|
||
EXPECT_GT(tErr, 0.0f);
|
||
EXPECT_LT(tErr, 0.1f);
|
||
EXPECT_GT(rErr, 0.0f);
|
||
EXPECT_LT(rErr, 2.0f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcKittiSequenceErrorsIdenticalTrajectories)
|
||
{
|
||
const std::vector<Transform> traj = lineTrajectory(901, 1.0f);
|
||
|
||
float tErr = -1.0f;
|
||
float rErr = -1.0f;
|
||
graph::calcKittiSequenceErrors(traj, traj, tErr, rErr);
|
||
EXPECT_NEAR(tErr, 0.0f, 1e-5f);
|
||
EXPECT_NEAR(rErr, 0.0f, 1e-5f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcKittiSequenceErrorsWithNoise)
|
||
{
|
||
const std::vector<Transform> gt = lineTrajectory(901, 1.0f);
|
||
ASSERT_EQ(gt.size(), 901u);
|
||
ASSERT_NEAR(gt[0].x(), 0.0f, 1e-5f);
|
||
|
||
std::vector<Transform> est;
|
||
est.reserve(gt.size());
|
||
for(unsigned int i = 0; i < gt.size(); ++i)
|
||
{
|
||
const int ii = static_cast<int>(i);
|
||
const float dx = 0.02f * static_cast<float>((ii % 3) - 1);
|
||
const float dy = 0.01f * static_cast<float>((ii % 5) - 2);
|
||
est.push_back(Transform(
|
||
gt.at(i).x() + dx,
|
||
gt.at(i).y() + dy,
|
||
gt.at(i).z(),
|
||
0.0f,
|
||
0.0f,
|
||
0.0f));
|
||
}
|
||
ASSERT_EQ(est.size(), 901u);
|
||
ASSERT_NEAR(est.at(0).x(), -0.02f, 1e-3f);
|
||
ASSERT_NEAR(est.at(800).x(), 800.02f, 1e-1f);
|
||
|
||
const Transform poseDeltaEst = est.at(0).inverse() * est.at(800);
|
||
ASSERT_NEAR(poseDeltaEst.getNorm(), 800.0f, 5.0f);
|
||
|
||
float tErr = 0.0f;
|
||
float rErr = 0.0f;
|
||
graph::calcKittiSequenceErrors(gt, est, tErr, rErr);
|
||
EXPECT_TRUE(std::isfinite(tErr)) << "tErr=" << tErr;
|
||
EXPECT_TRUE(std::isfinite(rErr)) << "rErr=" << rErr;
|
||
EXPECT_GT(tErr, 0.0f);
|
||
EXPECT_LT(tErr, 2.0f); // translation error (%)
|
||
EXPECT_NEAR(rErr, 0.0f, 0.5f); // no orientation noise on the trajectory
|
||
}
|
||
|
||
TEST(GraphTest, CalcRMSEIdenticalMaps)
|
||
{
|
||
const std::map<int, Transform> gt = linePoses(3);
|
||
float tRmse = -1.0f;
|
||
float tMean = -1.0f;
|
||
float tMedian = -1.0f;
|
||
float tStd = -1.0f;
|
||
float tMin = -1.0f;
|
||
float tMax = -1.0f;
|
||
float rRmse = -1.0f;
|
||
float rMean = -1.0f;
|
||
float rMedian = -1.0f;
|
||
float rStd = -1.0f;
|
||
float rMin = -1.0f;
|
||
float rMax = -1.0f;
|
||
|
||
const Transform align = graph::calcRMSE(
|
||
gt, gt,
|
||
tRmse, tMean, tMedian, tStd, tMin, tMax,
|
||
rRmse, rMean, rMedian, rStd, rMin, rMax,
|
||
true);
|
||
EXPECT_TRUE(align.isIdentity());
|
||
EXPECT_NEAR(tRmse, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(rRmse, 0.0f, 1e-4f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcRMSEWithNoise)
|
||
{
|
||
const std::map<int, Transform> gt = linePoses(8, 1.0f);
|
||
std::map<int, Transform> est = gt;
|
||
est[2] = Transform(1.05f, 0.02f, 0, 0, 0, 0.01f);
|
||
est[4] = Transform(3.02f, -0.03f, 0.01f, 0, 0, -0.02f);
|
||
est[6] = Transform(5.01f, 0.01f, -0.02f, 0, 0, 0.015f);
|
||
est[8] = Transform(7.0f, -0.01f, 0.02f, 0, 0, -0.005f);
|
||
|
||
const float tRmse = calcTranslationalRmse(gt, est, true);
|
||
EXPECT_GT(tRmse, 0.0f);
|
||
EXPECT_LT(tRmse, 0.1f);
|
||
}
|
||
|
||
TEST(GraphTest, CalcRMSEAlignsRotatedTrajectory)
|
||
{
|
||
// Eight poses so calcRMSE uses SVD alignment (more than five matched poses).
|
||
const std::map<int, Transform> gt = linePoses(8, 1.0f);
|
||
|
||
std::map<int, Transform> noisy = gt;
|
||
noisy[2] = Transform(1.05f, 0.02f, 0, 0, 0, 0.01f);
|
||
noisy[5] = Transform(4.02f, -0.02f, 0, 0, 0, -0.01f);
|
||
noisy[7] = Transform(6.01f, 0.01f, 0, 0, 0, 0.02f);
|
||
|
||
const Transform yaw90(0, 0, 0, 0, 0, static_cast<float>(CV_PI / 2.0));
|
||
const std::map<int, Transform> rotated = transformPoses(gt, yaw90);
|
||
|
||
// Without alignment, positions would differ a lot (x vs y).
|
||
const Transform p = gt.at(4);
|
||
const Transform r = rotated.at(4);
|
||
EXPECT_GT(p.getDistance(r), 1.0f);
|
||
|
||
const float rmseNoisy = calcTranslationalRmse(gt, noisy, true);
|
||
|
||
float tRmse = 0.0f;
|
||
float tMean = 0.0f;
|
||
float tMedian = 0.0f;
|
||
float tStd = 0.0f;
|
||
float tMin = 0.0f;
|
||
float tMax = 0.0f;
|
||
float rRmse = 0.0f;
|
||
float rMean = 0.0f;
|
||
float rMedian = 0.0f;
|
||
float rStd = 0.0f;
|
||
float rMin = 0.0f;
|
||
float rMax = 0.0f;
|
||
const Transform align = graph::calcRMSE(
|
||
gt,
|
||
rotated,
|
||
tRmse,
|
||
tMean,
|
||
tMedian,
|
||
tStd,
|
||
tMin,
|
||
tMax,
|
||
rRmse,
|
||
rMean,
|
||
rMedian,
|
||
rStd,
|
||
rMin,
|
||
rMax,
|
||
true);
|
||
|
||
EXPECT_LT(rmseNoisy, 0.1f);
|
||
EXPECT_LT(tRmse, 0.1f);
|
||
EXPECT_NEAR(tRmse, rmseNoisy, 0.08f);
|
||
|
||
// est = yaw90 * gt => align * est ≈ gt => align ≈ yaw90⁻¹
|
||
const Transform expectedAlign = yaw90.inverse();
|
||
EXPECT_NEAR(align.getAngle(expectedAlign), 0.0f, 0.05f);
|
||
EXPECT_NEAR(align.x(), expectedAlign.x(), 1e-2f);
|
||
EXPECT_NEAR(align.y(), expectedAlign.y(), 1e-2f);
|
||
EXPECT_NEAR(align.theta(), expectedAlign.theta(), 1e-2f);
|
||
EXPECT_LT((align * rotated.at(4)).getDistance(gt.at(4)), 1e-2f);
|
||
}
|
||
|
||
static graph::MaxGraphErrors maxGraphErrors(
|
||
const std::map<int, Transform> & poses,
|
||
const std::multimap<int, Link> & links,
|
||
bool for3DoF = false)
|
||
{
|
||
return graph::computeMaxGraphErrors(poses, links, for3DoF);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsZeroResidual)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(errors.linear, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(errors.angular, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(errors.linearRatio, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(errors.angularRatio, 0.0f, 1e-4f);
|
||
EXPECT_TRUE(errors.linearLink.isValid());
|
||
EXPECT_EQ(errors.linearLink.from(), 1);
|
||
EXPECT_EQ(errors.linearLink.to(), 2);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsKnownLinearResidual)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(2, 0, 0, 0, 0, 0))); // 2 m apart
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f)); // link says 1 m
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(errors.linear, 1.0f, 1e-4f);
|
||
EXPECT_NEAR(errors.linearRatio, 1.0f, 1e-4f); // default inf: variance 1, stddev 1
|
||
EXPECT_TRUE(errors.linearLink.isValid());
|
||
EXPECT_EQ(errors.linearLink.from(), 1);
|
||
EXPECT_EQ(errors.linearLink.to(), 2);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsPicksHighestLinearRatio)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(0.5f, 0, 0, 0, 0, 0))); // 0.5 m error vs link
|
||
poses.insert(std::make_pair(3, Transform(2.5f, 0, 0, 0, 0, 0))); // 1 m error vs link
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, Link(
|
||
1,
|
||
2,
|
||
Link::kNeighbor,
|
||
Transform(0, 0, 0, 0, 0, 0),
|
||
infMatrixDiagonal(100, 100, 100, 1, 1, 1))); // ratio ≈ 0.5 / 0.1 = 5
|
||
insertLink(links, neighborLink(2, 3, 1.0f)); // ratio ≈ 1 / 1 = 1
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(errors.linear, 0.5f, 1e-4f);
|
||
EXPECT_NEAR(errors.linearRatio, 5.0f, 1e-3f);
|
||
EXPECT_EQ(errors.linearLink.from(), 1);
|
||
EXPECT_EQ(errors.linearLink.to(), 2);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsAngularResidual)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0.5f)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(errors.linear, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(errors.angular, 0.5f, 1e-4f);
|
||
EXPECT_NEAR(errors.angularRatio, 0.5f, 1e-4f);
|
||
EXPECT_EQ(errors.angularLink.from(), 1);
|
||
EXPECT_EQ(errors.angularLink.to(), 2);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsDifferentWorstLinearAndAngularLinks)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(0.6f, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(3, Transform(1.6f, 0, 0, 0, 0, 0.5f)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, Link(
|
||
1,
|
||
2,
|
||
Link::kNeighbor,
|
||
Transform(1, 0, 0, 0, 0, 0),
|
||
infMatrixDiagonal(100, 100, 100, 1, 1, 1)));
|
||
insertLink(links, Link(
|
||
2,
|
||
3,
|
||
Link::kNeighbor,
|
||
Transform(1, 0, 0, 0, 0, 0),
|
||
infMatrixDiagonal(1, 1, 1, 100, 100, 100)));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_EQ(errors.linearLink.from(), 1);
|
||
EXPECT_EQ(errors.linearLink.to(), 2);
|
||
EXPECT_EQ(errors.angularLink.from(), 2);
|
||
EXPECT_EQ(errors.angularLink.to(), 3);
|
||
EXPECT_NE(errors.linearLink.from(), errors.angularLink.from());
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsFor3DoF)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(1, 0, 0.3f, 0, 0, 0)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f));
|
||
|
||
const graph::MaxGraphErrors errors6 = maxGraphErrors(poses, links, false);
|
||
const graph::MaxGraphErrors errors3 = maxGraphErrors(poses, links, true);
|
||
EXPECT_NEAR(errors6.linear, 0.3f, 1e-4f);
|
||
EXPECT_NEAR(errors3.linear, 0.0f, 1e-4f);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsSkipsSelfLinks)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, Link(1, 1, Link::kPosePrior, Transform(1, 2, 3, 0, 0, 0)));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_FLOAT_EQ(errors.linear, -1.0f);
|
||
EXPECT_FLOAT_EQ(errors.angular, -1.0f);
|
||
EXPECT_FALSE(errors.linearLink.isValid());
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsAbortsOnMissingPose)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_FLOAT_EQ(errors.linear, -1.0f);
|
||
EXPECT_FLOAT_EQ(errors.angular, -1.0f);
|
||
EXPECT_FALSE(errors.linearLink.isValid());
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsLandmarkSkipsUnconstrainedYaw)
|
||
{
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0.1f))); // 0.1 rad yaw vs neighbor link
|
||
poses.insert(std::make_pair(-10, Transform(2, 1, 0, 0, 0, 0.5f))); // 1 m y and 0.5 rad yaw vs landmark link
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2, 1.0f));
|
||
// Landmark yaw is not constrained; angular error is skipped even though poses disagree by 0.5 rad.
|
||
insertLink(links, Link(
|
||
1,
|
||
-10,
|
||
Link::kLandmark,
|
||
Transform(2, 0, 0, 0, 0, 0),
|
||
infMatrixDiagonal(1, 1, 1, 1, 1, 0.00001)));
|
||
|
||
const graph::MaxGraphErrors errors = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(errors.linear, 1.0f, 1e-4f);
|
||
EXPECT_EQ(errors.linearLink.from(), 1);
|
||
EXPECT_EQ(errors.linearLink.to(), -10);
|
||
EXPECT_NEAR(errors.angular, 0.1f, 1e-4f);
|
||
EXPECT_EQ(errors.angularLink.from(), 1);
|
||
EXPECT_EQ(errors.angularLink.to(), 2);
|
||
EXPECT_NE(errors.angularLink.type(), Link::kLandmark);
|
||
}
|
||
|
||
TEST(GraphTest, ComputeMaxGraphErrorsLandmarkTwoPoseObservations)
|
||
{
|
||
/* Same landmark -10 observed from poses 1 and 2 (two links sharing the landmark id).
|
||
|
||
-10 (1, 1)
|
||
/ \
|
||
1 2
|
||
(0,0) (2,0)
|
||
*/
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(2, 0, 0, 0, 0, 0)));
|
||
poses.insert(std::make_pair(-10, Transform(1, 1, 0, 0, 0, 0)));
|
||
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, Link(1, -10, Link::kLandmark, Transform(1, 1, 0, 0, 0, 0)));
|
||
insertLink(links, Link(2, -10, Link::kLandmark, Transform(-1, 1, 0, 0, 0, 0)));
|
||
|
||
const graph::MaxGraphErrors consistent = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(consistent.linear, 0.0f, 1e-4f);
|
||
EXPECT_NEAR(consistent.angular, 0.0f, 1e-4f);
|
||
|
||
// Poses still agree with observation from 1; link 2→-10 is wrong by 1 m in y.
|
||
std::multimap<int, Link> linksOneInconsistent;
|
||
insertLink(linksOneInconsistent, Link(1, -10, Link::kLandmark, Transform(1, 1, 0, 0, 0, 0)));
|
||
insertLink(linksOneInconsistent, Link(2, -10, Link::kLandmark, Transform(-1, 0, 0, 0, 0, 0)));
|
||
const graph::MaxGraphErrors oneInconsistent = maxGraphErrors(poses, linksOneInconsistent);
|
||
EXPECT_NEAR(oneInconsistent.linear, 1.0f, 1e-4f);
|
||
EXPECT_EQ(oneInconsistent.linearLink.from(), 2);
|
||
EXPECT_EQ(oneInconsistent.linearLink.to(), -10);
|
||
|
||
// Same mismatch with landmark as link.from (from < 0): measurement is inverted.
|
||
std::multimap<int, Link> linksOneInconsistentLandmarkFrom;
|
||
insertLink(linksOneInconsistentLandmarkFrom, Link(-10, 1, Link::kLandmark, Transform(-1, -1, 0, 0, 0, 0)));
|
||
insertLink(linksOneInconsistentLandmarkFrom, Link(-10, 2, Link::kLandmark, Transform(1, 0, 0, 0, 0, 0)));
|
||
const graph::MaxGraphErrors oneInconsistentLandmarkFrom =
|
||
maxGraphErrors(poses, linksOneInconsistentLandmarkFrom);
|
||
EXPECT_NEAR(oneInconsistentLandmarkFrom.linear, oneInconsistent.linear, 1e-4f);
|
||
EXPECT_NEAR(oneInconsistentLandmarkFrom.linearRatio, oneInconsistent.linearRatio, 1e-4f);
|
||
EXPECT_EQ(oneInconsistentLandmarkFrom.linearLink.from(), -10);
|
||
EXPECT_EQ(oneInconsistentLandmarkFrom.linearLink.to(), 2);
|
||
|
||
// Move the optimized landmark; both observations are now inconsistent by 1 m in y.
|
||
poses[-10] = Transform(1, 0, 0, 0, 0, 0);
|
||
const graph::MaxGraphErrors inconsistent = maxGraphErrors(poses, links);
|
||
EXPECT_NEAR(inconsistent.linear, 1.0f, 1e-4f);
|
||
EXPECT_TRUE(inconsistent.linearLink.isValid());
|
||
EXPECT_EQ(inconsistent.linearLink.to(), -10);
|
||
EXPECT_TRUE(inconsistent.linearLink.from() == 1 || inconsistent.linearLink.from() == 2);
|
||
}
|
||
|
||
TEST(GraphTest, GetMaxOdomInf)
|
||
{
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, Link(1, 2, Link::kNeighbor, Transform::getIdentity(), infMatrixDiagonal(1, 2, 3, 4, 5, 6)));
|
||
insertLink(links, Link(2, 3, Link::kNeighbor, Transform::getIdentity(), infMatrixDiagonal(6, 5, 4, 3, 2, 1)));
|
||
insertLink(links, Link(3, 4, Link::kGlobalClosure, Transform::getIdentity(), infMatrixDiagonal(99, 99, 99, 99, 99, 99)));
|
||
|
||
const std::vector<double> maxInf = graph::getMaxOdomInf(links);
|
||
ASSERT_EQ(maxInf.size(), 6u);
|
||
EXPECT_DOUBLE_EQ(maxInf[0], 6.0);
|
||
EXPECT_DOUBLE_EQ(maxInf[5], 6.0);
|
||
}
|
||
|
||
TEST(GraphTest, GetPathsNeighborChain)
|
||
{
|
||
std::map<int, Transform> poses = linePoses(3);
|
||
std::multimap<int, Link> links;
|
||
insertLink(links, neighborLink(1, 2));
|
||
insertLink(links, neighborLink(2, 3));
|
||
|
||
const std::list<std::map<int, Transform> > paths = graph::getPaths(poses, links);
|
||
ASSERT_EQ(paths.size(), 1u);
|
||
EXPECT_EQ(paths.front().size(), 3u);
|
||
EXPECT_EQ(poses.size(), 3u); // poses passed by value, caller's map is unchanged
|
||
}
|
||
|
||
// -------------------------------------------------------------------------
|
||
// exportPoses / importPoses round-trip for graph formats (3=TORO, 4=g2o).
|
||
// Skips when the underlying optimizer isn't built in (loadGraph requires it).
|
||
// -------------------------------------------------------------------------
|
||
namespace {
|
||
|
||
void expectPosesNearEqual(
|
||
const std::map<int, Transform> & a,
|
||
const std::map<int, Transform> & b,
|
||
float transTol,
|
||
float rotTol,
|
||
const std::string & label)
|
||
{
|
||
ASSERT_EQ(a.size(), b.size()) << label << " pose count differs";
|
||
for(const auto & kv : a)
|
||
{
|
||
const auto it = b.find(kv.first);
|
||
ASSERT_TRUE(it != b.end()) << label << " missing pose id=" << kv.first;
|
||
const Transform & A = kv.second;
|
||
const Transform & B = it->second;
|
||
EXPECT_LT(A.getDistance(B), transTol)
|
||
<< label << " id=" << kv.first
|
||
<< " A=" << A.prettyPrint() << " B=" << B.prettyPrint();
|
||
EXPECT_LT(A.getAngle(B), rotTol)
|
||
<< label << " id=" << kv.first
|
||
<< " angle diff exceeds tolerance";
|
||
}
|
||
}
|
||
|
||
void expectLinksNearEqual(
|
||
const std::multimap<int, Link> & a,
|
||
const std::multimap<int, Link> & b,
|
||
float transTol,
|
||
float rotTol,
|
||
bool slam2d,
|
||
bool landmarkWithRotation,
|
||
bool priorWithRotation,
|
||
const std::string & label)
|
||
{
|
||
ASSERT_EQ(a.size(), b.size()) << label << " link count differs";
|
||
// Index both maps by (from, to) so the comparison is independent of
|
||
// the multimap key. rtabmap's Memory keys landmark links by the
|
||
// landmark id (negative), while OptimizerG2O::loadGraph keys every
|
||
// link by `from` -- the file format doesn't encode the rtabmap key
|
||
// convention, so we can't expect it to round-trip.
|
||
auto index = [](const std::multimap<int, Link> & m) {
|
||
std::map<std::pair<int, int>, const Link *> out;
|
||
for(const auto & kv : m)
|
||
{
|
||
out.emplace(std::make_pair(kv.second.from(), kv.second.to()),
|
||
&kv.second);
|
||
}
|
||
return out;
|
||
};
|
||
const auto idxA = index(a);
|
||
const auto idxB = index(b);
|
||
ASSERT_EQ(idxA.size(), idxB.size())
|
||
<< label << " unique (from,to) link pair count differs";
|
||
// Note: TORO's text format stores edges generically and doesn't preserve
|
||
// rtabmap's Link::Type tag, so from / to / transform / infMatrix are all
|
||
// that round-trip there. g2o carries the type in a column of its own,
|
||
// which G2oRoundTripPreservesLinkTypes below checks.
|
||
//
|
||
// Landmark links in g2o are written as EDGE_SE3_TRACKXYZ (3D point
|
||
// observation): only the translation and the 3x3 translation block
|
||
// of the info matrix survive. Rotation and the 3x3 rotation block
|
||
// are skipped for these links.
|
||
for(const auto & kvA : idxA)
|
||
{
|
||
const auto itB = idxB.find(kvA.first);
|
||
ASSERT_TRUE(itB != idxB.end())
|
||
<< label << " missing link "
|
||
<< kvA.first.first << "->" << kvA.first.second;
|
||
const Link & la = *kvA.second;
|
||
const Link & lb = *itB->second;
|
||
// Position-only links (point-landmark observations and
|
||
// position-only priors) round-trip translation but not
|
||
// rotation, and only the translation block of the info
|
||
// matrix survives. The saver picks the serialization based
|
||
// on the rotation-block variance.
|
||
const bool isLandmark = la.type() == Link::kLandmark
|
||
|| lb.type() == Link::kLandmark
|
||
|| la.to() < 0 || lb.to() < 0;
|
||
const bool isPrior = la.from() == la.to();
|
||
const bool isPositionOnly =
|
||
(isLandmark && !landmarkWithRotation) ||
|
||
(isPrior && !priorWithRotation);
|
||
EXPECT_EQ(la.from(), lb.from()) << label;
|
||
EXPECT_EQ(la.to(), lb.to()) << label;
|
||
EXPECT_LT(la.transform().getDistance(lb.transform()), transTol)
|
||
<< label << " link " << la.from() << "->" << la.to()
|
||
<< " translation drift";
|
||
if(!isPositionOnly)
|
||
{
|
||
EXPECT_LT(la.transform().getAngle(lb.transform()), rotTol)
|
||
<< label << " link " << la.from() << "->" << la.to()
|
||
<< " rotation drift";
|
||
}
|
||
const cv::Mat & infA = la.infMatrix();
|
||
const cv::Mat & infB = lb.infMatrix();
|
||
ASSERT_EQ(infA.rows, infB.rows)
|
||
<< label << " infMatrix row count differs";
|
||
ASSERT_EQ(infA.cols, infB.cols)
|
||
<< label << " infMatrix col count differs";
|
||
// Which DoFs survive the round trip:
|
||
// * slam3d, non-landmark: all 6 (x,y,z,roll,pitch,yaw).
|
||
// * slam2d, non-landmark: x,y,yaw (file stores a 3x3 block,
|
||
// unused z/roll/pitch refilled with defaults on load).
|
||
// * slam3d, landmark : x,y,z only (EDGE_SE3_TRACKXYZ is a
|
||
// 3D point obs, no orientation block).
|
||
// * slam2d, landmark : x,y only (EDGE_SE2_XY / point2 obs).
|
||
const int dim = infA.rows;
|
||
auto isActive = [slam2d, isPositionOnly](int idx) {
|
||
if(slam2d && isPositionOnly) return idx == 0 || idx == 1;
|
||
if(slam2d) return idx == 0 || idx == 1 || idx == 5;
|
||
if(isPositionOnly) return idx >= 0 && idx <= 2;
|
||
return idx >= 0 && idx < 6;
|
||
};
|
||
for(int r = 0; r < dim; ++r)
|
||
{
|
||
for(int c = 0; c < dim; ++c)
|
||
{
|
||
if(!isActive(r) || !isActive(c)) continue;
|
||
EXPECT_NEAR(infA.at<double>(r, c),
|
||
infB.at<double>(r, c),
|
||
/*absTol=*/1e-3)
|
||
<< label << " link "
|
||
<< la.from() << "->" << la.to()
|
||
<< " infMatrix(" << r << "," << c << ") drift";
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
class GraphIoRoundTripTest
|
||
: public ::testing::TestWithParam<std::tuple<int, bool>>
|
||
{
|
||
protected:
|
||
int format() const { return std::get<0>(GetParam()); }
|
||
bool force3DoF() const { return std::get<1>(GetParam()); }
|
||
const char * formatName() const
|
||
{
|
||
switch(format())
|
||
{
|
||
case 3: return "TORO";
|
||
case 4: return "g2o";
|
||
default: return "unknown";
|
||
}
|
||
}
|
||
std::string label() const
|
||
{
|
||
return std::string(formatName()) + (force3DoF() ? "/slam2d" : "/slam3d");
|
||
}
|
||
Optimizer::Type requiredOptimizer() const
|
||
{
|
||
switch(format())
|
||
{
|
||
case 3: return Optimizer::kTypeTORO;
|
||
case 4: return Optimizer::kTypeG2O;
|
||
default: return Optimizer::kTypeUndef;
|
||
}
|
||
}
|
||
};
|
||
|
||
} // namespace
|
||
|
||
TEST_P(GraphIoRoundTripTest, RoundTripsPosesAndConstraints)
|
||
{
|
||
if(!Optimizer::isAvailable(requiredOptimizer()))
|
||
{
|
||
GTEST_SKIP() << formatName() << " optimizer not built in";
|
||
}
|
||
|
||
// Small 3-node chain with one loop closure -- exercises both
|
||
// neighbor and global-closure link types, plus a non-identity
|
||
// information matrix to verify it survives the round trip.
|
||
//
|
||
// Poses are 2D-compatible (z = roll = pitch = 0) so the slam2d
|
||
// branch (Reg/Force3DoF=true) can serialize them without lossy
|
||
// projection.
|
||
const float deg = static_cast<float>(M_PI) / 180.0f;
|
||
std::map<int, Transform> poses = {
|
||
{1, Transform(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)},
|
||
{2, Transform(1.0f, 0.05f, 0.0f, 0.0f, 0.0f, 3.0f * deg)},
|
||
{3, Transform(2.0f, 0.10f, 0.0f, 0.0f, 0.0f, 6.0f * deg)},
|
||
};
|
||
std::multimap<int, Link> links;
|
||
cv::Mat infMat = cv::Mat::eye(6, 6, CV_64F) * 100.0;
|
||
infMat.at<double>(0, 0) = 50.0; // distinct diagonals so any
|
||
infMat.at<double>(5, 5) = 200.0; // shuffling of channels shows up
|
||
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor,
|
||
poses.at(1).inverse() * poses.at(2), infMat)));
|
||
links.insert(std::make_pair(2, Link(2, 3, Link::kNeighbor,
|
||
poses.at(2).inverse() * poses.at(3), infMat)));
|
||
links.insert(std::make_pair(1, Link(1, 3, Link::kGlobalClosure,
|
||
poses.at(1).inverse() * poses.at(3), infMat)));
|
||
|
||
// Reg/Force3DoF toggles the slam2d branch in OptimizerTORO /
|
||
// OptimizerG2O's saveGraph (writes 2D node/edge records). The
|
||
// importPoses path detects 2D-vs-3D from the file contents, so
|
||
// passing parameters only matters on the export side.
|
||
ParametersMap params;
|
||
params[Parameters::kRegForce3DoF()] = force3DoF() ? "true" : "false";
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_round_trip_%s_%s_%d.graph",
|
||
formatName(),
|
||
force3DoF() ? "slam2d" : "slam3d",
|
||
test::getPid()));
|
||
UFile::erase(path);
|
||
|
||
ASSERT_TRUE(graph::exportPoses(path, format(), poses, links,
|
||
/*stamps=*/std::map<int, double>(), params))
|
||
<< label() << " exportPoses failed for " << path;
|
||
ASSERT_TRUE(UFile::exists(path))
|
||
<< label() << " expected " << path << " on disk";
|
||
|
||
std::map<int, Transform> posesOut;
|
||
std::multimap<int, Link> linksOut;
|
||
ASSERT_TRUE(graph::importPoses(path, format(), posesOut, &linksOut))
|
||
<< label() << " importPoses failed for " << path;
|
||
|
||
expectPosesNearEqual(poses, posesOut, /*transTol=*/1e-4f,
|
||
/*rotTol=*/1e-4f, label() + " poses");
|
||
expectLinksNearEqual(links, linksOut, /*transTol=*/1e-4f,
|
||
/*rotTol=*/1e-4f, /*slam2d=*/force3DoF(),
|
||
/*landmarkWithRotation=*/false /* no landmarks in this test */,
|
||
/*priorWithRotation=*/false /* no priors in this test */,
|
||
label() + " links");
|
||
}
|
||
|
||
INSTANTIATE_TEST_SUITE_P(
|
||
GraphFormats,
|
||
GraphIoRoundTripTest,
|
||
::testing::Combine(
|
||
::testing::Values(3 /*TORO*/, 4 /*g2o*/),
|
||
::testing::Bool() /*force3DoF*/),
|
||
[](const ::testing::TestParamInfo<std::tuple<int, bool>> & info)
|
||
{
|
||
const char * fmt;
|
||
switch(std::get<0>(info.param))
|
||
{
|
||
case 3: fmt = "TORO"; break;
|
||
case 4: fmt = "g2o"; break;
|
||
default: fmt = "unknown"; break;
|
||
}
|
||
return std::string(fmt) + (std::get<1>(info.param) ? "_slam2d" : "_slam3d");
|
||
});
|
||
|
||
// -------------------------------------------------------------------------
|
||
// g2o-specific round-trip with prior + landmark links. TORO's text graph
|
||
// format doesn't carry rtabmap's prior/landmark link types, so this test
|
||
// is g2o-only. Parameterized on (slam2d, landmarkWithRotation):
|
||
//
|
||
// * landmarkWithRotation=true -> landmark exported as VERTEX_SE3:QUAT
|
||
// (or VERTEX_SE2 in slam2d), EDGE_SE3:QUAT / EDGE_SE2. Full pose obs.
|
||
// * landmarkWithRotation=false -> landmark exported as VERTEX_TRACKXYZ
|
||
// (or VERTEX_XY in slam2d), EDGE_SE3_TRACKXYZ / EDGE_SE2_XY. 3D-point
|
||
// (or 2D-point) obs, no rotation.
|
||
//
|
||
// The branch is selected by the rotation block of the landmark link's
|
||
// info matrix: if 1/inf(3..5,3..5) >= 9999 (i.e. effectively infinite
|
||
// rotation variance), the saver emits the point variant; otherwise SE3.
|
||
// -------------------------------------------------------------------------
|
||
class GraphIoG2oPriorsAndLandmarksTest
|
||
: public ::testing::TestWithParam<std::tuple<bool, bool, bool>>
|
||
{
|
||
protected:
|
||
bool force3DoF() const { return std::get<0>(GetParam()); }
|
||
bool landmarkWithRotation() const { return std::get<1>(GetParam()); }
|
||
bool priorWithRotation() const { return std::get<2>(GetParam()); }
|
||
std::string label() const
|
||
{
|
||
return std::string("g2o/") + (force3DoF() ? "slam2d" : "slam3d")
|
||
+ (landmarkWithRotation() ? "/rotLm" : "/pointLm")
|
||
+ (priorWithRotation() ? "/rotPrior" : "/posPrior");
|
||
}
|
||
};
|
||
|
||
TEST_P(GraphIoG2oPriorsAndLandmarksTest, RoundTripsPriorAndLandmarkLinks)
|
||
{
|
||
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
|
||
{
|
||
GTEST_SKIP() << "g2o optimizer not built in";
|
||
}
|
||
|
||
// 3 poses + 1 landmark (negative id). 2 neighbor links chain the
|
||
// poses, 1 pose prior anchors pose 1 in the world frame, 1 landmark
|
||
// link records pose 2's observation of the landmark.
|
||
const float deg = static_cast<float>(M_PI) / 180.0f;
|
||
// Landmark's "pose" has a non-zero yaw only when the variant uses
|
||
// the SE-rotation landmark path; otherwise rtabmap would round-trip
|
||
// only translation and the test would have to ignore the rotation.
|
||
const float landmarkYaw =
|
||
landmarkWithRotation() ? 45.0f * deg : 0.0f;
|
||
std::map<int, Transform> poses = {
|
||
{1, Transform(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)},
|
||
{2, Transform(1.0f, 0.05f, 0.0f, 0.0f, 0.0f, 3.0f * deg)},
|
||
{3, Transform(2.0f, 0.10f, 0.0f, 0.0f, 0.0f, 6.0f * deg)},
|
||
{-10, Transform(1.5f, 0.50f, 0.0f, 0.0f, 0.0f, landmarkYaw)},
|
||
};
|
||
|
||
cv::Mat infMat = cv::Mat::eye(6, 6, CV_64F) * 100.0;
|
||
infMat.at<double>(0, 0) = 50.0;
|
||
infMat.at<double>(5, 5) = 200.0;
|
||
|
||
// Helper: blank out the rotation diagonal so the saver's
|
||
// `1 / inf(3,3) >= 9999` check picks the position-only serialization.
|
||
auto stripRotationVariance = [](cv::Mat m) {
|
||
// 1/1e-5 = 1e5 > 9999 -> "effectively infinite variance".
|
||
m.at<double>(3, 3) = 1e-5;
|
||
m.at<double>(4, 4) = 1e-5;
|
||
m.at<double>(5, 5) = 1e-5;
|
||
return m;
|
||
};
|
||
|
||
// Landmark info matrix: rotation block blanked when point-landmark.
|
||
cv::Mat landmarkInfMat = infMat.clone();
|
||
if(!landmarkWithRotation())
|
||
{
|
||
landmarkInfMat = stripRotationVariance(landmarkInfMat);
|
||
}
|
||
// Prior info matrix: same logic. Saver picks EDGE_SE3_PRIOR
|
||
// (rotation) vs EDGE_POINTXYZ_PRIOR / EDGE_PRIOR_SE2_XY (no rotation)
|
||
// based on the rotation block diagonal.
|
||
cv::Mat priorInfMat = infMat.clone();
|
||
if(!priorWithRotation())
|
||
{
|
||
priorInfMat = stripRotationVariance(priorInfMat);
|
||
}
|
||
|
||
std::multimap<int, Link> links;
|
||
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor,
|
||
poses.at(1).inverse() * poses.at(2), infMat)));
|
||
links.insert(std::make_pair(2, Link(2, 3, Link::kNeighbor,
|
||
poses.at(2).inverse() * poses.at(3), infMat)));
|
||
// Pose prior on node 1 (from == to). Transform is the world-frame
|
||
// prior pose; rtabmap convention places it on the source node.
|
||
links.insert(std::make_pair(1, Link(1, 1, Link::kPosePrior,
|
||
poses.at(1), priorInfMat)));
|
||
// Landmark observation: node 2 observes landmark -10. Transform is
|
||
// the landmark's position in pose 2's frame. Convention: rtabmap
|
||
// keys landmark links in the multimap by the *landmark id*
|
||
// (negative), not by the source node -- see Signature::addLandmark
|
||
// and OptimizerG2O::saveGraph's isLandmarkWithRotation lookup.
|
||
links.insert(std::make_pair(-10, Link(2, -10, Link::kLandmark,
|
||
poses.at(2).inverse() * poses.at(-10), landmarkInfMat)));
|
||
|
||
ParametersMap params;
|
||
params[Parameters::kRegForce3DoF()] = force3DoF() ? "true" : "false";
|
||
// Optimizer/PriorsIgnored defaults to true (so global SLAM doesn't
|
||
// fight against drift-prone priors). We need it off here so the
|
||
// prior edge actually gets written by OptimizerG2O::saveGraph.
|
||
params[Parameters::kOptimizerPriorsIgnored()] = "false";
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_g2o_priors_landmarks_%s_%s_%d.g2o",
|
||
force3DoF() ? "slam2d" : "slam3d",
|
||
landmarkWithRotation() ? "rotLm" : "pointLm",
|
||
test::getPid()));
|
||
UFile::erase(path);
|
||
|
||
ASSERT_TRUE(graph::exportPoses(path, /*format=*/4, poses, links,
|
||
std::map<int, double>(), params))
|
||
<< label() << " exportPoses failed";
|
||
ASSERT_TRUE(UFile::exists(path)) << label() << " file missing";
|
||
|
||
std::map<int, Transform> posesOut;
|
||
std::multimap<int, Link> linksOut;
|
||
ASSERT_TRUE(graph::importPoses(path, /*format=*/4, posesOut, &linksOut))
|
||
<< label() << " importPoses failed";
|
||
|
||
expectPosesNearEqual(poses, posesOut, /*transTol=*/1e-4f,
|
||
/*rotTol=*/1e-4f, label() + " poses (incl. landmark)");
|
||
expectLinksNearEqual(links, linksOut, /*transTol=*/1e-4f,
|
||
/*rotTol=*/1e-4f, /*slam2d=*/force3DoF(),
|
||
/*landmarkWithRotation=*/landmarkWithRotation(),
|
||
/*priorWithRotation=*/priorWithRotation(),
|
||
label() + " links (incl. prior + landmark)");
|
||
}
|
||
|
||
INSTANTIATE_TEST_SUITE_P(
|
||
G2oVariants,
|
||
GraphIoG2oPriorsAndLandmarksTest,
|
||
::testing::Combine(
|
||
::testing::Bool() /*force3DoF*/,
|
||
::testing::Bool() /*landmarkWithRotation*/,
|
||
::testing::Bool() /*priorWithRotation*/),
|
||
[](const ::testing::TestParamInfo<std::tuple<bool, bool, bool>> & info)
|
||
{
|
||
return std::string(std::get<0>(info.param) ? "slam2d" : "slam3d")
|
||
+ "_"
|
||
+ (std::get<1>(info.param) ? "rotLm" : "pointLm")
|
||
+ "_"
|
||
+ (std::get<2>(info.param) ? "rotPrior" : "posPrior");
|
||
});
|
||
|
||
// -------------------------------------------------------------------------
|
||
// The type of a link (a loop closure against an odometry link, and which kind
|
||
// of loop closure) decides how the graph is traversed: Memory::getNeighborsId()
|
||
// follows a global closure without spending any depth, skips a proximity one
|
||
// and spends a depth on a neighbor. The g2o format defines no field for it, so
|
||
// OptimizerG2O writes it as a column past the ones it defines, which its own
|
||
// loader reads back and g2o's ignores.
|
||
//
|
||
// Also checks that a link handed over in both directions, which is how Memory
|
||
// stores it, is written once: g2o reads two lines as two constraints and would
|
||
// count the information of the link twice.
|
||
// -------------------------------------------------------------------------
|
||
TEST(GraphG2oTest, G2oRoundTripPreservesLinkTypes)
|
||
{
|
||
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
|
||
{
|
||
GTEST_SKIP() << "g2o optimizer not built in";
|
||
}
|
||
|
||
const Link::Type types[] = {
|
||
Link::kNeighbor,
|
||
Link::kNeighborMerged,
|
||
Link::kGlobalClosure,
|
||
Link::kLocalSpaceClosure,
|
||
Link::kLocalTimeClosure,
|
||
Link::kUserClosure,
|
||
};
|
||
const size_t typeCount = sizeof(types)/sizeof(Link::Type);
|
||
|
||
std::map<int, Transform> poses;
|
||
for(size_t i=0; i<=typeCount; ++i)
|
||
{
|
||
poses.insert(std::make_pair((int)i+1, Transform((float)i, 0.0f, 0.0f, 0, 0, 0)));
|
||
}
|
||
|
||
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
|
||
std::multimap<int, Link> links;
|
||
for(size_t i=0; i<typeCount; ++i)
|
||
{
|
||
const int from = (int)i+1, to = (int)i+2;
|
||
const Transform t = poses.at(from).inverse() * poses.at(to);
|
||
// Both directions, as Memory holds them: one link stored on each of the
|
||
// two nodes it connects.
|
||
links.insert(std::make_pair(from, Link(from, to, types[i], t, infMatrix)));
|
||
links.insert(std::make_pair(to, Link(to, from, types[i], t.inverse(), infMatrix)));
|
||
}
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_link_types_%d.g2o", test::getPid()));
|
||
UFile::erase(path);
|
||
ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links));
|
||
ASSERT_TRUE(UFile::exists(path));
|
||
|
||
// One line per link, not two, and each one carrying its type last.
|
||
std::ifstream file(path.c_str());
|
||
std::string line;
|
||
std::map<std::pair<int,int>, int> written;
|
||
while(std::getline(file, line))
|
||
{
|
||
if(line.compare(0, 5, "EDGE_") != 0)
|
||
{
|
||
continue;
|
||
}
|
||
std::istringstream in(line);
|
||
std::string tag;
|
||
int from = 0, to = 0;
|
||
in >> tag >> from >> to;
|
||
std::string last;
|
||
while(in >> last) {}
|
||
const std::pair<int,int> pair(std::min(from,to), std::max(from,to));
|
||
EXPECT_TRUE(written.insert(std::make_pair(pair, atoi(last.c_str()))).second)
|
||
<< "link " << from << "->" << to << " written more than once";
|
||
}
|
||
ASSERT_EQ(written.size(), typeCount);
|
||
for(size_t i=0; i<typeCount; ++i)
|
||
{
|
||
EXPECT_EQ(written.at(std::make_pair((int)i+1, (int)i+2)), (int)types[i])
|
||
<< "type column of link " << i+1 << "->" << i+2;
|
||
}
|
||
|
||
// And read back as the types they were.
|
||
std::map<int, Transform> posesOut;
|
||
std::multimap<int, Link> linksOut;
|
||
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
|
||
ASSERT_EQ(linksOut.size(), typeCount);
|
||
std::map<std::pair<int,int>, Link::Type> loaded;
|
||
for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
|
||
{
|
||
loaded.insert(std::make_pair(
|
||
std::make_pair(std::min(iter->second.from(), iter->second.to()),
|
||
std::max(iter->second.from(), iter->second.to())),
|
||
iter->second.type()));
|
||
}
|
||
for(size_t i=0; i<typeCount; ++i)
|
||
{
|
||
const std::pair<int,int> pair((int)i+1, (int)i+2);
|
||
ASSERT_TRUE(loaded.find(pair) != loaded.end()) << "link " << i+1 << "->" << i+2 << " missing";
|
||
EXPECT_EQ(loaded.at(pair), types[i]) << "type of link " << i+1 << "->" << i+2;
|
||
}
|
||
UFile::erase(path);
|
||
}
|
||
|
||
// A file without the type column, which is every file g2o itself writes and
|
||
// every one rtabmap wrote before, still loads: the type stays the one its tag
|
||
// implies, as it did.
|
||
TEST(GraphG2oTest, G2oWithoutTypeColumnStillLoads)
|
||
{
|
||
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
|
||
{
|
||
GTEST_SKIP() << "g2o optimizer not built in";
|
||
}
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_no_type_column_%d.g2o", test::getPid()));
|
||
UFile::erase(path);
|
||
{
|
||
std::ofstream file(path.c_str());
|
||
file << "VERTEX_SE2 1 0 0 0\n";
|
||
file << "VERTEX_SE2 2 1 0 0\n";
|
||
file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100\n";
|
||
}
|
||
|
||
std::map<int, Transform> poses;
|
||
std::multimap<int, Link> links;
|
||
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
|
||
EXPECT_EQ(poses.size(), 2u);
|
||
ASSERT_EQ(links.size(), 1u);
|
||
EXPECT_EQ(links.begin()->second.from(), 1);
|
||
EXPECT_EQ(links.begin()->second.to(), 2);
|
||
UFile::erase(path);
|
||
}
|
||
|
||
// The switchable edges of vertigo, which saveGraph() writes for a link that is not a neighbor
|
||
// when Optimizer/Robust is enabled: the tag inserts the id of a switch vertex of its own
|
||
// before the fields of the link, so the type column lands one field further than on the
|
||
// ordinary tags and the writer and the loader have to agree on where it sits.
|
||
TEST(GraphG2oTest, G2oRoundTripsSwitchableEdges)
|
||
{
|
||
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
|
||
{
|
||
GTEST_SKIP() << "g2o optimizer not built in";
|
||
}
|
||
|
||
// EDGE_SE2_SWITCHABLE in 2D, EDGE_SE3_SWITCHABLE in 3D.
|
||
for(int slam2d = 1; slam2d >= 0; --slam2d)
|
||
{
|
||
SCOPED_TRACE(slam2d ? "slam2d" : "slam3d");
|
||
|
||
std::map<int, Transform> poses;
|
||
poses.insert(std::make_pair(1, Transform(0.0f, 0.0f, 0.0f, 0, 0, 0)));
|
||
poses.insert(std::make_pair(2, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0)));
|
||
poses.insert(std::make_pair(3, Transform(2.0f, 0.0f, 0.0f, 0, 0, 0)));
|
||
|
||
const cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64F) * 100.0;
|
||
std::multimap<int, Link> links;
|
||
// The odometry links are written as the ordinary tag whatever Optimizer/Robust is,
|
||
// and the loop closure as the switchable one.
|
||
const int pairs[][3] = {
|
||
{1, 2, Link::kNeighbor},
|
||
{2, 3, Link::kNeighbor},
|
||
{1, 3, Link::kGlobalClosure}};
|
||
for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
|
||
{
|
||
const int from = pairs[i][0], to = pairs[i][1];
|
||
const Link::Type type = (Link::Type)pairs[i][2];
|
||
const Transform t = poses.at(from).inverse() * poses.at(to);
|
||
links.insert(std::make_pair(from, Link(from, to, type, t, infMatrix)));
|
||
links.insert(std::make_pair(to, Link(to, from, type, t.inverse(), infMatrix)));
|
||
}
|
||
|
||
ParametersMap params;
|
||
params.insert(ParametersPair(Parameters::kOptimizerRobust(), "true"));
|
||
params.insert(ParametersPair(Parameters::kRegForce3DoF(), slam2d ? "true" : "false"));
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_switchable_%d_%d.g2o", slam2d, test::getPid()));
|
||
UFile::erase(path);
|
||
ASSERT_TRUE(graph::exportPoses(path, 4 /*g2o*/, poses, links, std::map<int, double>(), params));
|
||
|
||
// Written as the switchable tag, which is what puts the type column one field further.
|
||
const std::string switchableTag = slam2d ? "EDGE_SE2_SWITCHABLE" : "EDGE_SE3_SWITCHABLE";
|
||
int switchableLines = 0;
|
||
{
|
||
std::ifstream file(path.c_str());
|
||
std::string line;
|
||
while(std::getline(file, line))
|
||
{
|
||
if(line.compare(0, switchableTag.size(), switchableTag) == 0)
|
||
{
|
||
++switchableLines;
|
||
}
|
||
}
|
||
}
|
||
EXPECT_EQ(switchableLines, 1);
|
||
|
||
std::map<int, Transform> posesOut;
|
||
std::multimap<int, Link> linksOut;
|
||
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, posesOut, &linksOut));
|
||
EXPECT_EQ(posesOut.size(), poses.size()); // the switch vertices are none of the poses
|
||
ASSERT_EQ(linksOut.size(), sizeof(pairs)/sizeof(pairs[0]));
|
||
|
||
std::map<std::pair<int,int>, Link::Type> loaded;
|
||
for(std::multimap<int, Link>::const_iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
|
||
{
|
||
loaded.insert(std::make_pair(
|
||
std::make_pair(std::min(iter->second.from(), iter->second.to()),
|
||
std::max(iter->second.from(), iter->second.to())),
|
||
iter->second.type()));
|
||
}
|
||
for(size_t i = 0; i < sizeof(pairs)/sizeof(pairs[0]); ++i)
|
||
{
|
||
const std::pair<int,int> pair(pairs[i][0], pairs[i][1]);
|
||
ASSERT_TRUE(loaded.find(pair) != loaded.end())
|
||
<< "link " << pair.first << "->" << pair.second << " missing";
|
||
EXPECT_EQ(loaded.at(pair), (Link::Type)pairs[i][2])
|
||
<< "type of link " << pair.first << "->" << pair.second;
|
||
}
|
||
UFile::erase(path);
|
||
}
|
||
}
|
||
|
||
// A type column holding something that is not one of the types, which nothing rtabmap writes
|
||
// but another writer of the same format could: it is ignored and the type stays the one the
|
||
// tag implies, so a column that means something else elsewhere cannot turn a link into a type
|
||
// it is not.
|
||
TEST(GraphG2oTest, G2oOutOfRangeTypeColumnIsIgnored)
|
||
{
|
||
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
|
||
{
|
||
GTEST_SKIP() << "g2o optimizer not built in";
|
||
}
|
||
|
||
const std::string path = test::tempPath(
|
||
uFormat("rtabmap_graph_bad_type_column_%d.g2o", test::getPid()));
|
||
UFile::erase(path);
|
||
{
|
||
std::ofstream file(path.c_str());
|
||
file << "VERTEX_SE2 1 0 0 0\n";
|
||
file << "VERTEX_SE2 2 1 0 0\n";
|
||
file << "EDGE_SE2 1 2 1 0 0 100 0 0 100 0 100 4242\n";
|
||
}
|
||
|
||
std::map<int, Transform> poses;
|
||
std::multimap<int, Link> links;
|
||
ASSERT_TRUE(graph::importPoses(path, 4 /*g2o*/, poses, &links));
|
||
EXPECT_EQ(poses.size(), 2u);
|
||
ASSERT_EQ(links.size(), 1u);
|
||
EXPECT_EQ(links.begin()->second.type(), Link::kUndef); // the type EDGE_SE2 implies
|
||
UFile::erase(path);
|
||
}
|