mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
Added Registration tests
This commit is contained in:
matlabbe
@@ -36,52 +36,90 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
/**
|
||||
* @class Registration
|
||||
* @brief Abstract base for registering two observations (visual, ICP, or both).
|
||||
*
|
||||
* Factory @ref create() builds @ref RegistrationVis, @ref RegistrationIcp, or
|
||||
* @ref RegistrationVisIcp from **Reg/Strategy** in a @ref ParametersMap.
|
||||
*
|
||||
* @ref computeTransformation() wraps signatures and delegates to
|
||||
* @ref computeTransformationMod(), which calls @ref computeTransformationImpl()
|
||||
* on the concrete strategy, optionally chains a @ref child_ registration (e.g. ICP
|
||||
* after visual), applies **Reg/Force3DoF**, and may repeat once with the first
|
||||
* result as guess when **Reg/RepeatOnce** is enabled.
|
||||
*
|
||||
* Requirement queries (@ref isImageRequired(), @ref isScanRequired(), etc.) OR-combine
|
||||
* with the child when the parent returns false. Minimum correspondence thresholds
|
||||
* take the maximum between parent and child.
|
||||
*
|
||||
* @see RegistrationVis
|
||||
* @see RegistrationIcp
|
||||
* @see RegistrationInfo
|
||||
*/
|
||||
class RTABMAP_CORE_EXPORT Registration
|
||||
{
|
||||
public:
|
||||
/** @brief Registration strategy selected by **Reg/Strategy**. */
|
||||
enum Type {
|
||||
kTypeUndef = -1,
|
||||
kTypeVis = 0,
|
||||
kTypeIcp = 1,
|
||||
kTypeVisIcp = 2
|
||||
};
|
||||
/** @brief Minimum diagonal value for linear covariance (m²). */
|
||||
static double COVARIANCE_LINEAR_EPSILON;
|
||||
/** @brief Minimum diagonal value for angular covariance (rad²). */
|
||||
static double COVARIANCE_ANGULAR_EPSILON;
|
||||
|
||||
public:
|
||||
/** @brief Creates a registration from **Reg/Strategy** in @p parameters. Caller owns the pointer. */
|
||||
static Registration * create(const ParametersMap & parameters);
|
||||
/** @brief Creates a registration of @p type (may be adjusted, e.g. to @ref kTypeVis). Caller owns the pointer. */
|
||||
static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
|
||||
|
||||
public:
|
||||
virtual ~Registration();
|
||||
/** @brief Parses **Reg/RepeatOnce**, **Reg/Force3DoF** and forwards to @ref child_ if set. */
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
|
||||
/** @brief True if images are needed (parent or child). */
|
||||
bool isImageRequired() const;
|
||||
/** @brief True if laser scans are needed (parent or child). */
|
||||
bool isScanRequired() const;
|
||||
/** @brief True if user data is needed (parent or child). */
|
||||
bool isUserDataRequired() const;
|
||||
|
||||
/** @brief True if the strategy can refine an initial guess (parent or child). */
|
||||
bool canUseGuess() const;
|
||||
|
||||
/** @brief Minimum visual inliers required (max of parent and child thresholds). */
|
||||
int getMinVisualCorrespondences() const;
|
||||
/** @brief Minimum geometry inlier ratio required (max of parent and child thresholds). */
|
||||
float getMinGeometryCorrespondencesRatio() const;
|
||||
|
||||
/** @return Value of **Reg/RepeatOnce**. */
|
||||
bool repeatOnce() const {return repeatOnce_;}
|
||||
/** @return Value of **Reg/Force3DoF**. */
|
||||
bool force3DoF() const {return force3DoF_;}
|
||||
|
||||
// take ownership!
|
||||
/** @brief Replaces @ref child_; takes ownership of @p child. */
|
||||
void setChildRegistration(Registration * child);
|
||||
|
||||
/** @brief Registers @p from to @p to using immutable signatures (copied internally). */
|
||||
Transform computeTransformation(
|
||||
const Signature & from,
|
||||
const Signature & to,
|
||||
Transform guess = Transform::getIdentity(),
|
||||
RegistrationInfo * info = 0) const;
|
||||
/** @brief Registers two @ref SensorData observations (wrapped as signatures). */
|
||||
Transform computeTransformation(
|
||||
const SensorData & from,
|
||||
const SensorData & to,
|
||||
Transform guess = Transform::getIdentity(),
|
||||
RegistrationInfo * info = 0) const;
|
||||
|
||||
/** @brief Registers @p from to @p to; signatures may be modified by the implementation. */
|
||||
Transform computeTransformationMod(
|
||||
Signature & from,
|
||||
Signature & to,
|
||||
@@ -89,11 +127,14 @@ public:
|
||||
RegistrationInfo * info = 0) const;
|
||||
|
||||
protected:
|
||||
// take ownership of child
|
||||
/** @brief @p child is owned and deleted in the destructor. */
|
||||
Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
|
||||
|
||||
// It is safe to modify the signatures in the implementation, if so, the
|
||||
// child registration will use these modifications.
|
||||
/**
|
||||
* @brief Strategy-specific registration.
|
||||
*
|
||||
* May modify @p from and @p to; a @ref child_ registration reuses those changes.
|
||||
*/
|
||||
virtual Transform computeTransformationImpl(
|
||||
Signature & from,
|
||||
Signature & to,
|
||||
|
||||
@@ -141,6 +141,11 @@ add_executable(test_globalmap test_globalmap.cpp)
|
||||
target_link_libraries(test_globalmap gtest_main rtabmap_core)
|
||||
add_test(NAME test_globalmap COMMAND test_globalmap)
|
||||
|
||||
#Registration.h
|
||||
add_executable(test_registration test_registration.cpp)
|
||||
target_link_libraries(test_registration gtest_main rtabmap_core)
|
||||
add_test(NAME test_registration COMMAND test_registration)
|
||||
|
||||
#LocalGridMaker.h
|
||||
add_executable(test_localgridmaker test_localgridmaker.cpp)
|
||||
target_link_libraries(test_localgridmaker gtest_main rtabmap_core)
|
||||
|
||||
@@ -0,0 +1,266 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rtabmap/core/Registration.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
using namespace rtabmap;
|
||||
|
||||
namespace {
|
||||
|
||||
class MockRegistration : public Registration
|
||||
{
|
||||
public:
|
||||
explicit MockRegistration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0) :
|
||||
Registration(parameters, child)
|
||||
{
|
||||
}
|
||||
|
||||
mutable int implCallCount = 0;
|
||||
mutable std::vector<Transform> guessHistory;
|
||||
Transform resultTransform = Transform::getIdentity();
|
||||
bool failRegistration = false;
|
||||
|
||||
bool imageRequiredFlag = false;
|
||||
bool scanRequiredFlag = false;
|
||||
bool userDataRequiredFlag = false;
|
||||
bool canUseGuessFlag = false;
|
||||
int minVisualValue = 0;
|
||||
float minGeometryValue = 0.f;
|
||||
|
||||
protected:
|
||||
Transform computeTransformationImpl(
|
||||
Signature & /*from*/,
|
||||
Signature & /*to*/,
|
||||
Transform guess,
|
||||
RegistrationInfo & /*info*/) const override
|
||||
{
|
||||
++implCallCount;
|
||||
guessHistory.push_back(guess);
|
||||
if(failRegistration)
|
||||
{
|
||||
return Transform();
|
||||
}
|
||||
return resultTransform;
|
||||
}
|
||||
|
||||
bool isImageRequiredImpl() const override { return imageRequiredFlag; }
|
||||
bool isScanRequiredImpl() const override { return scanRequiredFlag; }
|
||||
bool isUserDataRequiredImpl() const override { return userDataRequiredFlag; }
|
||||
bool canUseGuessImpl() const override { return canUseGuessFlag; }
|
||||
int getMinVisualCorrespondencesImpl() const override { return minVisualValue; }
|
||||
float getMinGeometryCorrespondencesRatioImpl() const override { return minGeometryValue; }
|
||||
};
|
||||
|
||||
static ParametersMap registrationTestParams()
|
||||
{
|
||||
ParametersMap params;
|
||||
params.insert(ParametersPair(Parameters::kRegRepeatOnce(), "false"));
|
||||
params.insert(ParametersPair(Parameters::kRegForce3DoF(), "false"));
|
||||
return params;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(RegistrationTest, ConstructorParsesParameters)
|
||||
{
|
||||
ParametersMap params;
|
||||
params[Parameters::kRegRepeatOnce()] = "true";
|
||||
params[Parameters::kRegForce3DoF()] = "true";
|
||||
|
||||
MockRegistration reg(params);
|
||||
EXPECT_TRUE(reg.repeatOnce());
|
||||
EXPECT_TRUE(reg.force3DoF());
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, ParseParametersUpdatesFlags)
|
||||
{
|
||||
MockRegistration reg(registrationTestParams());
|
||||
EXPECT_FALSE(reg.repeatOnce());
|
||||
EXPECT_FALSE(reg.force3DoF());
|
||||
|
||||
ParametersMap update;
|
||||
update[Parameters::kRegRepeatOnce()] = "true";
|
||||
reg.parseParameters(update);
|
||||
EXPECT_TRUE(reg.repeatOnce());
|
||||
EXPECT_FALSE(reg.force3DoF());
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, RequirementFlagsCombineWithChild)
|
||||
{
|
||||
MockRegistration * child = new MockRegistration();
|
||||
child->scanRequiredFlag = true;
|
||||
|
||||
MockRegistration parent(registrationTestParams(), child);
|
||||
EXPECT_FALSE(parent.isImageRequired());
|
||||
EXPECT_TRUE(parent.isScanRequired());
|
||||
EXPECT_FALSE(parent.isUserDataRequired());
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, MinCorrespondencesTakeMaximumWithChild)
|
||||
{
|
||||
MockRegistration * child = new MockRegistration();
|
||||
child->minVisualValue = 10;
|
||||
child->minGeometryValue = 0.5f;
|
||||
|
||||
MockRegistration parent(registrationTestParams(), child);
|
||||
parent.minVisualValue = 5;
|
||||
parent.minGeometryValue = 0.2f;
|
||||
|
||||
EXPECT_EQ(parent.getMinVisualCorrespondences(), 10);
|
||||
EXPECT_FLOAT_EQ(parent.getMinGeometryCorrespondencesRatio(), 0.5f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, CanUseGuessCombinesWithChild)
|
||||
{
|
||||
MockRegistration * child = new MockRegistration();
|
||||
child->canUseGuessFlag = true;
|
||||
|
||||
MockRegistration parent(registrationTestParams(), child);
|
||||
EXPECT_FALSE(parent.canUseGuessFlag);
|
||||
EXPECT_TRUE(parent.canUseGuess());
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, SetChildRegistrationTakesOwnership)
|
||||
{
|
||||
MockRegistration parent(registrationTestParams());
|
||||
MockRegistration * firstChild = new MockRegistration();
|
||||
firstChild->minVisualValue = 3;
|
||||
parent.setChildRegistration(firstChild);
|
||||
|
||||
MockRegistration * secondChild = new MockRegistration();
|
||||
secondChild->minVisualValue = 8;
|
||||
parent.setChildRegistration(secondChild);
|
||||
|
||||
EXPECT_EQ(parent.getMinVisualCorrespondences(), 8);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, ComputeTransformationCallsImpl)
|
||||
{
|
||||
MockRegistration reg(registrationTestParams());
|
||||
reg.resultTransform = Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
const Transform result = reg.computeTransformation(from, to);
|
||||
|
||||
EXPECT_EQ(reg.implCallCount, 1);
|
||||
EXPECT_FALSE(result.isNull());
|
||||
EXPECT_NEAR(result.x(), 1.f, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, ComputeTransformationChainsChild)
|
||||
{
|
||||
MockRegistration * child = new MockRegistration(registrationTestParams());
|
||||
child->resultTransform = Transform(2.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
MockRegistration parent(registrationTestParams(), child);
|
||||
parent.resultTransform = Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
const Transform result = parent.computeTransformation(from, to);
|
||||
|
||||
EXPECT_EQ(parent.implCallCount, 1);
|
||||
EXPECT_EQ(child->implCallCount, 1);
|
||||
EXPECT_FALSE(result.isNull());
|
||||
EXPECT_NEAR(result.x(), 2.f, 1e-6f);
|
||||
EXPECT_EQ(child->guessHistory.size(), 1u);
|
||||
EXPECT_NEAR(child->guessHistory[0].x(), 1.f, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, ComputeTransformationParentFailsUsesGuessForChild)
|
||||
{
|
||||
MockRegistration * child = new MockRegistration(registrationTestParams());
|
||||
child->resultTransform = Transform(2.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
MockRegistration parent(registrationTestParams(), child);
|
||||
parent.failRegistration = true;
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
const Transform guess(0.5f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
const Transform result = parent.computeTransformation(from, to, guess);
|
||||
|
||||
EXPECT_EQ(parent.implCallCount, 1);
|
||||
EXPECT_EQ(child->implCallCount, 1);
|
||||
EXPECT_FALSE(result.isNull());
|
||||
EXPECT_NEAR(result.x(), 2.f, 1e-6f);
|
||||
EXPECT_NEAR(child->guessHistory[0].x(), 0.5f, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, RepeatOnceRefinesWithFirstResultAsGuess)
|
||||
{
|
||||
ParametersMap params = registrationTestParams();
|
||||
params[Parameters::kRegRepeatOnce()] = "true";
|
||||
|
||||
MockRegistration reg(params);
|
||||
EXPECT_TRUE(reg.repeatOnce());
|
||||
reg.canUseGuessFlag = true;
|
||||
reg.resultTransform = Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
Transform nullGuess;
|
||||
EXPECT_TRUE(nullGuess.isNull());
|
||||
const Transform result = reg.computeTransformation(from, to, nullGuess);
|
||||
|
||||
EXPECT_EQ(reg.implCallCount, 2);
|
||||
EXPECT_FALSE(result.isNull());
|
||||
ASSERT_EQ(reg.guessHistory.size(), 2u);
|
||||
EXPECT_TRUE(reg.guessHistory[0].isNull());
|
||||
EXPECT_NEAR(reg.guessHistory[1].x(), 1.f, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, Force3DoFConstrainsGuessAndResult)
|
||||
{
|
||||
ParametersMap params = registrationTestParams();
|
||||
params[Parameters::kRegForce3DoF()] = "true";
|
||||
|
||||
MockRegistration reg(params);
|
||||
EXPECT_TRUE(reg.force3DoF());
|
||||
reg.resultTransform = Transform(1.f, 0.f, 2.f, 0.1f, 0.2f, 0.3f);
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
const Transform guess(0.f, 0.f, 5.f, 0.5f, 0.5f, 0.5f);
|
||||
const Transform result = reg.computeTransformation(from, to, guess);
|
||||
|
||||
ASSERT_EQ(reg.guessHistory.size(), 1u);
|
||||
EXPECT_TRUE(reg.guessHistory[0].is3DoF());
|
||||
EXPECT_NEAR(reg.guessHistory[0].z(), 0.f, 1e-6f);
|
||||
EXPECT_TRUE(result.is3DoF());
|
||||
EXPECT_NEAR(result.z(), 0.f, 1e-6f);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, CovarianceEpsilonAppliedWhenEmpty)
|
||||
{
|
||||
MockRegistration reg(registrationTestParams());
|
||||
reg.resultTransform = Transform::getIdentity();
|
||||
|
||||
const Signature from;
|
||||
const Signature to;
|
||||
RegistrationInfo info;
|
||||
const Transform result = reg.computeTransformation(from, to, Transform::getIdentity(), &info);
|
||||
|
||||
EXPECT_FALSE(result.isNull());
|
||||
ASSERT_FALSE(info.covariance.empty());
|
||||
EXPECT_EQ(info.covariance.rows, 6);
|
||||
EXPECT_EQ(info.covariance.cols, 6);
|
||||
EXPECT_GE(info.covariance.at<double>(0, 0), Registration::COVARIANCE_LINEAR_EPSILON);
|
||||
EXPECT_GE(info.covariance.at<double>(3, 3), Registration::COVARIANCE_ANGULAR_EPSILON);
|
||||
EXPECT_GE(info.totalTime, 0.0);
|
||||
}
|
||||
|
||||
TEST(RegistrationTest, ComputeTransformationFromSensorData)
|
||||
{
|
||||
MockRegistration reg(registrationTestParams());
|
||||
reg.resultTransform = Transform(1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
|
||||
|
||||
const SensorData from;
|
||||
const SensorData to;
|
||||
const Transform result = reg.computeTransformation(from, to);
|
||||
|
||||
EXPECT_EQ(reg.implCallCount, 1);
|
||||
EXPECT_FALSE(result.isNull());
|
||||
EXPECT_NEAR(result.x(), 1.f, 1e-6f);
|
||||
}
|
||||
Reference in New Issue
Block a user