Refactored how IMU is used in odometry (if guess is not set, use imu orientation for guess). Changed canProcessIMU() to canProcessAsynIMU() to make it more clear for odometry approaches able to process IMU between image frames (VIO approaches). ZedOC: fixed device closed if imu is not detected.

corelib/include/rtabmap/core/Odometry.h

@@ -67,7 +67,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() = 0;
 	virtual bool canProcessRawImages() const {return false;}
-	virtual bool canProcessIMU() const {return false;}
+	virtual bool canProcessAsyncIMU() const {return false;}
 
 	//getters
 	const Transform & getPose() const {return _pose;}

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -50,7 +50,6 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	const Signature & getMap() const {return *map_;}
 	const Signature & getLastFrame() const {return *lastFrame_;}
-	virtual bool canProcessIMU() const;
 
 	virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
 
@@ -79,7 +78,6 @@ private:
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
-	bool initGravity_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;

corelib/include/rtabmap/core/odometry/OdometryMSCKF.h

@@ -44,7 +44,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

corelib/include/rtabmap/core/odometry/OdometryOkvis.h

@@ -46,7 +46,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeOkvis;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

corelib/include/rtabmap/core/odometry/OdometryVINS.h

@@ -43,7 +43,7 @@ public:
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
 	virtual bool canProcessRawImages() const {return true;}
-	virtual bool canProcessIMU() const {return true;}
+	virtual bool canProcessAsyncIMU() const {return true;}
 
 private:
 	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

corelib/src/Odometry.cpp

@@ -284,6 +284,34 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
 
+	// cache imu data
+	if(!data.imu().empty() && !this->canProcessAsyncIMU())
+	{
+		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
+		{
+			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
+			// orientation includes roll and pitch but not yaw in local transform
+			Transform imuT = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
+
+			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
+				this->framesProcessed() == 0)
+			{
+				Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
+				Transform previous = this->getPose();
+				Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
+				UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
+				this->reset(newFramePose);
+			}
+
+			imus_.insert(std::make_pair(data.stamp(), imuT));
+			if(imus_.size() > 1000)
+			{
+				imus_.erase(imus_.begin());
+			}
+		}
+	}
+
+
 	if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
 	{
 		if(data.stereoCameraModel().isValidForRectification())
@@ -386,21 +414,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 	}
 
-	// cache imu data
-	if(!data.imu().empty())
-	{
-		if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
-		{
-			Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
-			// orientation includes roll and pitch but not yaw in local transform
-			imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
-			if(imus_.size() > 1000)
-			{
-				imus_.erase(imus_.begin());
-			}
-		}
-	}
-
 	// KITTI datasets start with stamp=0
 	double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
 	Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
@@ -447,7 +460,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	{
 		guess = guessIn;
 	}
-	else if(!data.imu().empty() && !imus_.empty())
+	else if(!imus_.empty())
 	{
 		// replace orientation guess with IMU (if available)
 		imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
@@ -458,6 +471,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
 					orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
 					orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
+			if(_force3DoF)
+			{
+				guess = guess.to3DoF();
+			}
 		}
 	}
 
@@ -523,7 +540,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}
 	}
-	else
+	else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
 	{
 		t = this->computeTransform(data, guess, info);
 	}

corelib/src/camera/CameraStereoZedOC.cpp

@@ -307,7 +307,7 @@ bool initCalibration(std::string calibration_file, cv::Size image_size, rtabmap:
     // (Linux only) Safety check A: Wrong "." or "," reading in file conf.
 #ifndef _WIN32
     if (right_cam_k1 == 0 && left_cam_k1 == 0 && left_cam_k2 == 0 && right_cam_k2 == 0) {
-        std::cout << "ZED File invalid" << std::endl;
+        UERROR("ZED File invalid");
 
         std::string cmd = "rm " + calibration_file;
         int res = system(cmd.c_str());
@@ -596,15 +596,14 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
 	zed_ = new sl_oc::video::VideoCapture(params);
 	if( !zed_->initializeVideo(usbDevice_) )
 	{
-		std::cerr << "Cannot open camera video capture" << std::endl;
-		std::cerr << "See verbosity level for more details." << std::endl;
+		UERROR("Cannot open camera video capture. Set log level <= info for more details.");
 
 		delete zed_;
 		zed_ = 0;
 		return false;
 	}
 	int sn = zed_->getSerialNumber();
-	std::cout << "Connected to camera sn: " << sn << std::endl;
+	UINFO("Connected to camera sn: %d", sn);
 	// <---- Create Video Capture
 
 	// ----> Retrieve calibration file from Stereolabs server
@@ -614,12 +613,12 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
 	// Download camera calibration file
 	if( !downloadCalibrationFile(serial_number, calibration_file) )
 	{
-		std::cerr << "Could not load calibration file from Stereolabs servers" << std::endl;
+		UERROR("Could not load calibration file from Stereolabs servers");
 		delete zed_;
 		zed_ = 0;
 		return false;
 	}
-	std::cout << "Calibration file found. Loading..." << std::endl;
+	UINFO("Calibration file found. Loading...");
 
 	// ----> Frame size
 	int w,h;
@@ -629,8 +628,11 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
 	 // ----> Initialize calibration
 	if(initCalibration(calibration_file, cv::Size(w/2,h), stereoModel_, this->getLocalTransform()))
 	{
-		std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
-		std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
+		if(ULogger::level() <= ULogger::kInfo)
+		{
+			std::cout << "Calibration left:" << std::endl << stereoModel_.left() << std::endl;
+			std::cout << "Calibration right:" << std::endl << stereoModel_.right() << std::endl;
+		}
 		stereoModel_.initRectificationMap();
 	}
 
@@ -638,37 +640,42 @@ bool CameraStereoZedOC::init(const std::string & calibrationFolder, const std::s
 	sensors_ = new sl_oc::sensors::SensorCapture((sl_oc::VERBOSITY)params.verbose);
 	if( !sensors_->initializeSensors(serial_number) ) // Note: we use the serial number acquired by the VideoCapture object
 	{
-		std::cerr << "Cannot open sensors capture" << std::endl;
-		std::cerr << "Try to enable verbose to get more info" << std::endl;
+		UERROR("Cannot open sensors capture. Set log level <= info for more details.");
 		delete sensors_;
 		sensors_ = 0;
 	}
 	else
 	{
-		std::cout << "Sensors Capture connected to camera sn: " << sensors_->getSerialNumber() << std::endl;
-
+		UINFO("Sensors Capture connected to camera sn: %d", sensors_->getSerialNumber());
+		UINFO("Wait max 5 sec to see if the camera has imu...");
 		// Check is IMU data is available
-		const sl_oc::sensors::data::Imu& imu = sensors_->getLastIMUData();
-		if(imu.valid != sl_oc::sensors::data::Imu::NEW_VAL)
+		UTimer timer;
+		while(timer.elapsed() < 5 &&
+			  sensors_->getLastIMUData().valid != sl_oc::sensors::data::Imu::NEW_VAL)
 		{
-			UINFO("CameraStereoZEDOC: Camera doesn't have IMU sensor");
-			delete sensors_;
-			sensors_ = 0;
+			// wait 5 sec to see if we can get an imu stream...
+			uSleep(100);
+		}
+		if(timer.elapsed() > 5)
+		{
+			UINFO("Camera doesn't have IMU sensor");
 		}
 		else
 		{
+			UINFO("Camera has IMU");
+
 			// Start the sensor capture thread. Note: since sensor data can be retrieved at 400Hz and video data frequency is
 			// minor (max 100Hz), we use a separated thread for sensors.
 			// Transform based on ZED2: x->down, y->right, z->backward
 			Transform imuLocalTransform_ = this->getLocalTransform() * Transform(0,1,0,0, 1,0,0,0, 0,0,-1,0);
-			std::cout << imuLocalTransform_ << std::endl;
+			//std::cout << imuLocalTransform_ << std::endl;
 			imuThread_ = new ZedOCThread(sensors_, imuLocalTransform_);
 			// <---- Create Sensors Capture
 
 			// ----> Enable video/sensors synchronization
 			if(!zed_->enableSensorSync(sensors_))
 			{
-				UWARN("CameraStereoZEDOC: Failed to start synchronization");
+				UWARN("Failed to enable image/imu synchronization");
 			}
 			// <---- Enable video/sensors synchronization
 
@@ -712,7 +719,8 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
 	{
 		UTimer timer;
 
-		bool imuReceived = true;
+		bool imuReceived = imuThread_!=0;
+		bool warned = false;
 		do
 		{
 			const sl_oc::video::Frame frame = zed_->getLastFrame();
@@ -729,6 +737,11 @@ SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
 				{
 					imuThread_->getIMU(stamp, imu, 10);
 					imuReceived = !imu.empty();
+					if(!imuReceived && !warned && timer.elapsed() > 1.0)
+					{
+						UWARN("Waiting for synchronized imu (this can take several seconds when camera has been just started)...");
+						warned = true;
+					}
 				}
 
 				if(imuReceived)

corelib/src/odometry/OdometryF2M.cpp

@@ -73,7 +73,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1)),
 	lastFrameOldestNewId_(0),
-	initGravity_(false),
 	bundleSeq_(0),
 	sba_(0)
 {
@@ -181,27 +180,19 @@ OdometryF2M::~OdometryF2M()
 void OdometryF2M::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
-	if(!initGravity_)
-	{
-		UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
-		Odometry::reset(initialPose);
-		*lastFrame_ = Signature(1);
-		*map_ = Signature(-1);
-		scansBuffer_.clear();
-		bundleWordReferences_.clear();
-		bundlePoses_.clear();
-		bundleLinks_.clear();
-		bundleModels_.clear();
-		bundlePoseReferences_.clear();
-		bundleSeq_ = 0;
-		lastFrameOldestNewId_ = 0;
-	}
-	initGravity_ = false;
-}
 
-bool OdometryF2M::canProcessIMU() const
-{
-	return sba_ && sba_->gravitySigma() > 0.0f;
+	UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
+	Odometry::reset(initialPose);
+	*lastFrame_ = Signature(1);
+	*map_ = Signature(-1);
+	scansBuffer_.clear();
+	bundleWordReferences_.clear();
+	bundlePoses_.clear();
+	bundleLinks_.clear();
+	bundleModels_.clear();
+	bundlePoseReferences_.clear();
+	bundleSeq_ = 0;
+	lastFrameOldestNewId_ = 0;
 }
 
 // return not null transform if odometry is correctly computed
@@ -220,33 +211,9 @@ Transform OdometryF2M::computeTransform(
 	}
 
 	Transform imuT;
-	if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
+	if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
 	{
-		if(imus().empty())
-		{
-			UERROR("IMU received doesn't have orientation set, it is ignored. If you are using RTAB-Map standalone, enable IMU filtering in Preferences->Source panel. On ROS, use \"imu_filter_madgwick\" or \"imu_complementary_filter\" packages to compute the orientation.");
-		}
-		else
-		{
-			imuT = Transform::getTransform(imus(), data.stamp());
-			if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f)
-			{
-				if(!imuT.isNull())
-				{
-					Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
-					Transform previous = this->getPose();
-					Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
-					UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
-					initGravity_ = true;
-					this->reset(newFramePose);
-				}
-			}
-		}
-
-		if(data.imageRaw().empty() && data.laserScanRaw().isEmpty())
-		{
-			return output;
-		}
+		imuT = Transform::getTransform(imus(), data.stamp());
 	}
 
 	RegistrationInfo regInfo;