merged master->ros2

src/CoreWrapper.cpp

@@ -468,18 +468,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 
 	int estimationType = Parameters::defaultVisEstimationType();
 	Parameters::parse(parameters_, Parameters::kVisEstimationType(), estimationType);
-	int cameras = this->rgbdCameras();
-	bool subscribeRGBD = this->isSubscribedToRGBD();
-	if(subscribeRGBD && cameras> 1 && estimationType>0)
-	{
-		RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (%d is not supported "
-				"for multi-cameras) as \"subscribe_rgbd\" is "
-				"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
-				Parameters::kVisEstimationType().c_str(),
-				estimationType,
-				Parameters::kVisEstimationType().c_str());
-		uInsert(parameters_, ParametersPair(Parameters::kVisEstimationType(), "0"));
-	}
 
 	// modify default parameters with those in the database
 	if(!deleteDbOnStart)
@@ -1002,6 +990,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
 		lastPoseIntermediate_ = false;
 		lastPose_ = odom;
 		lastPoseStamp_ = stamp;
+		lastPoseVelocity_.resize(6);
+		lastPoseVelocity_[0] = odomMsg.twist.twist.linear.x;
+		lastPoseVelocity_[1] = odomMsg.twist.twist.linear.y;
+		lastPoseVelocity_[2] = odomMsg.twist.twist.linear.z;
+		lastPoseVelocity_[3] = odomMsg.twist.twist.angular.x;
+		lastPoseVelocity_[4] = odomMsg.twist.twist.angular.y;
+		lastPoseVelocity_[5] = odomMsg.twist.twist.angular.z;
 
 		// Only update variance if odom is not null
 		if(!odom.isNull())
@@ -1087,6 +1082,7 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp)
 		lastPoseIntermediate_ = false;
 		lastPose_ = odom;
 		lastPoseStamp_ = stamp;
+		lastPoseVelocity_.clear();
 
 		bool ignoreFrame = false;
 		if(stamp.seconds() == 0.0)
@@ -1122,12 +1118,13 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp)
 	return false;
 }
 
-void CoreWrapper::commonDepthCallback(
+void CoreWrapper::commonMultiCameraCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 		const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+		const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
 		const sensor_msgs::msg::LaserScan & scan2dMsg,
 		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -1178,11 +1175,12 @@ void CoreWrapper::commonDepthCallback(
 		return;
 	}
 
-	commonDepthCallbackImpl(odomFrameId,
+	commonMultiCameraCallbackImpl(odomFrameId,
 			userDataMsg,
 			imageMsgs,
 			depthMsgs,
 			cameraInfoMsgs,
+			depthCameraInfoMsgs,
 			scan2dMsg,
 			scan3dMsg,
 			odomInfoMsg,
@@ -1192,12 +1190,13 @@ void CoreWrapper::commonDepthCallback(
 			localDescriptors);
 }
 
-void CoreWrapper::commonDepthCallbackImpl(
+void CoreWrapper::commonMultiCameraCallbackImpl(
 		const std::string & odomFrameId,
 		const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
 		const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
 		const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
 		const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
+		const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
 		const sensor_msgs::msg::LaserScan & scan2dMsg,
 		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
 		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -1210,21 +1209,26 @@ void CoreWrapper::commonDepthCallbackImpl(
 	cv::Mat rgb;
 	cv::Mat depth;
 	std::vector<rtabmap::CameraModel> cameraModels;
+	std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
 	std::vector<cv::KeyPoint> keypoints;
 	std::vector<cv::Point3f> points;
 	cv::Mat descriptors;
+
 	if(!rtabmap_ros::convertRGBDMsgs(
 			imageMsgs,
 			depthMsgs,
 			cameraInfoMsgs,
+			depthCameraInfoMsgs,
 			frameId_,
 			odomSensorSync_?odomFrameId:"",
 			lastPoseStamp_,
 			rgb,
 			depth,
 			cameraModels,
+			stereoCameraModels,
 			*tfBuffer_,
 			waitForTransform_,
+			alreadyRectifiedImages_,
 			localKeyPointsMsgs,
 			localPoints3dMsgs,
 			localDescriptorsMsgs,
@@ -1235,6 +1239,73 @@ void CoreWrapper::commonDepthCallbackImpl(
 		RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmap update...");
 		return;
 	}
+	UDEBUG("cameraModels=%ld stereoCameraModels=%ld", cameraModels.size(), stereoCameraModels.size());
+	UDEBUG("rgb=%dx%d(type=%d), depth/right=%dx%d(type=%d)", rgb.rows, rgb.cols, rgb.type(), depth.rows, depth.cols, depth.type());
+
+	if(stereoCameraModels.size() && stereoToDepth_)
+	{
+		UASSERT(depth.type() == CV_8UC1);
+		cv::Mat leftMono;
+		if(rgb.channels() == 3)
+		{
+			cv::cvtColor(rgb, leftMono, CV_BGR2GRAY);
+		}
+		else
+		{
+			leftMono = rgb;
+		}
+		cv::Mat rightMono = depth;
+		depth = cv::Mat();
+
+		UASSERT(int((leftMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == leftMono.cols);
+		UASSERT(int((rightMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == rightMono.cols);
+		int subImageWidth = leftMono.cols/stereoCameraModels.size();
+		for(size_t i=0; i<stereoCameraModels.size(); ++i)
+		{
+			cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
+			cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
+
+			// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
+			cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
+					left,
+					right,
+					parameters_);
+			if(disparity.empty())
+			{
+				RCLCPP_ERROR(this->get_logger(), "Could not compute disparity image (\"stereo_to_depth\" is true)!");
+				return;
+			}
+			cv::Mat subDepth = rtabmap::util2d::depthFromDisparity(
+							disparity,
+							stereoCameraModels[i].left().fx(),
+							stereoCameraModels[i].baseline());
+
+			if(subDepth.empty())
+			{
+				RCLCPP_ERROR(this->get_logger(), "Could not compute depth image (\"stereo_to_depth\" is true)!");
+				return;
+			}
+			UASSERT(subDepth.type() == CV_16UC1 || subDepth.type() == CV_32FC1);
+
+			if(depth.empty())
+			{
+				depth = cv::Mat(subDepth.rows, subDepth.cols*stereoCameraModels.size(), subDepth.type());
+			}
+
+			if(subDepth.type() == depth.type())
+			{
+				subDepth.copyTo(cv::Mat(depth, cv::Rect(i*subDepth.cols, 0, subDepth.cols, subDepth.rows)));
+			}
+			else
+			{
+				RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type!");
+				return;
+			}
+
+			cameraModels.push_back(stereoCameraModels[i].left());
+		}
+		stereoCameraModels.clear();
+	}
 
 	UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
 	if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
@@ -1253,7 +1324,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 
 	LaserScan scan;
 	bool genMaxScanPts = 0;
-	if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && genScan_)
+	if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && stereoCameraModels.empty() && genScan_)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
 		*scanCloud2d = util3d::laserScanFromDepthImages(
@@ -1364,14 +1435,29 @@ void CoreWrapper::commonDepthCallbackImpl(
 		userData_ = cv::Mat();
 	}
 
-	SensorData data(
-			scan,
-			rgb,
-			depth,
-			cameraModels,
-			lastPoseIntermediate_?-1:0,
-			rtabmap_ros::timestampFromROS(lastPoseStamp_),
-			userData);
+	SensorData data;
+	if(!stereoCameraModels.empty())
+	{
+		data = SensorData(
+				scan,
+				rgb,
+				depth,
+				stereoCameraModels,
+				lastPoseIntermediate_?-1:0,
+				rtabmap_ros::timestampFromROS(lastPoseStamp_),
+				userData);
+	}
+	else
+	{
+		data = SensorData(
+				scan,
+				rgb,
+				depth,
+				cameraModels,
+				lastPoseIntermediate_?-1:0,
+				rtabmap_ros::timestampFromROS(lastPoseStamp_),
+				userData);
+	}
 
 	OdometryInfo odomInfo;
 	if(odomInfoMsg.get())
@@ -1394,6 +1480,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
+			lastPoseVelocity_,
 			odomFrameId,
 			covariance_,
 			odomInfo,
@@ -1401,255 +1488,6 @@ void CoreWrapper::commonDepthCallbackImpl(
 	covariance_ = cv::Mat();
 }
 
-void CoreWrapper::commonStereoCallback(
-		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
-		const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
-		const cv_bridge::CvImageConstPtr& leftImageMsg,
-		const cv_bridge::CvImageConstPtr& rightImageMsg,
-		const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
-		const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
-		const sensor_msgs::msg::LaserScan & scan2dMsg,
-		const sensor_msgs::msg::PointCloud2 & scan3dMsg,
-		const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
-		const std::vector<rtabmap_ros::msg::GlobalDescriptor> & globalDescriptorMsgs,
-		const std::vector<rtabmap_ros::msg::KeyPoint> & localKeyPointsMsg,
-		const std::vector<rtabmap_ros::msg::Point3f> & localPoints3dMsg,
-		const cv::Mat & localDescriptorsMsg)
-{
-	UTimer timerConversion;
-	std::string odomFrameId = odomFrameId_;
-	if(odomMsg.get())
-	{
-		odomFrameId = odomMsg->header.frame_id;
-		if(!scan2dMsg.ranges.empty())
-		{
-			if(!odomUpdate(*odomMsg, scan2dMsg.header.stamp))
-			{
-				return;
-			}
-		}
-		else if(!scan3dMsg.data.empty())
-		{
-			if(!odomUpdate(*odomMsg, scan3dMsg.header.stamp))
-			{
-				return;
-			}
-		}
-		else if(leftImageMsg.get() == 0 || !odomUpdate(*odomMsg, leftImageMsg->header.stamp))
-		{
-			return;
-		}
-	}
-	else if(!scan2dMsg.ranges.empty())
-	{
-		if(!odomTFUpdate(scan2dMsg.header.stamp))
-		{
-			return;
-		}
-	}
-	else if(!scan3dMsg.data.empty())
-	{
-		if(!odomTFUpdate(scan3dMsg.header.stamp))
-		{
-			return;
-		}
-	}
-	else if(leftImageMsg.get() == 0 || !odomTFUpdate(leftImageMsg->header.stamp))
-	{
-		return;
-	}
-
-	cv::Mat left;
-	cv::Mat right;
-	StereoCameraModel stereoModel;
-	if(!rtabmap_ros::convertStereoMsg(
-			leftImageMsg,
-			rightImageMsg,
-			leftCamInfoMsg,
-			rightCamInfoMsg,
-			frameId_,
-			odomSensorSync_?odomFrameId:"",
-			lastPoseStamp_,
-			left,
-			right,
-			stereoModel,
-			*tfBuffer_,
-			waitForTransform_,
-			alreadyRectifiedImages_))
-	{
-		RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap update...");
-		return;
-	}
-
-	if(stereoToDepth_)
-	{
-		// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
-		cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
-				left,
-				right,
-				parameters_);
-		if(disparity.empty())
-		{
-			RCLCPP_ERROR(this->get_logger(), "Could not compute disparity image (\"stereo_to_depth\" is true)!");
-			return;
-		}
-		cv::Mat depth = rtabmap::util2d::depthFromDisparity(
-						disparity,
-						stereoModel.left().fx(),
-						stereoModel.baseline());
-
-		if(depth.empty())
-		{
-			RCLCPP_ERROR(this->get_logger(), "Could not compute depth image (\"stereo_to_depth\" is true)!");
-			return;
-		}
-		UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
-
-		// move to common depth callback
-		cv_bridge::CvImagePtr imgDepth(new cv_bridge::CvImage);
-		if(depth.type() == CV_16UC1)
-		{
-			imgDepth->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
-		}
-		else // CV_32FC1
-		{
-			imgDepth->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-		}
-		imgDepth->image = depth;
-		imgDepth->header = leftImageMsg->header;
-		std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
-		std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
-		std::vector<sensor_msgs::msg::CameraInfo> cameraInfos(1);
-		rgbImages[0] = leftImageMsg;
-		depthImages[0] = imgDepth;
-		cameraInfos[0] = leftCamInfoMsg;
-
-		std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPointsMsgs;
-		std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3dMsgs;
-		std::vector<cv::Mat> localDescriptorsMsgs;
-		if(!localKeyPointsMsg.empty())
-		{
-			localKeyPointsMsgs.push_back(localKeyPointsMsg);
-		}
-		if(!localPoints3dMsg.empty())
-		{
-			localPoints3dMsgs.push_back(localPoints3dMsg);
-		}
-		if(!localDescriptorsMsg.empty())
-		{
-			localDescriptorsMsgs.push_back(localDescriptorsMsg);
-		}
-		commonDepthCallbackImpl(odomFrameId,
-				rtabmap_ros::msg::UserData::SharedPtr(),
-				rgbImages, depthImages, cameraInfos,
-				scan2dMsg, scan3dMsg,
-				odomInfoMsg,
-				globalDescriptorMsgs, localKeyPointsMsgs, localPoints3dMsgs, localDescriptorsMsgs);
-		return;
-	}
-
-	LaserScan scan;
-	if(!scan2dMsg.ranges.empty())
-	{
-		if(!rtabmap_ros::convertScanMsg(
-				scan2dMsg,
-				frameId_,
-				odomSensorSync_?odomFrameId:"",
-				lastPoseStamp_,
-				scan,
-				*tfBuffer_,
-				waitForTransform_,
-				// backward compatibility, project 2D scan in /base_link frame
-				rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
-		{
-			RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update...");
-			return;
-		}
-	}
-	else if(!scan3dMsg.data.empty())
-	{
-		if(!rtabmap_ros::convertScan3dMsg(
-				scan3dMsg,
-				frameId_,
-				odomSensorSync_?odomFrameId:"",
-				lastPoseStamp_,
-				scan,
-				*tfBuffer_,
-				waitForTransform_,
-				scanCloudMaxPoints_))
-		{
-			RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
-			return;
-		}
-	}
-
-	cv::Mat userData;
-	if(userDataMsg.get())
-	{
-		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
-		UScopeMutex lock(userDataMutex_);
-		if(!userData_.empty())
-		{
-			RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
-			userData_ = cv::Mat();
-		}
-	}
-	else
-	{
-		UScopeMutex lock(userDataMutex_);
-		userData = userData_;
-		userData_ = cv::Mat();
-	}
-
-	SensorData data(
-			scan,
-			left,
-			right,
-			stereoModel,
-			lastPoseIntermediate_?-1:0,
-			rtabmap_ros::timestampFromROS(lastPoseStamp_),
-			userData);
-
-	OdometryInfo odomInfo;
-	if(odomInfoMsg.get())
-	{
-		odomInfo = odomInfoFromROS(*odomInfoMsg);
-	}
-
-	if(!globalDescriptorMsgs.empty())
-	{
-		data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
-	}
-
-	std::vector<cv::KeyPoint> keypoints;
-	std::vector<cv::Point3f> points;
-	if(!localKeyPointsMsg.empty())
-	{
-		keypoints = rtabmap_ros::keypointsFromROS(localKeyPointsMsg);
-	}
-	if(!localPoints3dMsg.empty())
-	{
-		// Points should be in base frame
-		points = rtabmap_ros::points3fFromROS(localPoints3dMsg, stereoModel.localTransform());
-	}
-	if(!keypoints.empty())
-	{
-		UASSERT(points.empty() || points.size() == keypoints.size());
-		UASSERT(localDescriptorsMsg.empty() || localDescriptorsMsg.rows == (int)keypoints.size());
-		data.setFeatures(keypoints, points, localDescriptorsMsg);
-	}
-
-	process(lastPoseStamp_,
-			data,
-			lastPose_,
-			odomFrameId,
-			covariance_,
-			odomInfo,
-			timerConversion.ticks());
-
-	covariance_ = cv::Mat();
-}
-
 void CoreWrapper::commonLaserScanCallback(
 		const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
 		const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -1758,7 +1596,7 @@ void CoreWrapper::commonLaserScanCallback(
 			scan,
 			cv::Mat(),
 			cv::Mat(),
-			CameraModel(),
+			rtabmap::CameraModel(),
 			lastPoseIntermediate_?-1:0,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
@@ -1777,6 +1615,7 @@ void CoreWrapper::commonLaserScanCallback(
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
+			lastPoseVelocity_,
 			odomFrameId,
 			covariance_,
 			odomInfo,
@@ -1821,7 +1660,7 @@ void CoreWrapper::commonOdomCallback(
 	SensorData data(
 			cv::Mat(),
 			cv::Mat(),
-			CameraModel(),
+			rtabmap::CameraModel(),
 			lastPoseIntermediate_?-1:0,
 			rtabmap_ros::timestampFromROS(lastPoseStamp_),
 			userData);
@@ -1835,6 +1674,7 @@ void CoreWrapper::commonOdomCallback(
 	process(lastPoseStamp_,
 			data,
 			lastPose_,
+			lastPoseVelocity_,
 			odomFrameId,
 			covariance_,
 			odomInfo,
@@ -1847,6 +1687,7 @@ void CoreWrapper::process(
 		const rclcpp::Time & stamp,
 		SensorData & data,
 		const Transform & odom,
+		const std::vector<float> & odomVelocityIn,
 		const std::string & odomFrameId,
 		const cv::Mat & odomCovariance,
 		const OdometryInfo & odomInfo,
@@ -1904,7 +1745,7 @@ void CoreWrapper::process(
 						covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
 					}
 
-					SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
+					SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
 					Transform gt;
 					if(!groundTruthFrameId_.empty())
 					{
@@ -1932,6 +1773,16 @@ void CoreWrapper::process(
 							odomVelocity[5] = yaw/info.interval;
 						}
 					}
+					if(odomVelocity.empty())
+					{
+						odomVelocity.resize(6);
+						odomVelocity[0] = iter->first.twist.twist.linear.x;
+						odomVelocity[1] = iter->first.twist.twist.linear.y;
+						odomVelocity[2] = iter->first.twist.twist.linear.z;
+						odomVelocity[3] = iter->first.twist.twist.angular.x;
+						odomVelocity[4] = iter->first.twist.twist.angular.y;
+						odomVelocity[5] = iter->first.twist.twist.angular.z;
+					}
 
 					rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
 				}
@@ -2099,6 +1950,10 @@ void CoreWrapper::process(
 				odomVelocity[5] = yaw/odomInfo.interval;
 			}
 		}
+		if(odomVelocity.empty())
+		{
+			odomVelocity = odomVelocityIn;
+		}
 		if(rtabmapROSStats_.size())
 		{
 			externalStats.insert(rtabmapROSStats_.begin(), rtabmapROSStats_.end());
@@ -2806,6 +2661,7 @@ void CoreWrapper::resetRtabmapCallback(
 	rtabmap_.resetMemory();
 	covariance_ = cv::Mat();
 	lastPose_.setIdentity();
+	lastPoseVelocity_.clear();
 	lastPoseIntermediate_ = false;
 	currentMetricGoal_.setNull();
 	lastPublishedMetricGoal_.setNull();
@@ -2899,6 +2755,7 @@ void CoreWrapper::loadDatabaseCallback(
 
 	covariance_ = cv::Mat();
 	lastPose_.setIdentity();
+	lastPoseVelocity_.clear();
 	lastPoseIntermediate_ = false;
 	currentMetricGoal_.setNull();
 	lastPublishedMetricGoal_.setNull();
@@ -3025,6 +2882,7 @@ void CoreWrapper::backupDatabaseCallback(
 
 	covariance_ = cv::Mat();
 	lastPose_.setIdentity();
+	lastPoseVelocity_.clear();
 	currentMetricGoal_.setNull();
 	lastPublishedMetricGoal_.setNull();
 	goalFrameId_.clear();
@@ -4112,8 +3970,8 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 			int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
 			if(id>0)
 			{
-				std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true);
-				std::map<int, int> missingIds;
+				std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, true, false, true);
+				std::multimap<int, int> missingIds;
 				for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 				{
 					if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
@@ -4140,7 +3998,7 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
 
 				int loaded = 0;
 				std::stringstream stream;
-				for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
+				for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
 				{
 					signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
 					nodesToRepublish_.erase(iter->second);