#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>

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: graph file formats (TORO / g2o) store edges generically and
	// don't preserve rtabmap's Link::Type tag, so we only round-trip
	// from / to / transform / infMatrix here. The loader assigns a
	// placeholder type for ordinary edges.
	//
	// 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");
		});