diff --git a/corelib/include/rtabmap/core/OdometryThread.h b/corelib/include/rtabmap/core/OdometryThread.h
index 159ee55f..6d581b73 100644
--- a/corelib/include/rtabmap/core/OdometryThread.h
+++ b/corelib/include/rtabmap/core/OdometryThread.h
@@ -67,8 +67,8 @@ private:
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
-	double _lastImuStamp;
-	double _imuEstimatedDelay;
+	double _oldestAsyncImuStamp;
+	double _newestAsyncImuStamp;
 };
 
 } // namespace rtabmap
diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp
index 0926019e..bf605853 100644
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -445,6 +445,10 @@ bool importPoses(
 				std::list<std::string> strList = uSplit(str);
 				if((strList.size() >=  8 && format!=11) || (strList.size() ==  9 && format==11))
 				{
+					if(!uIsNumber(strList.front())) {
+						UWARN("Skipping \"%s\"", str.c_str());
+						continue;
+					}
 					double stamp = uStr2Double(strList.front());
 					strList.pop_front();
 					if(format==11)
diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp
index f03dc626..2b25e15c 100644
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -322,7 +322,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				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());
+				std::map<double, rtabmap::Transform> imus = imus_;
 				this->reset(newFramePose);
+				imus_ = imus;
 			}
 
 			imus_.insert(std::make_pair(data.stamp(), imuT));
diff --git a/corelib/src/OdometryThread.cpp b/corelib/src/OdometryThread.cpp
index 33968c31..490a3753 100644
--- a/corelib/src/OdometryThread.cpp
+++ b/corelib/src/OdometryThread.cpp
@@ -40,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
 	_resetPose(Transform::getIdentity()),
-	_lastImuStamp(0.0),
-	_imuEstimatedDelay(0.0)
+	_oldestAsyncImuStamp(0.0),
+	_newestAsyncImuStamp(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -110,7 +110,8 @@ void OdometryThread::mainLoop()
 		UScopeMutex lock(_dataMutex);
 		_dataBuffer.clear();
 		_imuBuffer.clear();
-		_lastImuStamp = 0.0f;
+		_oldestAsyncImuStamp = 0.0;
+		_newestAsyncImuStamp = 0.0;
 	}
 
 	SensorData data;
@@ -161,22 +162,39 @@ void OdometryThread::addData(const SensorData & data)
 			!data.laserScanCompressed().empty() || 
 			data.imu().empty())
 		{
-			_dataBuffer.push_back(data);
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-			{
-				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-				_dataBuffer.erase(_dataBuffer.begin());
+			if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
 				notify = false;
 			}
+			else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
+				UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
+					"(%f), skipping that frame (imu buffer size=%ld). "
+					"When using async IMU, make sure IMU is published faster "
+					"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
+					data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
+				notify = false;
+			}
+			else {
+				_dataBuffer.push_back(data);
+				while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+				{
+					UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+					_dataBuffer.erase(_dataBuffer.begin());
+					notify = false;
+				}
+			}
 		}
 		else
 		{
 			_imuBuffer.push_back(data);
-			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
-			{
-				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
+			if(_oldestAsyncImuStamp == 0) {
+				_oldestAsyncImuStamp = data.stamp();
 			}
-			_lastImuStamp = data.stamp();
+			_newestAsyncImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -195,18 +213,14 @@ bool OdometryThread::getData(SensorData & data)
 	{
 		if(!_dataBuffer.empty())
 		{
-			if(!_imuBuffer.empty())
+			// Send IMU up to stamp greater than image (OpenVINS needs this).
+			while(!_imuBuffer.empty())
 			{
-				// Send IMU up to stamp greater than image (OpenVINS needs this).
-				while(!_imuBuffer.empty())
-				{
-					_odometry->process(_imuBuffer.front());
-					double stamp = _imuBuffer.front().stamp();
-					_imuBuffer.pop_front();
-					if(stamp > _dataBuffer.front().stamp())
-					{
-						break;
-					}
+				_odometry->process(_imuBuffer.front());
+				double stamp =_imuBuffer.front().stamp();
+				_imuBuffer.pop_front();
+				if(stamp > _dataBuffer.front().stamp()) {
+					break;
 				}
 			}
 
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index 6eb50a99..86e69244 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -8693,11 +8693,13 @@ void MainWindow::changeState(MainWindow::State newState)
 
 		if(_sensorCapture)
 		{
-			_sensorCapture->start();
 			if(_imuThread)
 			{
 				_imuThread->start();
+				// give imu thread a head start
+				uSleep(10);
 			}
+			_sensorCapture->start();
 			ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 		}
 		break;
@@ -8729,11 +8731,13 @@ void MainWindow::changeState(MainWindow::State newState)
 
 			if(_sensorCapture)
 			{
-				_sensorCapture->start();
 				if(_imuThread)
 				{
 					_imuThread->start();
+					// give imu thread a head start
+					uSleep(10);
 				}
+				_sensorCapture->start();
 				ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 			}
 		}