mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-11 22:10:21 +08:00
Muhammadandmatlabbe
f647014f54
* Add FlannIndex abstract interface and implement NanoFlannIndex subclass * Refactored: made NanoFlann a new NN type instead of inheriting FlannIndex. Added tests. Vendoring nanoflann.h directly in the repo. RegistrationVis now use NANOFLANN_INDEX_KDTREE_SINGLE (instead of FLANN_INDEX_KDTREE_SINGLE) flann index for 2d points matching. * cleanup comments, added FlannIndex doxygen * Fixing windows tests * updating flaky test * Simplified interface, added flann kdtree single approach selectable by parameters. * RegVis: symmetry of nanoflann for two branches of guess feature matching * cv::BFMatcher baseline * Small cmake optimization FLANN_KDTREE_MEM_OPT only defined for FlannIndex * Refactored where FLANN_KDTREE_MEM_OPT is defined * fixed file name already exist * cleanup * fixup build --------- Co-authored-by: matlabbe <matlabbe@gmail.com>
647 lines
23 KiB
C++
647 lines
23 KiB
C++
#include <gtest/gtest.h>
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <rtabmap/core/RegistrationVis.h>
|
|
#include <rtabmap/core/Parameters.h>
|
|
#include <rtabmap/core/Version.h>
|
|
#include <rtabmap/core/Features2d.h>
|
|
#include <rtabmap/core/CameraModel.h>
|
|
#include <rtabmap/core/StereoCameraModel.h>
|
|
#include <rtabmap/core/stereo/StereoBM.h>
|
|
#include <rtabmap/core/util2d.h>
|
|
#include <opencv2/imgcodecs.hpp>
|
|
#include <opencv2/imgproc.hpp>
|
|
|
|
using namespace rtabmap;
|
|
|
|
namespace {
|
|
|
|
// Shared by RGB-D and stereo-as-RGB-D (stereo depth from StereoBM).
|
|
static constexpr int kVisMaxFeatures = 3000;
|
|
static constexpr int kGfttMinDistance = 5;
|
|
static constexpr double kGfttQualityLevel = 0.01;
|
|
static const char kRoiRatios[] = "0 0 0 0.3";
|
|
|
|
static ParametersMap registrationVisTestParams(int estimationType = 1, int corType = 0)
|
|
{
|
|
ParametersMap params;
|
|
params[Parameters::kVisFeatureType()] = "8"; // GFTT/ORB
|
|
params[Parameters::kVisMaxFeatures()] = std::to_string(kVisMaxFeatures);
|
|
params[Parameters::kGFTTMinDistance()] = std::to_string(kGfttMinDistance);
|
|
params[Parameters::kGFTTQualityLevel()] = std::to_string(kGfttQualityLevel);
|
|
params[Parameters::kVisEstimationType()] = std::to_string(estimationType);
|
|
params[Parameters::kVisCorType()] = std::to_string(corType); // 0=feature matching, 1=optical flow
|
|
params[Parameters::kVisBundleAdjustment()] = "0";
|
|
params[Parameters::kVisRoiRatios()] = kRoiRatios;
|
|
if(corType == 1)
|
|
{
|
|
// Lucas-Kanade tracker defaults (win=16, levels=3) leave enough
|
|
// sub-pixel drift to push most correspondences past the 2 px
|
|
// reprojection bound on some OpenCV builds (SIMD/IPP differences in
|
|
// cv::calcOpticalFlowPyrLK). A larger window + more pyramid levels
|
|
// converge tighter and keep the OF test robust across platforms
|
|
// (~90% inlier ratio instead of ~5% on Ubuntu's DFSG OpenCV).
|
|
params[Parameters::kVisCorFlowWinSize()] = "21";
|
|
params[Parameters::kVisCorFlowMaxLevel()] = "5";
|
|
}
|
|
return params;
|
|
}
|
|
|
|
#if defined(RTABMAP_G2O)
|
|
static ParametersMap registrationVisTestParamsWithG2oBundleAdjustment(int estimationType, int corType = 0)
|
|
{
|
|
ParametersMap params = registrationVisTestParams(estimationType, corType);
|
|
params[Parameters::kVisBundleAdjustment()] = "1";
|
|
params[Parameters::kVisPnPRefineIterations()] = "0";
|
|
return params;
|
|
}
|
|
#endif
|
|
|
|
static void expectNearIdentity(const Transform & t)
|
|
{
|
|
ASSERT_FALSE(t.isNull());
|
|
if(t.isIdentity())
|
|
{
|
|
return;
|
|
}
|
|
EXPECT_NEAR(t.x(), 0.f, 0.05f);
|
|
EXPECT_NEAR(t.y(), 0.f, 0.05f);
|
|
EXPECT_NEAR(t.z(), 0.f, 0.05f);
|
|
EXPECT_NEAR(t.theta(), 0.f, 0.1f);
|
|
}
|
|
|
|
// Epipolar (Vis/EstimationType=2) is degenerate for identical views (no parallax).
|
|
static void expectSameFrameResult(int estimationType, const Transform & result, const RegistrationInfo & info, int minInliers)
|
|
{
|
|
if(estimationType == 2 && result.isNull())
|
|
{
|
|
EXPECT_EQ(info.inliers, 0);
|
|
return;
|
|
}
|
|
expectNearIdentity(result);
|
|
EXPECT_GE(info.inliers, minInliers);
|
|
}
|
|
|
|
static SensorData loadRgbdSensorData(const std::string & id)
|
|
{
|
|
const std::string root = std::string(RTABMAP_TEST_DATA_ROOT);
|
|
const cv::Mat depth = cv::imread(root + "/rgbd/depth/" + id + ".png", cv::IMREAD_UNCHANGED);
|
|
const cv::Mat rgb = cv::imread(root + "/rgbd/rgb/" + id + ".jpg", cv::IMREAD_COLOR);
|
|
if(depth.empty() || rgb.empty())
|
|
{
|
|
return SensorData();
|
|
}
|
|
CameraModel model;
|
|
if(!model.load(root + "/rgbd/calib/" + id + ".yaml"))
|
|
{
|
|
return SensorData();
|
|
}
|
|
return SensorData(rgb, depth, model);
|
|
}
|
|
|
|
// Stereo rectified pair -> RGB-D via StereoBM (same path as SensorCaptureThread stereo-to-depth).
|
|
static SensorData loadStereoSensorData(const std::string & id)
|
|
{
|
|
const std::string root = std::string(RTABMAP_TEST_DATA_ROOT);
|
|
const cv::Mat left = cv::imread(root + "/stereo_rect/left/" + id + ".jpg", cv::IMREAD_UNCHANGED);
|
|
const cv::Mat right = cv::imread(root + "/stereo_rect/right/" + id + ".jpg", cv::IMREAD_GRAYSCALE);
|
|
if(left.empty() || right.empty())
|
|
{
|
|
return SensorData();
|
|
}
|
|
StereoCameraModel stereoModel;
|
|
if(!stereoModel.load(root + "/stereo_rect", "stereo") || !stereoModel.isValidForProjection())
|
|
{
|
|
return SensorData();
|
|
}
|
|
|
|
return SensorData(left, right, stereoModel);
|
|
}
|
|
|
|
static std::string estimationTypeName(int type)
|
|
{
|
|
switch(type)
|
|
{
|
|
case 0: return "3DTo3D";
|
|
case 1: return "PnP";
|
|
case 2: return "Epipolar";
|
|
default: return "Unknown";
|
|
}
|
|
}
|
|
|
|
static Transform computeRegistration(
|
|
const SensorData & fromData,
|
|
const SensorData & toData,
|
|
const ParametersMap & params,
|
|
RegistrationInfo * infoOut = nullptr,
|
|
const Transform & guess = Transform())
|
|
{
|
|
RegistrationVis reg(params);
|
|
Signature from(fromData);
|
|
Signature to(toData);
|
|
RegistrationInfo info;
|
|
return reg.computeTransformation(from, to, guess, infoOut ? infoOut : &info);
|
|
}
|
|
|
|
// Golden transforms (GFTT/ORB, MinDistance=3, QualityLevel=0.01, MaxFeatures=3000, RoiRatios=0 0 0 0.3).
|
|
// Captured with Vis/CorType=0 (feature matching); also used for optical flow (CorType=1) within tolerance.
|
|
// Shared by FM/OF, Vis/BundleAdjustment=0 and g2o BA=1.
|
|
static constexpr float kGoldenTransTolM = 0.20f;
|
|
static constexpr float kGoldenTransTolEpipolarM = 0.70f; // CI can give up to 0.61 depending on platform and opencv version used
|
|
static constexpr float kGoldenAngleTolRad = 0.28f;
|
|
|
|
static float goldenTransTolForEstimationType(int estimationType)
|
|
{
|
|
return estimationType == 2 ? kGoldenTransTolEpipolarM : kGoldenTransTolM;
|
|
}
|
|
|
|
// rgbd/17 -> rgbd/154
|
|
static const Transform kRgbd17To154Expected[3] = {
|
|
Transform(0.99870515f, 0.04859976f, 0.01503834f, 0.35593706f,
|
|
-0.04851507f, 0.99880475f, -0.00594683f, -0.21210882f,
|
|
-0.01530938f, 0.00520954f, 0.99986923f, 0.04961354f),
|
|
Transform(0.99626255f, 0.08612057f, 0.00666280f, 0.40622258f,
|
|
-0.08607896f, 0.99626857f, -0.00629793f, -0.15886482f,
|
|
-0.00718031f, 0.00570087f, 0.99995804f, 0.02304785f),
|
|
Transform(0.99893183f, 0.03752048f, -0.02697127f, 0.51900554f,
|
|
-0.03745415f, 0.99929398f, 0.00296024f, -0.29134610f,
|
|
0.02706329f, -0.00194689f, 0.99963182f, -0.10698023f)};
|
|
|
|
// stereo_rect/50 -> stereo_rect/60 (native stereo rectified)
|
|
static const Transform kStereo50To60Expected[3] = {
|
|
Transform(0.99237216f, -0.11357912f, 0.04792929f, 0.13281979f,
|
|
0.11265049f, 0.99339861f, 0.02165955f, 0.14165790f,
|
|
-0.05007296f, -0.01609508f, 0.99861586f, 0.02466334f),
|
|
Transform(0.99054426f, -0.12871768f, 0.04747626f, 0.13877144f,
|
|
0.12772329f, 0.99153322f, 0.02342802f, 0.02014065f,
|
|
-0.05008988f, -0.01714267f, 0.99859768f, 0.02536142f),
|
|
Transform(0.99255836f, -0.11204252f, 0.04769079f, 0.15163948f,
|
|
0.11127493f, 0.99361807f, 0.01846521f, 0.16984999f,
|
|
-0.04945532f, -0.01302101f, 0.99869144f, 0.02214202f)};
|
|
|
|
static void expectTransformNearExpected(
|
|
const Transform & result,
|
|
const Transform & expected,
|
|
float transTolM,
|
|
float angleTolRad,
|
|
const std::string & label)
|
|
{
|
|
ASSERT_FALSE(result.isNull()) << label;
|
|
EXPECT_LT(result.getDistance(expected), transTolM) << label;
|
|
EXPECT_LT(result.getAngle(expected), angleTolRad) << label;
|
|
}
|
|
|
|
static Transform computeRegistrationRobust(
|
|
const SensorData & fromData,
|
|
const SensorData & toData,
|
|
const ParametersMap & params,
|
|
RegistrationInfo * infoOut = nullptr,
|
|
const Transform & guess = Transform())
|
|
{
|
|
const int estimationType = std::atoi(params.at(Parameters::kVisEstimationType()).c_str());
|
|
// Epipolar (type=2) is the most RANSAC-sensitive so it gets the biggest
|
|
// attempt budget; PnP (type=1) also uses RANSAC internally via
|
|
// cv::solvePnPRansac and has been observed to fail one-shot on CI's
|
|
// Ubuntu OpenCV build, so retry it too. F2F (type=0) is largely
|
|
// deterministic but cheap, so we still seed and retry a few times for
|
|
// uniformity.
|
|
const int maxAttempts = estimationType == 2 ? 50 : 10;
|
|
Transform result;
|
|
RegistrationInfo info;
|
|
for(int attempt = 0; attempt < maxAttempts; ++attempt)
|
|
{
|
|
cv::theRNG() = cv::RNG(static_cast<uint64_t>(0x9e3779b97f4a7c15ULL) ^
|
|
static_cast<uint64_t>(attempt + 1));
|
|
result = computeRegistration(fromData, toData, params, &info, guess);
|
|
if(!result.isNull())
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
if(infoOut)
|
|
{
|
|
*infoOut = info;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
static void expectAllEstimationTypesMatchExpected(
|
|
const SensorData & fromData,
|
|
const SensorData & toData,
|
|
const Transform expectedByType[3],
|
|
float transTolM,
|
|
float angleTolRad,
|
|
const std::function<ParametersMap(int)> & paramsForEstimationType =
|
|
[](int estimationType) { return registrationVisTestParams(estimationType); })
|
|
{
|
|
for(int estimationType = 0; estimationType <= 2; ++estimationType)
|
|
{
|
|
RegistrationInfo info;
|
|
const Transform result = computeRegistrationRobust(
|
|
fromData, toData, paramsForEstimationType(estimationType), &info);
|
|
const std::string label = estimationTypeName(estimationType);
|
|
const float transTol = goldenTransTolForEstimationType(estimationType);
|
|
expectTransformNearExpected(result, expectedByType[estimationType], transTol, angleTolRad, label);
|
|
EXPECT_GE(info.inliers, 6) << label;
|
|
EXPECT_FALSE(info.covariance.empty()) << label;
|
|
}
|
|
}
|
|
|
|
class RegistrationVisEstimationTypeTest : public ::testing::TestWithParam<int>
|
|
{
|
|
protected:
|
|
ParametersMap params() const
|
|
{
|
|
return registrationVisTestParams(GetParam());
|
|
}
|
|
};
|
|
|
|
} // namespace
|
|
|
|
TEST(RegistrationVisTest, ConstructorAndParseParameters)
|
|
{
|
|
RegistrationVis reg(registrationVisTestParams());
|
|
EXPECT_TRUE(reg.isImageRequired());
|
|
EXPECT_FALSE(reg.isScanRequired());
|
|
EXPECT_EQ(reg.getMinInliers(), 20);
|
|
EXPECT_EQ(reg.getEstimationType(), 1);
|
|
EXPECT_NE(reg.getDetector(), nullptr);
|
|
|
|
ParametersMap update;
|
|
update[Parameters::kVisMinInliers()] = "10";
|
|
update[Parameters::kVisInlierDistance()] = "0.2";
|
|
update[Parameters::kVisEstimationType()] = "2";
|
|
reg.parseParameters(update);
|
|
EXPECT_EQ(reg.getMinInliers(), 10);
|
|
EXPECT_FLOAT_EQ(reg.getInlierDistance(), 0.2f);
|
|
EXPECT_EQ(reg.getEstimationType(), 2);
|
|
}
|
|
|
|
TEST(RegistrationVisTest, ParseParametersWithGetters)
|
|
{
|
|
struct Case
|
|
{
|
|
ParametersMap update;
|
|
std::function<void(const RegistrationVis &)> check;
|
|
};
|
|
|
|
const std::vector<Case> cases = {
|
|
{{{Parameters::kVisMinInliers(), "12"}},
|
|
[](const RegistrationVis & reg) { EXPECT_EQ(reg.getMinInliers(), 12); }},
|
|
{{{Parameters::kVisMinInliers(), "2"}},
|
|
[](const RegistrationVis & reg) {
|
|
EXPECT_EQ(reg.getMinInliers(), 6); // clamped to minimum 6
|
|
}},
|
|
{{{Parameters::kVisInlierDistance(), "0.25"}},
|
|
[](const RegistrationVis & reg) { EXPECT_FLOAT_EQ(reg.getInlierDistance(), 0.25f); }},
|
|
{{{Parameters::kVisIterations(), "150"}},
|
|
[](const RegistrationVis & reg) { EXPECT_EQ(reg.getIterations(), 150); }},
|
|
{{{Parameters::kVisEstimationType(), "0"}},
|
|
[](const RegistrationVis & reg) { EXPECT_EQ(reg.getEstimationType(), 0); }},
|
|
{{{Parameters::kVisEstimationType(), "2"}},
|
|
[](const RegistrationVis & reg) { EXPECT_EQ(reg.getEstimationType(), 2); }},
|
|
{{{Parameters::kVisCorNNDR(), "0.55"}},
|
|
[](const RegistrationVis & reg) { EXPECT_FLOAT_EQ(reg.getNNDR(), 0.55f); }},
|
|
{{{Parameters::kVisCorNNType(), "3"}},
|
|
[](const RegistrationVis & reg) { EXPECT_EQ(reg.getNNType(), 3); }},
|
|
{{{Parameters::kVisMaxFeatures(), "120"}},
|
|
[](const RegistrationVis & reg) {
|
|
ASSERT_NE(reg.getDetector(), nullptr);
|
|
EXPECT_EQ(reg.getDetector()->getMaxFeatures(), 120);
|
|
}},
|
|
{{{Parameters::kVisFeatureType(), "2"}},
|
|
[](const RegistrationVis & reg) {
|
|
ASSERT_NE(reg.getDetector(), nullptr);
|
|
EXPECT_EQ(reg.getDetector()->getType(), Feature2D::kFeatureOrb);
|
|
}},
|
|
{{{Parameters::kVisCorType(), "1"}},
|
|
[](const RegistrationVis & reg) { EXPECT_TRUE(reg.canUseGuess()); }},
|
|
{{{Parameters::kVisCorGuessWinSize(), "30"}},
|
|
[](const RegistrationVis & reg) { EXPECT_TRUE(reg.canUseGuess()); }},
|
|
};
|
|
|
|
for(size_t i = 0; i < cases.size(); ++i)
|
|
{
|
|
RegistrationVis reg(registrationVisTestParams());
|
|
reg.parseParameters(cases[i].update);
|
|
cases[i].check(reg);
|
|
}
|
|
}
|
|
|
|
TEST(RegistrationVisTest, ParseMaxFeaturesCapsExtractedKeypoints)
|
|
{
|
|
const SensorData data = loadRgbdSensorData("17");
|
|
ASSERT_FALSE(data.imageRaw().empty());
|
|
|
|
ParametersMap params = registrationVisTestParams();
|
|
params[Parameters::kVisMaxFeatures()] = "80";
|
|
RegistrationVis reg(params);
|
|
|
|
ASSERT_NE(reg.getDetector(), nullptr);
|
|
EXPECT_EQ(reg.getDetector()->getMaxFeatures(), 80);
|
|
|
|
Signature from(data);
|
|
Signature to;
|
|
RegistrationInfo info;
|
|
Transform nullGuess;
|
|
reg.computeTransformationMod(from, to, nullGuess, &info);
|
|
|
|
EXPECT_LE(from.sensorData().keypoints().size(), 80u);
|
|
EXPECT_GT(from.sensorData().keypoints().size(), 0u);
|
|
EXPECT_FALSE(from.sensorData().descriptors().empty());
|
|
}
|
|
|
|
TEST(RegistrationVisTest, HighMinInliersRejectsRegistration)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
ParametersMap params = registrationVisTestParams(1);
|
|
params[Parameters::kVisMinInliers()] = "10000";
|
|
|
|
RegistrationInfo info;
|
|
const Transform result = computeRegistration(fromData, toData, params, &info);
|
|
EXPECT_TRUE(result.isNull());
|
|
}
|
|
|
|
TEST(RegistrationVisTest, StrictCorNndrReducesInliers)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
ParametersMap looseParams = registrationVisTestParams(1);
|
|
looseParams[Parameters::kVisMinInliers()] = "6";
|
|
looseParams[Parameters::kVisCorNNDR()] = "0.99";
|
|
ParametersMap strictParams = registrationVisTestParams(1);
|
|
strictParams[Parameters::kVisMinInliers()] = "6";
|
|
strictParams[Parameters::kVisCorNNDR()] = "0.01";
|
|
|
|
RegistrationInfo looseInfo;
|
|
RegistrationInfo strictInfo;
|
|
const Transform loose = computeRegistration(fromData, toData, looseParams, &looseInfo);
|
|
const Transform strict = computeRegistration(fromData, toData, strictParams, &strictInfo);
|
|
|
|
ASSERT_FALSE(loose.isNull());
|
|
EXPECT_GE(looseInfo.inliers, strictInfo.inliers);
|
|
}
|
|
|
|
TEST(RegistrationVisTest, RgbdTwoFramesMatchExpectedTransformOpticalFlow)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData,
|
|
toData,
|
|
kRgbd17To154Expected,
|
|
kGoldenTransTolM,
|
|
kGoldenAngleTolRad,
|
|
[](int estimationType) { return registrationVisTestParams(estimationType, 1); });
|
|
}
|
|
|
|
TEST(RegistrationVisTest, StereoTwoFramesMatchExpectedTransformOpticalFlow)
|
|
{
|
|
const SensorData fromData = loadStereoSensorData("50");
|
|
const SensorData toData = loadStereoSensorData("60");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData,
|
|
toData,
|
|
kStereo50To60Expected,
|
|
kGoldenTransTolM,
|
|
kGoldenAngleTolRad,
|
|
[](int estimationType) { return registrationVisTestParams(estimationType, 1); });
|
|
}
|
|
|
|
TEST_P(RegistrationVisEstimationTypeTest, RgbdSameFrameNearIdentity)
|
|
{
|
|
const SensorData data = loadRgbdSensorData("17");
|
|
ASSERT_FALSE(data.imageRaw().empty());
|
|
ASSERT_FALSE(data.depthRaw().empty());
|
|
|
|
RegistrationVis reg(registrationVisTestParams(GetParam()));
|
|
EXPECT_EQ(reg.getEstimationType(), GetParam());
|
|
EXPECT_EQ(reg.getDetector()->getMaxFeatures(), kVisMaxFeatures);
|
|
|
|
const Signature from(data);
|
|
const Signature to(data);
|
|
|
|
RegistrationInfo info;
|
|
Transform nullGuess;
|
|
const Transform result = reg.computeTransformation(from, to, nullGuess, &info);
|
|
|
|
expectSameFrameResult(GetParam(), result, info, reg.getMinInliers());
|
|
if(!result.isNull())
|
|
{
|
|
EXPECT_GE(info.matches, info.inliers);
|
|
EXPECT_FALSE(info.covariance.empty());
|
|
}
|
|
}
|
|
|
|
TEST(RegistrationVisTest, RgbdTwoFramesMatchExpectedTransform)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData,
|
|
toData,
|
|
kRgbd17To154Expected,
|
|
kGoldenTransTolM,
|
|
kGoldenAngleTolRad,
|
|
[](int estimationType) { return registrationVisTestParams(estimationType, 0); });
|
|
}
|
|
|
|
TEST_P(RegistrationVisEstimationTypeTest, StereoSameFrameNearIdentity)
|
|
{
|
|
const SensorData data = loadStereoSensorData("50");
|
|
ASSERT_FALSE(data.imageRaw().empty());
|
|
ASSERT_FALSE(data.rightRaw().empty());
|
|
ASSERT_FALSE(data.stereoCameraModels().empty());
|
|
|
|
RegistrationVis reg(params());
|
|
EXPECT_EQ(reg.getEstimationType(), GetParam());
|
|
|
|
const Signature from(data);
|
|
const Signature to(data);
|
|
|
|
RegistrationInfo info;
|
|
Transform nullGuess;
|
|
const Transform result = reg.computeTransformation(from, to, nullGuess, &info);
|
|
|
|
expectSameFrameResult(GetParam(), result, info, reg.getMinInliers());
|
|
if(!result.isNull())
|
|
{
|
|
EXPECT_FALSE(info.covariance.empty());
|
|
}
|
|
}
|
|
|
|
TEST(RegistrationVisTest, StereoTwoFramesMatchExpectedTransform)
|
|
{
|
|
const SensorData fromData = loadStereoSensorData("50");
|
|
const SensorData toData = loadStereoSensorData("60");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData, toData, kStereo50To60Expected, kGoldenTransTolM, kGoldenAngleTolRad);
|
|
}
|
|
|
|
TEST(RegistrationVisTest, StereoFeatureMatchingAndOpticalFlowSucceed3DTo3D)
|
|
{
|
|
const SensorData fromData = loadStereoSensorData("50");
|
|
const SensorData toData = loadStereoSensorData("60");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
RegistrationInfo info;
|
|
const Transform fm = computeRegistration(fromData, toData, registrationVisTestParams(0, 0), &info);
|
|
const Transform of = computeRegistration(fromData, toData, registrationVisTestParams(0, 1), &info);
|
|
ASSERT_FALSE(fm.isNull());
|
|
ASSERT_FALSE(of.isNull());
|
|
EXPECT_LT(fm.getDistance(of), kGoldenTransTolM);
|
|
}
|
|
|
|
#if defined(RTABMAP_G2O)
|
|
// Guess based matching (Vis/CorGuessWinSize): the words of "from" are projected
|
|
// into "to" with the guess, and only the features within a radius of their
|
|
// projection are compared. Vis/CorGuessMatchToProjection picks which of the two
|
|
// sets is indexed and which one searches it, so both directions index 2D points
|
|
// and search them by radius.
|
|
TEST(RegistrationVisTest, RgbdGuessMatchingBothDirections)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
// A guess a few centimeters and a degree away from the answer, the way
|
|
// odometry gives one. Far enough to matter, close enough for the projected
|
|
// words to land inside the search radius.
|
|
const Transform expected = kRgbd17To154Expected[1]; // PnP
|
|
const Transform guess = expected * Transform(0.03f, -0.02f, 0.01f, 0.0f, 0.0f, 0.02f);
|
|
|
|
for(bool matchToProjection: {false, true})
|
|
{
|
|
ParametersMap params = registrationVisTestParams(1 /* PnP */);
|
|
params[Parameters::kVisCorGuessMatchToProjection()] = matchToProjection?"true":"false";
|
|
|
|
RegistrationInfo info;
|
|
const Transform result = computeRegistrationRobust(fromData, toData, params, &info, guess);
|
|
|
|
const std::string label = std::string(Parameters::kVisCorGuessMatchToProjection()) +
|
|
(matchToProjection?"=true":"=false");
|
|
expectTransformNearExpected(result, expected, kGoldenTransTolM, kGoldenAngleTolRad, label);
|
|
EXPECT_GE(info.inliers, 6) << label;
|
|
// Only the guess based matching fills projectedIDs, whichever of its two
|
|
// directions is taken, so this is what tells that it produced the
|
|
// result rather than the plain descriptor matching. OdometryF2M relies
|
|
// on them being filled to know which words of its map are still seen.
|
|
EXPECT_FALSE(info.projectedIDs.empty()) << label;
|
|
}
|
|
}
|
|
|
|
TEST(RegistrationVisTest, RgbdTwoFramesMatchExpectedTransformWithG2oBundleAdjustment)
|
|
{
|
|
const SensorData fromData = loadRgbdSensorData("17");
|
|
const SensorData toData = loadRgbdSensorData("154");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData,
|
|
toData,
|
|
kRgbd17To154Expected,
|
|
kGoldenTransTolM,
|
|
kGoldenAngleTolRad,
|
|
[](int estimationType) {
|
|
return registrationVisTestParamsWithG2oBundleAdjustment(estimationType, 0);
|
|
});
|
|
}
|
|
|
|
TEST(RegistrationVisTest, StereoTwoFramesMatchExpectedTransformWithG2oBundleAdjustment)
|
|
{
|
|
const SensorData fromData = loadStereoSensorData("50");
|
|
const SensorData toData = loadStereoSensorData("60");
|
|
ASSERT_FALSE(fromData.imageRaw().empty());
|
|
ASSERT_FALSE(toData.imageRaw().empty());
|
|
|
|
expectAllEstimationTypesMatchExpected(
|
|
fromData,
|
|
toData,
|
|
kStereo50To60Expected,
|
|
kGoldenTransTolM,
|
|
kGoldenAngleTolRad,
|
|
[](int estimationType) {
|
|
return registrationVisTestParamsWithG2oBundleAdjustment(estimationType);
|
|
});
|
|
}
|
|
#endif
|
|
|
|
INSTANTIATE_TEST_SUITE_P(
|
|
AllEstimationTypes,
|
|
RegistrationVisEstimationTypeTest,
|
|
::testing::Values(0, 1, 2),
|
|
[](const ::testing::TestParamInfo<int> & info) {
|
|
return estimationTypeName(info.param);
|
|
});
|
|
|
|
// Regenerate golden Transform(...) lines: build and run
|
|
// ./bin/test_registrationvis --gtest_also_run_disabled_tests --gtest_filter='*CaptureGoldenTransforms*'
|
|
TEST(RegistrationVisTest, DISABLED_CaptureGoldenTransforms)
|
|
{
|
|
auto printTransform = [](const char * label, const Transform & t) {
|
|
printf("\t\t// %s xyz=(%.8f,%.8f,%.8f)\n", label, t.x(), t.y(), t.z());
|
|
printf("\t\tTransform(%.8ff, %.8ff, %.8ff, %.8ff,\n", t.r11(), t.r12(), t.r13(), t.x());
|
|
printf("\t\t\t\t%.8ff, %.8ff, %.8ff, %.8ff,\n", t.r21(), t.r22(), t.r23(), t.y());
|
|
printf("\t\t\t\t%.8ff, %.8ff, %.8ff, %.8ff),\n", t.r31(), t.r32(), t.r33(), t.z());
|
|
};
|
|
const SensorData rgbdFrom = loadRgbdSensorData("17");
|
|
const SensorData rgbdTo = loadRgbdSensorData("154");
|
|
const SensorData stereoFrom = loadStereoSensorData("50");
|
|
const SensorData stereoTo = loadStereoSensorData("60");
|
|
ASSERT_FALSE(rgbdFrom.imageRaw().empty());
|
|
ASSERT_FALSE(stereoFrom.imageRaw().empty());
|
|
|
|
auto capture = [&](const char * setName, const SensorData & from, const SensorData & to) {
|
|
printf("// %s (Vis/CorType=0; shared golden for FM and OF)\n", setName);
|
|
printf("static const Transform kExpected[3] = {\n");
|
|
for(int estimationType = 0; estimationType <= 2; ++estimationType)
|
|
{
|
|
RegistrationInfo info;
|
|
const Transform result = computeRegistrationRobust(
|
|
from, to, registrationVisTestParams(estimationType, 0), &info);
|
|
printTransform(estimationTypeName(estimationType).c_str(), result);
|
|
}
|
|
printf("};\n");
|
|
};
|
|
capture("rgbd/17 -> rgbd/154", rgbdFrom, rgbdTo);
|
|
capture("stereo_rect/50 -> stereo_rect/60", stereoFrom, stereoTo);
|
|
|
|
auto printFmOfDist = [&](const char * label, const SensorData & from, const SensorData & to) {
|
|
for(int estimationType = 0; estimationType <= 2; ++estimationType)
|
|
{
|
|
const Transform fm = computeRegistrationRobust(
|
|
from, to, registrationVisTestParams(estimationType, 0));
|
|
const Transform of = computeRegistrationRobust(
|
|
from, to, registrationVisTestParams(estimationType, 1));
|
|
printf("// %s %s FM-OF distance: %.6f m\n",
|
|
label, estimationTypeName(estimationType).c_str(), fm.getDistance(of));
|
|
}
|
|
};
|
|
printFmOfDist("rgbd", rgbdFrom, rgbdTo);
|
|
printFmOfDist("stereo", stereoFrom, stereoTo);
|
|
}
|