Added Memory and Rtabmap tests

corelib/include/rtabmap/core/Memory.h

@@ -211,12 +211,22 @@ public:
 	/**
 	 * @brief Transfers oldest signatures from WM to LTM to respect memory and/or time limits.
 	 *
-	 * The number of signatures removed depends on the visual word dictionary growth
+	 * Two regimes are used depending on the visual word dictionary state:
-	 * since the previous iteration (including retrieved signatures). We remove signatures
+	 * - **Word-count regime**: active only when mapping mode is on, the @ref VWDictionary
-	 * until the number of visual words transferred is greater than the number of retrieved
+	 *   is in incremental mode, contains at least one word, and is *not* using incremental
-	 * ones on the last iteration. In the case that signatures don't have visual words (e.g., lidar-only mapping),
+	 *   FLANN. In this regime, signatures are transferred until the number of visual words
-	 * we remove at least one more signature than the total of signatures added/retrieved in the
+	 *   removed from the dictionary catches up with the number of new words indexed since
-	 * previous iteration.
+	 *   the previous iteration.
+	 * - **Signature-count regime**: used in every other case (localization mode, dictionary
+	 *   not incremental, dictionary still empty -- e.g. lidar-only mapping or no feature
+	 *   extraction -- or incremental FLANN, where the word count is no longer the bottleneck).
+	 *   In this regime, at least one more signature than the count added/retrieved in the
+	 *   previous iteration is transferred, regardless of words.
+	 *
+	 * In both regimes, candidate selection (see @c getRemovableSignatures()) honors
+	 * @p ignoredIds, skips intermediate nodes, and excludes WM nodes linked to STM (to
+	 * preserve rehearsal). Intermediate (weight==-1) nodes linked to a transferred
+	 * signature are dragged out with it.
 	 *
 	 * @param ignoredIds Signatures that must not be transferred (e.g. STM, retrieved ids, on the planned path).
 	 * @return Ids of signatures moved to LTM, in transfer order.

corelib/src/Memory.cpp

@@ -2588,7 +2588,9 @@ public:
 		}
 		return false;
 	}
-	int weight, age, id;
+	int weight;
+	double age;
+	int id;
 };
 std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
 {

corelib/src/Rtabmap.cpp

@@ -7004,6 +7004,33 @@ bool Rtabmap::computePath(int targetNode, bool global)
 			{
 				if(iter->first > 0)
 				{
+					// Skip intermediate nodes (weight==-1). They are not navigable
+					// waypoints and updateGoalIndex would otherwise abort the
+					// plan when it sees them. The poses of the remaining real
+					// nodes already account for cumulative transform through any
+					// intermediate chain (relative poses from graph::computePath).
+					int weight = 0;
+					const Signature * s = _memory->getSignature(iter->first);
+					if(s)
+					{
+						weight = s->getWeight();
+					}
+					else
+					{
+						// For nodes in LTM, fetch weight from the database.
+						Transform p, gt;
+						int mapId = 0;
+						std::string label;
+						double stamp = 0.0;
+						std::vector<float> vel;
+						GPS gps;
+						EnvSensors envs;
+						_memory->getNodeInfo(iter->first, p, mapId, weight, label, stamp, gt, vel, gps, envs, true);
+					}
+					if(weight == -1)
+					{
+						continue;
+					}
 					// just keep nodes in the path
 					_path[oi].first = iter->first;
 					_path[oi++].second = t * iter->second;
@@ -7277,18 +7304,14 @@ void Rtabmap::updateGoalIndex()
 	if( _memory && _path.size())
 	{
 		// remove all previous virtual links
-		bool hasIntermediateNodes = false;
 		for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
 		{
 			const Signature * s = _memory->getSignature(_path[i].first);
 			if(s)
 			{
+				UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
 				_memory->removeVirtualLinks(s->id());
 			}
-			if(s->getWeight() == -1)
-			{
-				hasIntermediateNodes = true;
-			}
 		}
 
 		// for the current index, only keep the newest virtual link
@@ -7314,51 +7337,42 @@ void Rtabmap::updateGoalIndex()
 			}
 		}
 
-		// Make sure the next signatures on the path are linked together
+		// Make sure the next signatures on the path are linked together.
+		// Intermediate nodes have been filtered out of _path by computePath, so
+		// every entry is a real node here.
 		float distanceSoFar = 0.0f;
-		for(unsigned int i=_pathCurrentIndex+1;
+		for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
-			i<_path.size() && !hasIntermediateNodes;
-			++i)
 		{
-			if(i>0)
+			if(_localRadius > 0.0f)
 			{
-				if(_localRadius > 0.0f)
+				distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-				{
+			}
-					distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
-				}
 
-				if(_path[i].first != _path[i-1].first)
+			if(_path[i].first != _path[i-1].first)
+			{
+				const Signature * s = _memory->getSignature(_path[i].first);
+				if(s)
 				{
-					const Signature * s = _memory->getSignature(_path[i].first);
+					UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
-					if(s)
+					const Signature * sPrev = _memory->getSignature(_path[i-1].first);
+					if(sPrev)
 					{
-						if(s->getWeight() == -1)
+						UASSERT_MSG(sPrev->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i-1, _path[i-1].first).c_str());
-						{
+					}
-							hasIntermediateNodes = true;
+					if(!s->hasLink(_path[i-1].first) && sPrev != 0)
-							break;
+					{
-						}
+						Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-						if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
+						_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
-						{
+						UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
-							Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
-							_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
-							UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
-						}
 					}
 				}
-
-				if(distanceSoFar > _localRadius)
-				{
-					UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
-					break;
-				}
 			}
-		}
 
-		if(hasIntermediateNodes)
+			if(distanceSoFar > _localRadius)
-		{
+			{
-			UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!");
+				UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
-			this->clearPath(-1);
+				break;
-			return;
+			}
 		}
 
 		UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);

corelib/src/util2d.cpp

@@ -2404,7 +2404,7 @@ bool rotateImagesUpsideUpIfNecessary(
 	Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
 	localTransform.getEulerAngles(roll, pitch, yaw);
 	UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
-	if(fabs(pitch > M_PI/4))
+	if(fabs(pitch) > M_PI/4)
 	{
 		// Return original because of ambiguity for what would be considered up...
 		UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
@@ -2416,29 +2416,49 @@ bool rotateImagesUpsideUpIfNecessary(
 	}
 	if(roll >= M_PI/4 && roll < 3*M_PI/4)
 	{
-		UDEBUG("ROTATION_90 (roll=%f)", roll);
+		// Body roll near +pi/2 (right side down): the world-up direction projects to
+		// the image's left, so rotate the image 90 degrees clockwise to bring it
+		// upright (transpose + horizontal flip). Image dimensions HxW become WxH.
+		//
+		// Marker X moves from top-left to top-right quadrant:
+		//   before (3x6):              after (6x3):
+		//   . X . . . .                . . .
+		//   . . . . . .                . . X
+		//   . . . . . .                . . .
+		//                              . . .
+		//                              . . .
+		//                              . . .
+		UDEBUG("Rotating image 90 deg clockwise to correct body roll (roll=%f)", roll);
 		if(!rgb.empty())
 		{
-			cv::flip(rgb,rgb,1);
 			cv::transpose(rgb,rgb);
+			cv::flip(rgb,rgb,1);
 		}
 		if(!depth.empty())
 		{
-			cv::flip(depth,depth,1);
 			cv::transpose(depth,depth);
+			cv::flip(depth,depth,1);
 		}
 		cv::Size sizet(model.imageHeight(), model.imageWidth());
 		model = CameraModel(
 				model.fy(),
 				model.fx(),
-				model.cy(),
+				model.cy()>0?model.imageHeight()-model.cy():0,
-				model.cx()>0?model.imageWidth()-model.cx():0,
+				model.cx(),
-				model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+				model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 		model.setImageSize(sizet);
 	}
 	else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
 	{
-		UDEBUG("ROTATION_180 (roll=%f)", roll);
+		// Body roll near pi (upside down): rotate the image 180 degrees (horizontal
+		// flip + vertical flip). Image dimensions unchanged.
+		//
+		// Marker X moves from top-left to bottom-right quadrant:
+		//   before (3x6):              after (3x6):
+		//   . X . . . .                . . . . . .
+		//   . . . . . .                . . . . . .
+		//   . . . . . .                . . . . X .
+		UDEBUG("Rotating image 180 deg to correct body roll (roll=%f)", roll);
 		if(!rgb.empty())
 		{
 			cv::flip(rgb,rgb,1);
@@ -2460,29 +2480,42 @@ bool rotateImagesUpsideUpIfNecessary(
 	}
 	else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
 	{
-		UDEBUG("ROTATION_270 (roll=%f)", roll);
+		// Body roll near -pi/2 / +3*pi/2 (left side down): the world-up direction
+		// projects to the image's right, so rotate the image 90 degrees counter-
+		// clockwise to bring it upright (horizontal flip + transpose). Image
+		// dimensions HxW become WxH.
+		//
+		// Marker X moves from top-left to bottom-left quadrant:
+		//   before (3x6):              after (6x3):
+		//   . X . . . .                . . .
+		//   . . . . . .                . . .
+		//   . . . . . .                . . .
+		//                              . . .
+		//                              X . .
+		//                              . . .
+		UDEBUG("Rotating image 90 deg counter-clockwise to correct body roll (roll=%f)", roll);
 		if(!rgb.empty())
 		{
-			cv::transpose(rgb,rgb);
 			cv::flip(rgb,rgb,1);
+			cv::transpose(rgb,rgb);
 		}
 		if(!depth.empty())
 		{
-			cv::transpose(depth,depth);
 			cv::flip(depth,depth,1);
+			cv::transpose(depth,depth);
 		}
 		cv::Size sizet(model.imageHeight(), model.imageWidth());
 		model = CameraModel(
 				model.fy(),
 				model.fx(),
-				model.cy()>0?model.imageHeight()-model.cy():0,
+				model.cy(),
-				model.cx(),
+				model.cx()>0?model.imageWidth()-model.cx():0,
-				model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+				model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 		model.setImageSize(sizet);
 	}
 	else
 	{
-		UDEBUG("ROTATION_0 (roll=%f)", roll);
+		UDEBUG("Not rotating image, body roll within +/- pi/4 of upright (roll=%f)", roll);
 		return false;
 	}
 	return true;

corelib/test/CMakeLists.txt

@@ -96,6 +96,16 @@ add_executable(test_bayesfilter test_bayesfilter.cpp)
 target_link_libraries(test_bayesfilter gtest_main rtabmap_core)
 add_test(NAME test_bayesfilter COMMAND test_bayesfilter)
 
+#Memory.h
+add_executable(test_memory test_memory.cpp)
+target_link_libraries(test_memory gtest_main rtabmap_core)
+add_test(NAME test_memory COMMAND test_memory)
+
+#Rtabmap.h
+add_executable(test_rtabmap test_rtabmap.cpp)
+target_link_libraries(test_rtabmap gtest_main rtabmap_core)
+add_test(NAME test_rtabmap COMMAND test_rtabmap)
+
 #Link.h
 add_executable(test_link test_link.cpp)
 target_link_libraries(test_link gtest_main rtabmap_core)

corelib/test/test_util2d.cpp

@@ -1259,10 +1259,42 @@ cv::Mat createTestImage(int width, int height, uchar value = 100) {
     return cv::Mat(height, width, CV_8UC3, cv::Scalar(value, value, value));
 }
 
+namespace {
+// A 3-channel "marker" pixel that's distinguishable from the uniform base color.
+const cv::Vec3b kRgbMarker(255, 0, 0);
+const cv::Vec3b kDepthMarker(123, 45, 67);
+// Marker pixel position in the input 640x480 (cols x rows) image (top-left quadrant).
+const int kMarkerRow = 100;
+const int kMarkerCol = 200;
+
+void stampMarker(cv::Mat & rgb, cv::Mat & depth) {
+    rgb.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kRgbMarker;
+    depth.at<cv::Vec3b>(kMarkerRow, kMarkerCol) = kDepthMarker;
+}
+
+// Verifies the marker landed at (expectedRow, expectedCol) and that no other pixel
+// in the rotated image carries the marker color (i.e. the rotation is direction-
+// correct, not just dimension-correct).
+void expectMarkerAt(const cv::Mat & img, const cv::Vec3b & marker, int expectedRow, int expectedCol) {
+    EXPECT_EQ(img.at<cv::Vec3b>(expectedRow, expectedCol), marker);
+    int strays = 0;
+    for(int r = 0; r < img.rows; ++r)
+    {
+        for(int c = 0; c < img.cols; ++c)
+        {
+            if(r == expectedRow && c == expectedCol) continue;
+            if(img.at<cv::Vec3b>(r, c) == marker) ++strays;
+        }
+    }
+    EXPECT_EQ(strays, 0);
+}
+} // namespace
+
 TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
     CameraModel model(500, 500, 320, 240, CameraModel::opticalRotation(), 0, cv::Size(640, 480));
     cv::Mat rgb = createTestImage(640, 480);
     cv::Mat depth = createTestImage(640, 480);
+    stampMarker(rgb, depth);
 
     bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
 
@@ -1271,6 +1303,9 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
     EXPECT_EQ(rgb.rows, 480);
     EXPECT_EQ(depth.cols, 640);
     EXPECT_EQ(depth.rows, 480);
+    // Marker stays in place when no rotation is applied.
+    expectMarkerAt(rgb,   kRgbMarker,   kMarkerRow, kMarkerCol);
+    expectMarkerAt(depth, kDepthMarker, kMarkerRow, kMarkerCol);
     float roll,pitch,yaw;
     (model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
     EXPECT_EQ(roll, 0.0f);
@@ -1279,43 +1314,54 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryNoRotation) {
 }
 
 TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation90Degrees) {
-    // Simulate 90° roll
+    // Simulate +pi/2 roll (camera tilted right) -> upright correction is a 90 CW
+    // rotation of the image (transpose then flip(axis=1)). For an input marker at
+    // (row=100, col=200) in a 480x640 image:
+    //   transpose: (100, 200) -> (200, 100) in 640x480
+    //   flip(1):   (200, 100) -> (200, 480-1-100) = (200, 379)
     Transform rot = Transform(0,0,0, M_PI / 2, 0, 0);
     CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
     cv::Mat rgb = createTestImage(640, 480, 150);
     cv::Mat depth = createTestImage(640, 480, 200);
+    stampMarker(rgb, depth);
 
     bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
 
     EXPECT_TRUE(rotated);
     EXPECT_EQ(rgb.cols, 480);  // Transposed
     EXPECT_EQ(rgb.rows, 640);
-    EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 150);  // Same pixel values
+    EXPECT_EQ(depth.cols, 480);
-    EXPECT_EQ(depth.cols, 480);  // Transposed
     EXPECT_EQ(depth.rows, 640);
-    EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 200);
+    expectMarkerAt(rgb,   kRgbMarker,   /*row=*/200, /*col=*/379);
+    expectMarkerAt(depth, kDepthMarker, /*row=*/200, /*col=*/379);
+    // After correction the camera is upright (roll=0).
     float roll,pitch,yaw;
     (model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
-    EXPECT_NEAR(roll, M_PI, 1e-5);
+    EXPECT_NEAR(roll, 0.0f, 1e-5);
     EXPECT_NEAR(pitch, 0.0f, 1e-5);
     EXPECT_NEAR(yaw, 0.0f, 1e-5);
 }
 
 TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation180Degrees) {
+    // Simulate pi roll (camera upside down) -> 180 rotation:
+    //   flip(1) + flip(0). For (100, 200) in 480x640:
+    //   flip(1): (100, 200) -> (100, 640-1-200) = (100, 439)
+    //   flip(0): (100, 439) -> (480-1-100, 439) = (379, 439)
     Transform rot = Transform(0,0,0, M_PI, 0, 0);
     CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
     cv::Mat rgb = createTestImage(640, 480, 123);
     cv::Mat depth = createTestImage(640, 480, 77);
+    stampMarker(rgb, depth);
 
     bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
 
     EXPECT_TRUE(rotated);
     EXPECT_EQ(rgb.cols, 640);  // Same size
     EXPECT_EQ(rgb.rows, 480);
-    EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 123);
     EXPECT_EQ(depth.cols, 640);  // Same size
     EXPECT_EQ(depth.rows, 480);
-    EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 77);
+    expectMarkerAt(rgb,   kRgbMarker,   /*row=*/379, /*col=*/439);
+    expectMarkerAt(depth, kDepthMarker, /*row=*/379, /*col=*/439);
     float roll,pitch,yaw;
     (model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
     EXPECT_NEAR(roll, 0.0f, 1e-5);
@@ -1324,23 +1370,29 @@ TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation180Degrees) {
 }
 
 TEST(Util2dTest, RotateImagesUpsideUpIfNecessaryRotation270Degrees) {
+    // Simulate 3*pi/2 roll (camera tilted left) -> upright correction is a 90 CCW
+    // rotation of the image (flip(axis=1) then transpose). For (100, 200) in 480x640:
+    //   flip(1):   (100, 200) -> (100, 640-1-200) = (100, 439)
+    //   transpose: (100, 439) -> (439, 100) in 640x480
     Transform rot = Transform(0,0,0, 3*M_PI/2, 0, 0);
     CameraModel model(500, 500, 320, 240, rot*CameraModel::opticalRotation(), 0, cv::Size(640, 480));
     cv::Mat rgb = createTestImage(640, 480, 90);
     cv::Mat depth = createTestImage(640, 480, 60);
+    stampMarker(rgb, depth);
 
     bool rotated = util2d::rotateImagesUpsideUpIfNecessary(model, rgb, depth);
 
     EXPECT_TRUE(rotated);
     EXPECT_EQ(rgb.cols, 480);
     EXPECT_EQ(rgb.rows, 640);
-    EXPECT_EQ(rgb.at<cv::Vec3b>(0, 0)[0], 90);
     EXPECT_EQ(depth.cols, 480);
     EXPECT_EQ(depth.rows, 640);
-    EXPECT_EQ(depth.at<cv::Vec3b>(0, 0)[0], 60);
+    expectMarkerAt(rgb,   kRgbMarker,   /*row=*/439, /*col=*/100);
+    expectMarkerAt(depth, kDepthMarker, /*row=*/439, /*col=*/100);
+    // After correction the camera is upright (roll=0).
     float roll,pitch,yaw;
     (model.localTransform() * CameraModel::opticalRotation().inverse()).getEulerAngles(roll, pitch, yaw);
-    EXPECT_NEAR(roll, M_PI, 1e-5);
+    EXPECT_NEAR(roll, 0.0f, 1e-5);
     EXPECT_NEAR(pitch, 0.0f, 1e-5);
     EXPECT_NEAR(yaw, 0.0f, 1e-5);
 }