CameraRealSense2: fixed wrong IR image index. DBReader: fixed imu not published. OdometryF2M: use IMU roll/pitch values when IMU is used (reducing variance on those angles before BA).

2026-09-02 17:40:23 +08:00 · 2019-09-13 21:45:13 -04:00
parent bdc8d839ff
commit 350d3cd85f
13 changed files with 182 additions and 99 deletions
--- a/corelib/include/rtabmap/core/Link.h
+++ b/corelib/include/rtabmap/core/Link.h
@@ -69,8 +69,8 @@ public:
 	const Transform & transform() const {return transform_;}
 	Type type() const {return type_;}
 	const cv::Mat & infMatrix() const {return infMatrix_;}
-	double rotVariance() const;
-	double transVariance() const;
+	double rotVariance(bool minimum = true) const;
+	double transVariance(bool minimum = true) const;

 	void setFrom(int from) {from_ = from;}
 	void setTo(int to) {to_ = to;}
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -86,7 +86,7 @@ public:

 	void changeLinkIds(int idFrom, int idTo);

-	void removeLinks();
+	void removeLinks(bool keepSelfReferringLinks = false);
 	void removeLink(int idTo);
 	void removeVirtualLinks();

--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -405,26 +405,18 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 	// IMU filtering
 	if(_imuFilter && !data.imu().empty())
 	{
-		_imuFilter->update(
-				data.imu().angularVelocity()[0],
-				data.imu().angularVelocity()[1],
-				data.imu().angularVelocity()[2],
-				data.imu().linearAcceleration()[0],
-				data.imu().linearAcceleration()[1],
-				data.imu().linearAcceleration()[2],
-				data.stamp());
-		double qx,qy,qz,qw;
-		_imuFilter->getOrientation(qx,qy,qz,qw);
-		data.setIMU(IMU(
-				cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
-				data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
-				data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
-				data.imu().localTransform()));
-		UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
-					data.imu().orientation()[0],
-					data.imu().orientation()[1],
-					data.imu().orientation()[2],
-					data.imu().orientation()[3],
+		if(data.imu().angularVelocity()[0] == 0 &&
+		   data.imu().angularVelocity()[1] == 0 &&
+		   data.imu().angularVelocity()[2] == 0 &&
+		   data.imu().linearAcceleration()[0] == 0 &&
+		   data.imu().linearAcceleration()[1] == 0 &&
+		   data.imu().linearAcceleration()[2] == 0)
+		{
+			UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
+		}
+		else
+		{
+			_imuFilter->update(
 					data.imu().angularVelocity()[0],
 					data.imu().angularVelocity()[1],
 					data.imu().angularVelocity()[2],
@@ -432,6 +424,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					data.imu().linearAcceleration()[1],
 					data.imu().linearAcceleration()[2],
 					data.stamp());
+			double qx,qy,qz,qw;
+			_imuFilter->getOrientation(qx,qy,qz,qw);
+			data.setIMU(IMU(
+					cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
+					data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
+					data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
+					data.imu().localTransform()));
+			UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
+						data.imu().orientation()[0],
+						data.imu().orientation()[1],
+						data.imu().orientation()[2],
+						data.imu().orientation()[3],
+						data.imu().angularVelocity()[0],
+						data.imu().angularVelocity()[1],
+						data.imu().angularVelocity()[2],
+						data.imu().linearAcceleration()[0],
+						data.imu().linearAcceleration()[1],
+						data.imu().linearAcceleration()[2],
+						data.stamp());
+		}
 	}
 }

--- a/corelib/src/DBReader.cpp
+++ b/corelib/src/DBReader.cpp
@@ -353,6 +353,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			{
 				globalPose = priorLinks.begin()->second.transform();
 				globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
+				if(data.gps().stamp() != 0.0 &&
+						globalPoseCov.at<double>(3,3)>=9999 &&
+						globalPoseCov.at<double>(4,4)>=9999 &&
+						globalPoseCov.at<double>(5,5)>=9999)
+				{
+					// clear global pose as GPS was used for prior
+					globalPose.setNull();
+				}
 			}

 			Transform gravityTransform;
@@ -488,13 +496,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
 						Transform::getIdentity())); // we assume that gravity links are already transformed in base_link
 			}

-			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, IMU=%d",
+			UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, Grid=%d, UserData=%d, GlobalPose=%d, GPS=%d, IMU=%d",
 					data.laserScanRaw().isEmpty()?0:1,
 					data.imageRaw().empty()?0:1,
 					data.depthOrRightRaw().empty()?0:1,
 					data.gridCellSize()==0.0f?0:1,
 					data.userDataRaw().empty()?0:1,
 					globalPose.isNull()?0:1,
+					data.gps().stamp()!=0.0?1:0,
 					gravityTransform.isNull()?0:1);

 			cv::Mat descriptors;
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -898,8 +898,8 @@ void computeMaxGraphErrors(
 					fabs(iter->second.transform().x() - t.x()),
 					fabs(iter->second.transform().y() - t.y()),
 					fabs(iter->second.transform().z() - t.z()));
-			UASSERT(iter->second.transVariance()>0.0);
-			float stddevLinear = sqrt(iter->second.transVariance());
+			UASSERT(iter->second.transVariance(false)>0.0);
+			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
 			if(linearErrorRatio > maxLinearErrorRatio)
 			{
@@ -920,8 +920,8 @@ void computeMaxGraphErrors(
 					fabs(opt_pitch - link_pitch),
 					fabs(opt_yaw - link_yaw));
 			angularError = angularError>M_PI?2*M_PI-angularError:angularError;
-			UASSERT(iter->second.rotVariance()>0.0);
-			float stddevAngular = sqrt(iter->second.rotVariance());
+			UASSERT(iter->second.rotVariance(false)>0.0);
+			float stddevAngular = sqrt(iter->second.rotVariance(false));
 			float angularErrorRatio = angularError/stddevAngular;
 			if(angularErrorRatio > maxAngularErrorRatio)
 			{
--- a/corelib/src/Link.cpp
+++ b/corelib/src/Link.cpp
@@ -63,17 +63,47 @@ Link::Link(int from,
 	}
 }

-double Link::rotVariance() const
+double Link::rotVariance(bool minimum) const
 {
-	double min = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
-	UASSERT(min > 0.0);
-	return 1.0/min;
+	double value;
+	if(minimum)
+	{
+		value = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
+	}
+	else
+	{
+		value = uMin3(
+				infMatrix_.at<double>(3,3) <= 0.0001?9999999.0:infMatrix_.at<double>(3,3),
+				infMatrix_.at<double>(4,4) <= 0.0001?9999999.0:infMatrix_.at<double>(4,4),
+				infMatrix_.at<double>(5,5) <= 0.0001?9999999.0:infMatrix_.at<double>(5,5));
+		if(value == 9999999.0)
+		{
+			value = 0.0001;
+		}
+	}
+	UASSERT(value > 0.0);
+	return 1.0/value;
 }
-double Link::transVariance() const
+double Link::transVariance(bool minimum) const
 {
-	double min = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
-	UASSERT(min > 0.0);
-	return 1.0/min;
+	double value;
+	if(minimum)
+	{
+		value = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
+	}
+	else
+	{
+		value = uMin3(
+				infMatrix_.at<double>(0,0) <= 0.0001?9999999.0:infMatrix_.at<double>(0,0),
+				infMatrix_.at<double>(1,1) <= 0.0001?9999999.0:infMatrix_.at<double>(1,1),
+				infMatrix_.at<double>(2,2) <= 0.0001?9999999.0:infMatrix_.at<double>(2,2));
+		if(value == 9999999.0)
+		{
+			value = 0.0001;
+		}
+	}
+	UASSERT(value > 0.0);
+	return 1.0/value;
 }

 void Link::setInfMatrix(const cv::Mat & infMatrix) {
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -2342,7 +2342,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 				}

 			}
-			s->removeLinks(); // remove all links
+			s->removeLinks(true); // remove all links, but keep self referring link
 			s->removeLandmarks(); // remove all landmarks
 			s->setWeight(0);
 			s->setLabel(""); // reset label
@@ -5107,62 +5107,59 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

-	if(!isIntermediateNode)
+	// prior
+	if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
 	{
-		// prior
-		if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
-		{
-			s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
-			UDEBUG("Added global pose prior: %s", data.globalPose().prettyPrint().c_str());
+		s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
+		UDEBUG("Added global pose prior: %s", data.globalPose().prettyPrint().c_str());

-			if(data.gps().stamp() > 0.0)
-			{
-				UWARN("GPS constraint ignored as global pose is also set.");
-			}
-		}
-		else if(data.gps().stamp() > 0.0)
+		if(data.gps().stamp() > 0.0)
 		{
-			if(_gpsOrigin.stamp() <= 0.0)
-			{
-				_gpsOrigin =  data.gps();
-				UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
-			}
-			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
-			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
-			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
-			if(data.gps().error() > 0.0)
-			{
-				UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
-				// only set x, y as we don't know variance for other degrees of freedom.
-				gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 1.0/data.gps().error();
-				gpsInfMatrix.at<double>(2,2) = 1; // z variance is set to avoid issues with g2o and gtsam requiring a prior on Z
-				s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
-			}
-			else
-			{
-				UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
-			}
+			UWARN("GPS constraint ignored as global pose is also set.");
 		}
-
-		// IMU / Gravity constraint
-		if(_useOdometryGravity && !pose.isNull())
+	}
+	else if(data.gps().stamp() > 0.0)
+	{
+		if(_gpsOrigin.stamp() <= 0.0)
 		{
-			s->addLink(Link(s->id(), s->id(), Link::kGravity, pose.rotation()));
-			UDEBUG("Added gravity constraint from odom pose: %s", pose.rotation().prettyPrint().c_str());
+			_gpsOrigin =  data.gps();
+			UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
 		}
-		else if(!data.imu().localTransform().isNull() &&
-			(data.imu().orientation()[0] != 0 ||
-			 data.imu().orientation()[1] != 0 ||
-			 data.imu().orientation()[2] != 0 ||
-			 data.imu().orientation()[3] != 0))
-
+		cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
+		Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
+		cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
+		if(data.gps().error() > 0.0)
 		{
-			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
-			orientation*=data.imu().localTransform().rotation().inverse();
-
-			s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
-			UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());
+			UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
+			// only set x, y as we don't know variance for other degrees of freedom.
+			gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 1.0/data.gps().error();
+			gpsInfMatrix.at<double>(2,2) = 1; // z variance is set to avoid issues with g2o and gtsam requiring a prior on Z
+			s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
 		}
+		else
+		{
+			UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
+		}
+	}
+
+	// IMU / Gravity constraint
+	if(_useOdometryGravity && !pose.isNull())
+	{
+		s->addLink(Link(s->id(), s->id(), Link::kGravity, pose.rotation()));
+		UDEBUG("Added gravity constraint from odom pose: %s", pose.rotation().prettyPrint().c_str());
+	}
+	else if(!data.imu().localTransform().isNull() &&
+		(data.imu().orientation()[0] != 0 ||
+		 data.imu().orientation()[1] != 0 ||
+		 data.imu().orientation()[2] != 0 ||
+		 data.imu().orientation()[3] != 0))
+
+	{
+		Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
+		orientation*=data.imu().localTransform().rotation().inverse();
+
+		s->addLink(Link(s->id(), s->id(), Link::kGravity, orientation));
+		UDEBUG("Added gravity constraint: %s", orientation.prettyPrint().c_str());
 	}

 	//landmarks
--- a/corelib/src/Signature.cpp
+++ b/corelib/src/Signature.cpp
@@ -156,11 +156,29 @@ void Signature::changeLinkIds(int idFrom, int idTo)
 	}
 }

-void Signature::removeLinks()
+void Signature::removeLinks(bool keepSelfReferringLinks)
 {
-	if(_links.size())
+	size_t sizeBefore = _links.size();
+	if(keepSelfReferringLinks)
+	{
+		for(std::multimap<int, Link>::iterator iter = _links.begin(); iter != _links.end();)
+		{
+			if(iter->second.from() == iter->second.to())
+			{
+				++iter;
+			}
+			else
+			{
+				_links.erase(iter++);
+			}
+		}
+	}
+	else
+	{
+		_links.clear();
+	}
+	if(_links.size() != sizeBefore)
 		_linksModified = true;
-	_links.clear();
 }

 void Signature::removeLink(int idTo)
--- a/corelib/src/camera/CameraRealSense2.cpp
+++ b/corelib/src/camera/CameraRealSense2.cpp
@@ -614,7 +614,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					video_profile.height() == cameraHeight_ &&
 					video_profile.fps()    == cameraFps_)
 				{
-					profilesPerSensor[irDepth_?1:i].push_back(profile);
+					if(irDepth_ && i==0)
+					{
+						profilesPerSensor[1].push_back(profile.clone(profile.stream_type(), 1, profile.format()));
+					}
+					else
+					{
+						profilesPerSensor[i].push_back(profile);
+					}
 					auto intrinsic = video_profile.get_intrinsics();
 					if(i==1)
 					{
@@ -734,7 +741,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	 }
 	 else
 	 {
-
+
 		// look for calibration files
 		 std::string serial = sn;
 		 if(!cameraName.empty())
--- a/corelib/src/odometry/OdometryF2M.cpp
+++ b/corelib/src/odometry/OdometryF2M.cpp
@@ -333,10 +333,20 @@ Transform OdometryF2M::computeTransform(
 				UDEBUG("Registration time = %fs", regInfo.totalTime);
 				if(!transform.isNull())
 				{
+					cv::Mat var = regInfo.covariance.clone();
 					Transform imuT;
 					if(!imus_.empty())
 					{
 						imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
+
+						if(!imuT.isNull())
+						{
+							float roll, pitch, yaw;
+							imuT.getEulerAngles(roll, pitch, yaw);
+							transform = Transform(transform.x(),transform.y(),transform.z(),roll,pitch,transform.theta());
+							var.at<double>(3,3)/=100.0; // roll
+							var.at<double>(4,4)/=100.0; // pitch
+						}
 					}

 					// local bundle adjustment
@@ -369,9 +379,6 @@ Transform OdometryF2M::computeTransform(

 							UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
 									uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
-							cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
-							//var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
-							//var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
 							bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
 							bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));