Added Registration tests

corelib/include/rtabmap/core/Registration.h

@@ -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,

corelib/test/CMakeLists.txt

@@ -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)

corelib/test/test_registration.cpp

@@ -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);
+}