LiDAR capture support in standalone library (#1264)

* Working rtabmap_lidar-mapping example (live and pcap)

* finalizing merge, added some deprecated

* fixed build

* Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI.

* backward compatibility

* fixed some not used variable warnings, fixed qt build for lidar mapping example

* Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8

* ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera  inputs.

* Working external lidar on iOS

* util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup

* fixed winsock include order

* reverted camera tool

* disable imu filtering when odom sensor is used

* Updated package version

* fixed windows build

* fixing more windows build erros

corelib/src/CMakeLists.txt

@@ -13,8 +13,10 @@ SET(SRC_FILES
     
     Recovery.cpp
     
+    SensorCapture.cpp
+    SensorCaptureThread.cpp
+    
     Camera.cpp
-    CameraThread.cpp
     CameraModel.cpp
     
     camera/CameraFreenect.cpp
@@ -39,6 +41,8 @@ SET(SRC_FILES
     camera/CameraMyntEye.cpp
     camera/CameraDepthAI.cpp
     
+    lidar/LidarVLP16.cpp
+    
     EpipolarGeometry.cpp
     VisualWord.cpp
     VWDictionary.cpp

corelib/src/Camera.cpp

@@ -26,42 +26,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/IMUFilter.h"
 
-#include <rtabmap/utilite/UEventsManager.h>
-#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <iostream>
-#include <cmath>
+#include <rtabmap/utilite/UEventsManager.h>
 
 namespace rtabmap
 {
 
 Camera::Camera(float imageRate, const Transform & localTransform) :
-	_imageRate(imageRate),
-	_localTransform(localTransform*CameraModel::opticalRotation()),
-	_targetImageSize(0,0),
-	_frameRateTimer(new UTimer()),
-	_seq(0)
-{
-}
+		SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
+		imuFilter_(0),
+		publishInterIMU_(false)
+{}
 
 Camera::~Camera()
 {
-	UDEBUG("");
-	delete _frameRateTimer;
-	UDEBUG("");
-}
-
-void Camera::resetTimer()
-{
-	_frameRateTimer->start();
+	delete imuFilter_;
 }
 
 bool Camera::initFromFile(const std::string & calibrationPath)
@@ -69,54 +52,32 @@ bool Camera::initFromFile(const std::string & calibrationPath)
 	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
 }
 
-SensorData Camera::takeImage(CameraInfo * info)
+void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
 {
-	bool warnFrameRateTooHigh = false;
-	float actualFrameRate = 0;
-	float imageRate = _imageRate;
-	if(imageRate>0)
-	{
-		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
-		if(sleepTime > 2)
-		{
-			uSleep(sleepTime-2);
-		}
-		else if(sleepTime < 0)
-		{
-			warnFrameRateTooHigh = true;
-			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
-		}
+	publishInterIMU_ = enabled;
+	delete imuFilter_;
+	imuFilter_ = filter;
+}
 
-		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
-		{
-			//
-		}
-
-		double slept = _frameRateTimer->getElapsedTime();
-		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
-	}
-
-	UTimer timer;
-	SensorData data = this->captureImage(info);
-	double captureTime = timer.ticks();
-	if(warnFrameRateTooHigh)
+void Camera::postInterIMU(const IMU & imu, double stamp)
+{
+	if(imuFilter_)
 	{
-		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-				imageRate, actualFrameRate, captureTime);
+		imuFilter_->update(
+				imu.angularVelocity()[0], imu.angularVelocity()[1], imu.angularVelocity()[2],
+				imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2],
+				stamp);
+		cv::Vec4d q;
+		imuFilter_->getOrientation(q[0],q[1],q[2],q[3]);
+		UEventsManager::post(new IMUEvent(IMU(
+				q, cv::Mat(),
+				imu.angularVelocity(), imu.angularVelocityCovariance(),
+				imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+				imu.localTransform()),
+				stamp));
+		return;
 	}
-	else
-	{
-		UDEBUG("Time capturing image = %fs", captureTime);
-	}
-	if(info)
-	{
-		info->id = data.id();
-		info->stamp = data.stamp();
-		info->timeCapture = captureTime;
-	}
-	return data;
+	UEventsManager::post(new IMUEvent(imu, stamp));
 }
 
 } // namespace rtabmap

corelib/src/DBReader.cpp

@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/DBDriver.h"
 
@@ -34,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UEventsManager.h>
 
-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
@@ -268,7 +268,7 @@ std::string DBReader::getSerial() const
 	return "DBReader";
 }
 
-SensorData DBReader::captureImage(CameraInfo * info)
+SensorData DBReader::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data = this->getNextData(info);
 	if(data.id()>0 && _stopId>0 && data.id() > _stopId)
@@ -370,7 +370,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
 	return data;
 }
 
-SensorData DBReader::getNextData(CameraInfo * info)
+SensorData DBReader::getNextData(SensorCaptureInfo * info)
 {
 	SensorData data;
 	if(_dbDriver)

corelib/src/LaserScan.cpp

@@ -65,6 +65,9 @@ std::string LaserScan::formatName(const Format & format)
 		case kXYZRGBNormal:
 			name = "XYZRGBNormal";
 			break;
+		case kXYZIT:
+			name = "XYZIT";
+			break;
 		default:
 			name = "Unknown";
 			break;
@@ -88,6 +91,7 @@ int LaserScan::channels(const Format & format)
 			channels = 4;
 			break;
 		case kXYNormal:
+		case kXYZIT:
 			channels = 5;
 			break;
 		case kXYZNormal:
@@ -119,7 +123,11 @@ bool LaserScan::isScanHasRGB(const Format & format)
 }
 bool LaserScan::isScanHasIntensity(const Format & format)
 {
-	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
+	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
+}
+bool LaserScan::isScanHasTime(const Format & format)
+{
+	return format==kXYZIT;
 }
 
 LaserScan LaserScan::backwardCompatibility(
@@ -213,7 +221,14 @@ LaserScan::LaserScan(
 		const LaserScan & scan,
 		int maxPoints,
 		float maxRange,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	UASSERT(scan.empty() || scan.format() != kUnknown);
 	init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
@@ -224,7 +239,14 @@ LaserScan::LaserScan(
 		int maxPoints,
 		float maxRange,
 		Format format,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
 }
@@ -234,7 +256,14 @@ LaserScan::LaserScan(
 		int maxPoints,
 		float maxRange,
 		Format format,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
 }
@@ -246,7 +275,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	UASSERT(scan.empty() || scan.format() != kUnknown);
 	init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
@@ -260,7 +296,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
 }
@@ -273,7 +316,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
 }
@@ -289,8 +339,7 @@ void LaserScan::init(
 		int maxPoints,
 		const Transform & localTransform)
 {
-	UASSERT(data.empty() || data.rows == 1);
-	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
+	UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
 	UASSERT(!localTransform.isNull());
 
 	bool is2D = false;
@@ -307,6 +356,10 @@ void LaserScan::init(
 		// 3D scan
 		UASSERT(rangeMax>=rangeMin);
 		maxPoints_ = maxPoints;
+		if(maxPoints_ == 0 && data.rows>1)
+		{
+			maxPoints_ = data.rows * data.cols;
+		}
 	}
 
 	data_ = data;
@@ -320,18 +373,18 @@ void LaserScan::init(
 
 	if(!data.empty() && !isCompressed())
 	{
-		if(is2D && data_.cols > maxPoints_)
+		if(is2D && (int)data_.total() > maxPoints_)
 		{
-			UWARN("The number of points (%d) in the scan is over the maximum "
+			UWARN("The number of points (%ld) in the scan is over the maximum "
 				  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
 				  "The scan info may be wrong!",
-				  data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
+				  data_.total(), maxPoints_, angleMin_, angleMax_, angleIncrement_);
 		}
-		else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
+		else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_)
 		{
-			UDEBUG("The number of points (%d) in the scan is over the maximum "
+			UDEBUG("The number of points (%ld) in the scan is over the maximum "
 				  "points (%d) defined by max points setting.",
-				  data_.cols, maxPoints_);
+				  data_.total(), maxPoints_);
 		}
 
 		if(format == kUnknown)
@@ -350,7 +403,7 @@ void LaserScan::init(
 			UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
-			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 		}
@@ -366,11 +419,36 @@ LaserScan LaserScan::clone() const
 	return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
 }
 
+LaserScan LaserScan::densify() const
+{
+	if(!isOrganized())
+	{
+		return *this;
+	}
+	cv::Mat output(1, data_.total(), data_.type());
+	int oi = 0;
+	for(int i=0; i<data_.rows; ++i)
+	{
+		for(int j=0; j<data_.cols; ++j)
+		{
+			const float * ptr = data_.ptr<float>(i, j);
+			float * outputPtr = output.ptr<float>(0, oi);
+			if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2]))))
+			{
+				memcpy(outputPtr, ptr, data_.elemSize());
+				++oi;
+			}
+		}
+	}
+	return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone());
+}
+
 float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
 {
-	UASSERT(pointIndex < (unsigned int)data_.cols);
+	UASSERT(pointIndex < (unsigned int)data_.total());
 	UASSERT(channelOffset < (unsigned int)data_.channels());
-	return data_.ptr<float>(0, pointIndex)[channelOffset];
+	unsigned int row = pointIndex / data_.cols;
+	return data_.ptr<float>(row, pointIndex - row * data_.cols)[channelOffset];
 }
 
 LaserScan & LaserScan::operator+=(const LaserScan & scan)
@@ -381,7 +459,7 @@ LaserScan & LaserScan::operator+=(const LaserScan & scan)
 
 LaserScan LaserScan::operator+(const LaserScan & scan)
 {
-	UASSERT(this->empty() || scan.empty() || this->format() == scan.format());
+	UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized()));
 	LaserScan dest;
 	if(!scan.empty())
 	{

corelib/src/Odometry.cpp

@@ -140,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_alignWithGround(Parameters::defaultOdomAlignWithGround()),
 		_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
 		_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
+		_deskewing(Parameters::defaultOdomDeskewing()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
@@ -169,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
 	Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+	Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing);
 
 	if(_imageDecimation == 0)
 	{
@@ -620,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	}
 
 	UTimer time;
+
+	// Deskewing lidar
+	if( _deskewing &&
+		!data.laserScanRaw().empty() &&
+		data.laserScanRaw().hasTime() &&
+		dt > 0 &&
+		!guess.isNull())
+	{
+		UDEBUG("Deskewing begin");
+		// Recompute velocity
+		float vx,vy,vz, vroll,vpitch,vyaw;
+		guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+
+		// transform to velocity
+		vx /= dt;
+		vy /= dt;
+		vz /= dt;
+		vroll /= dt;
+		vpitch /= dt;
+		vyaw /= dt;
+
+		if(!imus_.empty())
+		{
+			float scanTime =
+				data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
+				data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+
+			// replace orientation velocity based on IMU (if available)
+			Transform imuFirstScan = Transform::getTransform(imus_,
+					data.stamp() +
+					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
+			Transform imuLastScan = Transform::getTransform(imus_,
+					data.stamp() +
+					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
+			if(!imuFirstScan.isNull() && !imuLastScan.isNull())
+			{
+				Transform orientation = imuFirstScan.inverse() * imuLastScan;
+				orientation.getEulerAngles(vroll, vpitch, vyaw);
+				if(_force3DoF)
+				{
+					vroll=0;
+					vpitch=0;
+					vyaw /= scanTime;
+				}
+				else
+				{
+					vroll /= scanTime;
+					vpitch /= scanTime;
+					vyaw /= scanTime;
+				}
+			}
+		}
+
+		Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
+		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
+		if(!scanDeskewed.isEmpty())
+		{
+			data.setLaserScan(scanDeskewed);
+		}
+		info->timeDeskewing = time.ticks();
+		UDEBUG("Deskewing end");
+	}
+	if(data.laserScanRaw().isOrganized())
+	{
+		// Laser scans should be dense passing this point
+		data.setLaserScan(data.laserScanRaw().densify());
+	}
+
+
 	Transform t;
 	if(_imageDecimation > 1 && !data.imageRaw().empty())
 	{

corelib/src/OdometryThread.cpp

@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/odometry/OdometryMono.h"
 #include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/utilite/ULogger.h"
 
@@ -58,12 +58,12 @@ bool OdometryThread::handleEvent(UEvent * event)
 {
 	if(this->isRunning())
 	{
-		if(event->getClassName().compare("CameraEvent") == 0)
+		if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			CameraEvent * cameraEvent = (CameraEvent*)event;
-			if(cameraEvent->getCode() == CameraEvent::kCodeData)
+			SensorEvent * sensorEvent = (SensorEvent*)event;
+			if(sensorEvent->getCode() == SensorEvent::kCodeData)
 			{
-				this->addData(cameraEvent->data());
+				this->addData(sensorEvent->data());
 			}
 		}
 		else if(event->getClassName().compare("IMUEvent") == 0)

corelib/src/Parameters.cpp

@@ -1172,11 +1172,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 	return out;
 }
 
-
-void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
+void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, ParametersMap & parameters, bool modifiedOnly)
 {
-	CSimpleIniA ini;
-	ini.LoadFile(configFile.c_str());
 	const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core");
 	if(keyValMap)
 	{
@@ -1191,12 +1188,12 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 				{
 					if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
 					{
-						if(configFile.find(".rtabmap") != std::string::npos)
+						if(configFilePath.find(".rtabmap") != std::string::npos)
 						{
 							UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
 								   "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
 								   "to new version.",
-								   configFile.c_str(),
+								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);
 						}
@@ -1205,7 +1202,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 							UERROR("Version in the config file \"%s\" is more recent (\"%s\") than "
 								   "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
 								   "be ignored.",
-								   configFile.c_str(),
+								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);
 						}
@@ -1255,11 +1252,26 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 	else
 	{
 		ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
-				    "Ignore this warning if the ini file does not exist yet. "
-				    "The ini file will be automatically created when rtabmap will close.", configFile.c_str());
+					"Ignore this warning if the ini file does not exist yet. "
+					"The ini file will be automatically created when rtabmap will close.", configFilePath.c_str());
 	}
 }
 
+
+void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
+{
+	CSimpleIniA ini;
+	ini.LoadFile(configFile.c_str());
+	readINIImpl(ini, configFile, parameters, modifiedOnly);
+}
+
+void Parameters::readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly)
+{
+	CSimpleIniA ini;
+	ini.LoadData(configContent);
+	readINIImpl(ini, "", parameters, modifiedOnly);
+}
+
 void Parameters::writeINI(const std::string & configFile, const ParametersMap & parameters)
 {
 	CSimpleIniA ini;

corelib/src/Recovery.cpp

@@ -160,7 +160,7 @@ bool databaseRecovery(
 		DBReader dbReader(databasePath, 0, odometryIgnored);
 		dbReader.init();
 
-		CameraInfo info;
+		SensorCaptureInfo info;
 		SensorData data = dbReader.takeImage(&info);
 		int processed = 0;
 		if (progressState)

corelib/src/RegistrationIcp.cpp

@@ -69,6 +69,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_force4DoF(Parameters::defaultIcpForce4DoF()),
+	_filtersEnabled(Parameters::defaultIcpFiltersEnabled()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
 	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
@@ -115,6 +116,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
 	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
 	Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF);
+	Parameters::parse(parameters, Parameters::kIcpFiltersEnabled(), _filtersEnabled);
 	Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
@@ -337,6 +339,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
 	UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
 	UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
+	UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false");
 	UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
 	UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
 	UDEBUG("Strategy=%d", _strategy);
@@ -360,7 +363,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size();
 	int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size();
 
-	if(!dataFrom.laserScanRaw().empty())
+	if(!dataFrom.laserScanRaw().empty() && (_filtersEnabled & 1))
 	{
 		int pointsBeforeFiltering = dataFrom.laserScanRaw().size();
 		LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(),
@@ -401,7 +404,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 		float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering);
 		maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio);
 	}
-	if(!dataTo.laserScanRaw().empty())
+	if(!dataTo.laserScanRaw().empty() && (_filtersEnabled & 2))
 	{
 		int pointsBeforeFiltering = dataTo.laserScanRaw().size();
 		LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(),

corelib/src/RtabmapThread.cpp

@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/UserDataEvent.h"
@@ -371,11 +371,11 @@ bool RtabmapThread::handleEvent(UEvent* event)
 			// IMU events are published at high frequency, early exit
 			return false;
 		}
-		else if(event->getClassName().compare("CameraEvent") == 0)
+		else if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			UDEBUG("CameraEvent");
-			CameraEvent * e = (CameraEvent*)event;
-			if(e->getCode() == CameraEvent::kCodeData)
+			UDEBUG("SensorEvent");
+			SensorEvent * e = (SensorEvent*)event;
+			if(e->getCode() == SensorEvent::kCodeData)
 			{
 				if (_rtabmap->isRGBDMode())
 				{

corelib/src/SensorCapture.cpp

@@ -0,0 +1,114 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/SensorCapture.h"
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <iostream>
+#include <cmath>
+
+namespace rtabmap
+{
+
+SensorCapture::SensorCapture(float frameRate, const Transform & localTransform) :
+	_frameRate(frameRate),
+	_localTransform(localTransform),
+	_frameRateTimer(new UTimer()),
+	_seq(0)
+{
+}
+
+SensorCapture::~SensorCapture()
+{
+	delete _frameRateTimer;
+}
+
+void SensorCapture::resetTimer()
+{
+	_frameRateTimer->start();
+}
+
+SensorData SensorCapture::takeData(SensorCaptureInfo * info)
+{
+	bool warnFrameRateTooHigh = false;
+	float actualFrameRate = 0;
+	float frameRate = _frameRate;
+	if(frameRate>0)
+	{
+		int sleepTime = (1000.0f/frameRate - 1000.0f*_frameRateTimer->getElapsedTime());
+		if(sleepTime > 2)
+		{
+			uSleep(sleepTime-2);
+		}
+		else if(sleepTime < 0)
+		{
+			warnFrameRateTooHigh = true;
+			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
+		}
+
+		// Add precision at the cost of a small overhead
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(frameRate)-0.000001)
+		{
+			//
+		}
+
+		double slept = _frameRateTimer->getElapsedTime();
+		_frameRateTimer->start();
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
+	}
+
+	UTimer timer;
+	SensorData data = this->captureData(info);
+	double captureTime = timer.ticks();
+	if(warnFrameRateTooHigh)
+	{
+		UWARN("Camera: Cannot reach target frame rate %f Hz, current rate is %f Hz and capture time = %f s.",
+				frameRate, actualFrameRate, captureTime);
+	}
+	else
+	{
+		UDEBUG("Time capturing data = %fs", captureTime);
+	}
+	if(info)
+	{
+		info->id = data.id();
+		info->stamp = data.stamp();
+		info->timeCapture = captureTime;
+	}
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/SensorCaptureThread.cpp

@@ -25,9 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/SensorCaptureThread.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/Lidar.h"
+#include "rtabmap/core/SensorEvent.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
@@ -50,143 +51,149 @@ namespace rtabmap
 {
 
 // ownership transferred
-CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
-		_camera(camera),
-		_odomSensor(0),
-		_odomAsGt(false),
-		_poseTimeOffset(0.0),
-		_poseScaleFactor(1.0f),
-		_mirroring(false),
-		_stereoExposureCompensation(false),
-		_colorOnly(false),
-		_imageDecimation(1),
-		_histogramMethod(0),
-		_stereoToDepth(false),
-		_scanFromDepth(false),
-		_scanDownsampleStep(1),
-		_scanRangeMin(0.0f),
-		_scanRangeMax(0.0f),
-		_scanVoxelSize(0.0f),
-		_scanNormalsK(0),
-		_scanNormalsRadius(0.0f),
-		_scanForceGroundNormalsUp(false),
-		_stereoDense(StereoDense::create(parameters)),
-		_distortionModel(0),
-		_bilateralFiltering(false),
-		_bilateralSigmaS(10),
-		_bilateralSigmaR(0.1),
-		_imuFilter(0),
-		_imuBaseFrameConversion(false),
-		_featureDetector(0),
-		_depthAsMask(Parameters::defaultVisDepthAsMask())
+SensorCaptureThread::SensorCaptureThread(
+		Camera * camera,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
 {
-	UASSERT(_camera != 0);
+	UASSERT(camera != 0);
 }
 
 // ownership transferred
-CameraThread::CameraThread(
+SensorCaptureThread::SensorCaptureThread(
 		Camera * camera,
-		Camera * odomSensor,
+		SensorCapture * odomSensor,
 		const Transform & extrinsics,
 		double poseTimeOffset,
 		float poseScaleFactor,
-		bool odomAsGt,
+		double poseWaitTime,
 		const ParametersMap & parameters) :
-			_camera(camera),
-			_odomSensor(odomSensor),
-			_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
-			_odomAsGt(odomAsGt),
-			_poseTimeOffset(poseTimeOffset),
-			_poseScaleFactor(poseScaleFactor),
-			_mirroring(false),
-			_stereoExposureCompensation(false),
-			_colorOnly(false),
-			_imageDecimation(1),
-			_histogramMethod(0),
-			_stereoToDepth(false),
-			_scanFromDepth(false),
-			_scanDownsampleStep(1),
-			_scanRangeMin(0.0f),
-			_scanRangeMax(0.0f),
-			_scanVoxelSize(0.0f),
-			_scanNormalsK(0),
-			_scanNormalsRadius(0.0f),
-			_scanForceGroundNormalsUp(false),
-			_stereoDense(StereoDense::create(parameters)),
-			_distortionModel(0),
-			_bilateralFiltering(false),
-			_bilateralSigmaS(10),
-			_bilateralSigmaR(0.1),
-			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
 {
-	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
-	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
-	UDEBUG("_poseTimeOffset        =%f", _poseTimeOffset);
-	UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
-	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+	UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull());
 }
 
-// ownership transferred
-CameraThread::CameraThread(
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
+{
+	UASSERT(lidar != 0);
+}
+
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
 		Camera * camera,
-		bool odomAsGt,
 		const ParametersMap & parameters) :
-			_camera(camera),
-			_odomSensor(0),
-			_odomAsGt(odomAsGt),
-			_poseTimeOffset(0.0),
-			_poseScaleFactor(1.0f),
-			_mirroring(false),
-			_stereoExposureCompensation(false),
-			_colorOnly(false),
-			_imageDecimation(1),
-			_histogramMethod(0),
-			_stereoToDepth(false),
-			_scanFromDepth(false),
-			_scanDownsampleStep(1),
-			_scanRangeMin(0.0f),
-			_scanRangeMax(0.0f),
-			_scanVoxelSize(0.0f),
-			_scanNormalsK(0),
-			_scanNormalsRadius(0.0f),
-			_scanForceGroundNormalsUp(false),
-			_stereoDense(StereoDense::create(parameters)),
-			_distortionModel(0),
-			_bilateralFiltering(false),
-			_bilateralSigmaS(10),
-			_bilateralSigmaR(0.1),
-			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
 {
-	UASSERT(_camera != 0);
-	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+	UASSERT(lidar != 0 && camera != 0);
 }
 
-CameraThread::~CameraThread()
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		SensorCapture * odomSensor,
+		double poseTimeOffset,
+		float poseScaleFactor,
+		double poseWaitTime,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
+{
+	UASSERT(lidar != 0 && odomSensor != 0);
+}
+
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		Camera * camera,
+		SensorCapture * odomSensor,
+		const Transform & extrinsics,
+		double poseTimeOffset,
+		float poseScaleFactor,
+		double poseWaitTime,
+		const ParametersMap & parameters) :
+				_camera(camera),
+				_odomSensor(odomSensor),
+				_lidar(lidar),
+				_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
+				_odomAsGt(false),
+				_poseTimeOffset(poseTimeOffset),
+				_poseScaleFactor(poseScaleFactor),
+				_poseWaitTime(poseWaitTime),
+				_mirroring(false),
+				_stereoExposureCompensation(false),
+				_colorOnly(false),
+				_imageDecimation(1),
+				_histogramMethod(0),
+				_stereoToDepth(false),
+				_scanDeskewing(false),
+				_scanFromDepth(false),
+				_scanDownsampleStep(1),
+				_scanRangeMin(0.0f),
+				_scanRangeMax(0.0f),
+				_scanVoxelSize(0.0f),
+				_scanNormalsK(0),
+				_scanNormalsRadius(0.0f),
+				_scanForceGroundNormalsUp(false),
+				_stereoDense(StereoDense::create(parameters)),
+				_distortionModel(0),
+				_bilateralFiltering(false),
+				_bilateralSigmaS(10),
+				_bilateralSigmaR(0.1),
+				_imuFilter(0),
+				_imuBaseFrameConversion(false),
+				_featureDetector(0),
+				_depthAsMask(Parameters::defaultVisDepthAsMask())
+{
+	UASSERT(_camera != 0 || _lidar != 0);
+	if(_lidar && _camera)
+	{
+		_camera->setFrameRate(0);
+	}
+	if(_odomSensor)
+	{
+		if(_camera)
+		{
+			if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull())
+			{
+				_extrinsicsOdomToCamera.setIdentity();
+			}
+			UASSERT(!_extrinsicsOdomToCamera.isNull());
+			UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
+		}
+		UDEBUG("_poseTimeOffset        =%f", _poseTimeOffset);
+		UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
+		UDEBUG("_poseWaitTime          =%f", _poseWaitTime);
+	}
+}
+
+SensorCaptureThread::~SensorCaptureThread()
 {
 	join(true);
+	if(_odomSensor != _camera && _odomSensor != _lidar)
+	{
+		delete _odomSensor;
+	}
 	delete _camera;
-	delete _odomSensor;
+	delete _lidar;
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
 	delete _featureDetector;
 }
 
-void CameraThread::setImageRate(float imageRate)
+void SensorCaptureThread::setFrameRate(float frameRate)
 {
-	if(_camera)
+	if(_lidar)
 	{
-		_camera->setImageRate(imageRate);
+		_lidar->setFrameRate(frameRate);
+	}
+	else if(_camera)
+	{
+		_camera->setFrameRate(frameRate);
 	}
 }
 
-void CameraThread::setDistortionModel(const std::string & path)
+void SensorCaptureThread::setDistortionModel(const std::string & path)
 {
 	if(_distortionModel)
 	{
@@ -206,7 +213,7 @@ void CameraThread::setDistortionModel(const std::string & path)
 	}
 }
 
-void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
+void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 {
 	UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
 	_bilateralFiltering = true;
@@ -214,20 +221,20 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }
 
-void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
+void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
 {
 	delete _imuFilter;
 	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
 	_imuBaseFrameConversion = baseFrameConversion;
 }
 
-void CameraThread::disableIMUFiltering()
+void SensorCaptureThread::disableIMUFiltering()
 {
 	delete _imuFilter;
 	_imuFilter = 0;
 }
 
-void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
+void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters)
 {
 	delete _featureDetector;
 	ParametersMap params = parameters;
@@ -245,35 +252,38 @@ void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
 	_featureDetector = Feature2D::create(params);
 	_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
 }
-void CameraThread::disableFeatureDetection()
+void SensorCaptureThread::disableFeatureDetection()
 {
 	delete _featureDetector;
 	_featureDetector = 0;
 }
 
-void CameraThread::setScanParameters(
+void SensorCaptureThread::setScanParameters(
 	bool fromDepth,
 	int downsampleStep,
 	float rangeMin,
 	float rangeMax,
 	float voxelSize,
 	int normalsK,
-	int normalsRadius,
-	bool forceGroundNormalsUp)
+	float normalsRadius,
+	bool forceGroundNormalsUp,
+	bool deskewing)
 {
-	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
+	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing);
 }
 
-void CameraThread::setScanParameters(
+void SensorCaptureThread::setScanParameters(
 			bool fromDepth,
 			int downsampleStep, // decimation of the depth image in case the scan is from depth image
 			float rangeMin,
 			float rangeMax,
 			float voxelSize,
 			int normalsK,
-			int normalsRadius,
-			float groundNormalsUp)
+			float normalsRadius,
+			float groundNormalsUp,
+			bool deskewing)
 {
+	_scanDeskewing = deskewing;
 	_scanFromDepth = fromDepth;
 	_scanDownsampleStep=downsampleStep;
 	_scanRangeMin = rangeMin;
@@ -284,34 +294,178 @@ void CameraThread::setScanParameters(
 	_scanForceGroundNormalsUp = groundNormalsUp;
 }
 
-bool CameraThread::odomProvided() const
+bool SensorCaptureThread::odomProvided() const
 {
-	return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided()));
+	if(_odomAsGt)
+	{
+		return false;
+	}
+	return _odomSensor != 0;
 }
 
-void CameraThread::mainLoopBegin()
+void SensorCaptureThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
+	if(_lidar)
+	{
+		_lidar->resetTimer();
+	}
+	else if(_camera)
+	{
+		_camera->resetTimer();
+	}
 	if(_imuFilter)
 	{
 		// In case we paused the camera and moved somewhere else, restart filtering.
 		_imuFilter->reset();
 	}
-	_camera->resetTimer();
 }
 
-void CameraThread::mainLoop()
+void SensorCaptureThread::mainLoop()
 {
+	UASSERT(_lidar || _camera);
 	UTimer totalTime;
-	CameraInfo info;
-	SensorData data = _camera->takeImage(&info);
-
-	if(_odomSensor)
+	SensorCaptureInfo info;
+	SensorData data;
+	SensorData cameraData;
+	double lidarStamp = 0.0;
+	double cameraStamp = 0.0;
+	if(_lidar)
 	{
+		data = _lidar->takeData(&info);
+		if(data.stamp() == 0.0)
+		{
+			UERROR("Could not capture scan! Skipping this frame!");
+            return;
+		}
+		else
+		{
+			lidarStamp = data.stamp();
+			if(_camera)
+			{
+				cameraData = _camera->takeData();
+				if(cameraData.stamp() == 0.0)
+				{
+					UERROR("Could not capture image! Skipping this frame!");
+                    return;
+				}
+				else
+				{
+					double stampStart = UTimer::now();
+					while(cameraData.stamp() < data.stamp() &&
+						!isKilled() &&
+						UTimer::now() - stampStart < _poseWaitTime &&
+						!cameraData.imageRaw().empty())
+					{
+						// Make sure the camera frame is newer than lidar frame so
+						// that if there are imus published by the cameras, we can get
+						// them all in odometry before deskewing.
+						cameraData = _camera->takeData();
+					}
+
+					cameraStamp = cameraData.stamp();
+					if(cameraData.stamp() < data.stamp())
+					{
+						UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.",
+								cameraData.stamp(),
+								data.stamp(),
+								_poseWaitTime);
+					}
+
+					if(!cameraData.stereoCameraModels().empty())
+					{
+						data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true);
+					}
+					else
+					{
+						data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true);
+					}
+				}
+			}
+		}
+	}
+	else if(_camera)
+	{
+		data = _camera->takeData(&info);
+		if(data.stamp() == 0.0)
+		{
+			UERROR("Could not capture image! Skipping this frame!");
+            return;
+		}
+		else
+		{
+			cameraStamp = cameraData.stamp();
+		}
+	}
+
+	if(_odomSensor && data.stamp() != 0.0)
+	{
+		if(lidarStamp!=0.0 && _scanDeskewing)
+		{
+			UDEBUG("Deskewing begin");
+			if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime())
+			{
+				float scanTime =
+					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
+					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+
+				Transform poseFirstScan;
+				Transform poseLastScan;
+				cv::Mat cov;
+				double firstStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+				double lastStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()];
+				if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) &&
+				   _odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0))
+				{
+					if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
+					{
+						poseFirstScan.x() *= _poseScaleFactor;
+						poseFirstScan.y() *= _poseScaleFactor;
+						poseFirstScan.z() *= _poseScaleFactor;
+						poseLastScan.x() *= _poseScaleFactor;
+						poseLastScan.y() *= _poseScaleFactor;
+						poseLastScan.z() *= _poseScaleFactor;
+					}
+
+					UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull());
+
+					Transform transform = poseFirstScan.inverse() * poseLastScan;
+
+					// convert to velocity
+					float x,y,z,roll,pitch,yaw;
+					transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+					x/=scanTime;
+					y/=scanTime;
+					z/=scanTime;
+					roll /= scanTime;
+					pitch /= scanTime;
+					yaw /= scanTime;
+
+					Transform velocity(x,y,z,roll,pitch,yaw);
+					UTimer timeDeskewing;
+					LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
+					info.timeDeskewing = timeDeskewing.ticks();
+					if(!scanDeskewed.isEmpty())
+					{
+						data.setLaserScan(scanDeskewed);
+					}
+				}
+				else
+				{
+					UWARN("Failed to get poses for stamps %f and %f! Skipping this frame!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset);
+                    return;
+				}
+			}
+			else if(!data.laserScanRaw().empty())
+			{
+				UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str());
+			}
+			UDEBUG("Deskewing end");
+		}
+
 		Transform pose;
-		Transform poseToLeftCam;
 		cv::Mat covariance;
-		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance))
+		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
 		{
 			info.odomPose = pose;
 			info.odomCovariance = covariance;
@@ -321,21 +475,47 @@ void CameraThread::mainLoop()
 				info.odomPose.y() *= _poseScaleFactor;
 				info.odomPose.z() *= _poseScaleFactor;
 			}
-			// Adjust local transform of the camera based on the pose frame
-			if(!data.cameraModels().empty())
+
+			if(cameraStamp != 0.0)
 			{
-				UASSERT(data.cameraModels().size()==1);
-				CameraModel model = data.cameraModels()[0];
-				model.setLocalTransform(_extrinsicsOdomToCamera);
-				data.setCameraModel(model);
-			}
-			else if(!data.stereoCameraModels().empty())
-			{
-				UASSERT(data.stereoCameraModels().size()==1);
-				StereoCameraModel model = data.stereoCameraModels()[0];
-				model.setLocalTransform(_extrinsicsOdomToCamera);
-				data.setStereoCameraModel(model);
+				Transform cameraCorrection = Transform::getIdentity();
+				if(lidarStamp > 0.0 && lidarStamp != cameraStamp)
+				{
+					if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
+					{
+						cameraCorrection = info.odomPose.inverse() * pose;
+					}
+					else
+					{
+						UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp);
+					}
+				}
+
+				// Adjust local transform of the camera based on the pose frame
+				if(!data.cameraModels().empty())
+				{
+					UASSERT(data.cameraModels().size()==1);
+					CameraModel model = data.cameraModels()[0];
+					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
+					data.setCameraModel(model);
+				}
+				else if(!data.stereoCameraModels().empty())
+				{
+					UASSERT(data.stereoCameraModels().size()==1);
+					StereoCameraModel model = data.stereoCameraModels()[0];
+					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
+					data.setStereoCameraModel(model);
+				}
 			}
+
+			// Fake IMU to intialize gravity (assuming pose is aligned with gravity!)
+			Eigen::Quaterniond q = info.odomPose.getQuaterniond();
+			data.setIMU(IMU(
+					cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(),
+					cv::Vec3d(), cv::Mat(),
+					cv::Vec3d(), cv::Mat(),
+					Transform::getIdentity()));
+			this->disableIMUFiltering();
 		}
 		else
 		{
@@ -352,19 +532,19 @@ void CameraThread::mainLoop()
 	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
-		info.cameraName = _camera->getSerial();
+		info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
 		info.timeTotal = totalTime.ticks();
-		this->post(new CameraEvent(data, info));
+		this->post(new SensorEvent(data, info));
 	}
 	else if(!this->isKilled())
 	{
-		UWARN("no more images...");
+		UWARN("no more data...");
 		this->kill();
-		this->post(new CameraEvent());
+		this->post(new SensorEvent());
 	}
 }
 
-void CameraThread::mainLoopKill()
+void SensorCaptureThread::mainLoopKill()
 {
 	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
 	{
@@ -389,7 +569,7 @@ void CameraThread::mainLoopKill()
 	}
 }
 
-void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
+void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const
 {
 	UASSERT(dataPtr!=0);
 	SensorData & data = *dataPtr;

corelib/src/camera/CameraDepthAI.cpp

@@ -638,7 +638,7 @@ std::string CameraDepthAI::getSerial() const
 	return "";
 }
 
-SensorData CameraDepthAI::captureImage(CameraInfo * info)
+SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_DEPTHAI

corelib/src/camera/CameraFreenect.cpp

@@ -418,7 +418,7 @@ std::string CameraFreenect::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect::captureImage(CameraInfo * info)
+SensorData CameraFreenect::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT

corelib/src/camera/CameraFreenect2.cpp

@@ -334,7 +334,7 @@ std::string CameraFreenect2::getSerial() const
 	return "";
 }
 
-SensorData CameraFreenect2::captureImage(CameraInfo * info)
+SensorData CameraFreenect2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT2

corelib/src/camera/CameraImages.cpp

@@ -670,7 +670,7 @@ std::vector<std::string> CameraImages::filenames() const
 	return std::vector<std::string>();
 }
 
-SensorData CameraImages::captureImage(CameraInfo * info)
+SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 {
 	if(_syncImageRateWithStamps && _captureDelay>0.0)
 	{

corelib/src/camera/CameraK4A.cpp

@@ -424,7 +424,7 @@ std::string CameraK4A::getSerial() const
 #endif
 }
 
-SensorData CameraK4A::captureImage(CameraInfo * info)
+SensorData CameraK4A::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraK4W2.cpp

@@ -278,7 +278,7 @@ std::string CameraK4W2::getSerial() const
 	return "";
 }
 
-SensorData CameraK4W2::captureImage(CameraInfo * info)
+SensorData CameraK4W2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraMyntEye.cpp

@@ -598,7 +598,7 @@ void CameraMyntEye::getPoseAndIMU(
 }
 #endif
 
-SensorData CameraMyntEye::captureImage(CameraInfo * info)
+SensorData CameraMyntEye::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_MYNTEYE

corelib/src/camera/CameraOpenNI2.cpp

@@ -473,7 +473,7 @@ std::string CameraOpenNI2::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenNI2::captureImage(CameraInfo * info)
+SensorData CameraOpenNI2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_OPENNI2

corelib/src/camera/CameraOpenNICV.cpp

@@ -105,7 +105,7 @@ bool CameraOpenNICV::isCalibrated() const
 	return true;
 }
 
-SensorData CameraOpenNICV::captureImage(CameraInfo * info)
+SensorData CameraOpenNICV::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 	if(_capture.isOpened())

corelib/src/camera/CameraOpenni.cpp

@@ -182,7 +182,7 @@ std::string CameraOpenni::getSerial() const
 	return "";
 }
 
-SensorData CameraOpenni::captureImage(CameraInfo * info)
+SensorData CameraOpenni::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_OPENNI

corelib/src/camera/CameraRGBDImages.cpp

@@ -70,7 +70,7 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
 	return success;
 }
 
-SensorData CameraRGBDImages::captureImage(CameraInfo * info)
+SensorData CameraRGBDImages::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraRealSense.cpp

@@ -856,7 +856,7 @@ Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
 }
 #endif
 
-SensorData CameraRealSense::captureImage(CameraInfo * info)
+SensorData CameraRealSense::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_REALSENSE

corelib/src/camera/CameraRealSense2.cpp

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UStl.h>
 #include <opencv2/imgproc/types_c.h>
 
@@ -78,7 +77,6 @@ CameraRealSense2::CameraRealSense2(
 	cameraDepthHeight_(480),
 	cameraDepthFps_(30),
 	globalTimeSync_(true),
-	publishInterIMU_(false),
 	dualMode_(false),
 	closing_(false)
 #endif
@@ -138,12 +136,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
 {
 	auto stream = frame.get_profile().stream_type();
 	cv::Vec3f crnt_reading = *reinterpret_cast<const cv::Vec3f*>(frame.get_data());
-	UDEBUG("%s callback! %f (%f %f %f)",
-			stream == RS2_STREAM_GYRO?"GYRO":"ACC",
-			frame.get_timestamp(),
-			crnt_reading[0],
-			crnt_reading[1],
-			crnt_reading[2]);
+	//UDEBUG("%s callback! %f (%f %f %f)",
+	//		stream == RS2_STREAM_GYRO?"GYRO":"ACC",
+	//		frame.get_timestamp(),
+	//		crnt_reading[0],
+	//		crnt_reading[1],
+	//		crnt_reading[2]);
 	UScopeMutex sm(imuMutex_);
 	if(stream == RS2_STREAM_GYRO)
 	{
@@ -194,7 +192,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 
 void CameraRealSense2::frame_callback(rs2::frame frame)
 {
-	UDEBUG("Frame callback! %f", frame.get_timestamp());
+	//UDEBUG("Frame callback! %f", frame.get_timestamp());
 	syncer_(frame);
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -1139,14 +1137,14 @@ bool CameraRealSense2::odomProvided() const
 #endif
 }
 
-bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance)
+bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
 {
 #ifdef RTABMAP_REALSENSE2
 	IMU imu;
 	unsigned int confidence = 0;
 	double rsStamp = stamp*1000.0;
 	Transform p;
-	getPoseAndIMU(rsStamp, p, confidence, imu);
+	getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000);
 
 	if(!p.isNull())
 	{
@@ -1202,13 +1200,6 @@ void CameraRealSense2::setGlobalTimeSync(bool enabled)
 #endif
 }
 
-void CameraRealSense2::publishInterIMU(bool enabled)
-{
-#ifdef RTABMAP_REALSENSE2
-	publishInterIMU_ = enabled;
-#endif
-}
-
 void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 {
 #ifdef RTABMAP_REALSENSE2
@@ -1252,7 +1243,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled,
 #endif
 }
 
-SensorData CameraRealSense2::captureImage(CameraInfo * info)
+SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_REALSENSE2
@@ -1466,11 +1457,11 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
 				info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
 			}
-			if(!imu.empty() && !publishInterIMU_)
+			if(!imu.empty() && !isInterIMUPublishing())
 			{
 				data.setIMU(imu);
 			}
-			else if(publishInterIMU_ && !gyroBuffer_.empty())
+			else if(isInterIMUPublishing() && !gyroBuffer_.empty())
 			{
 				if(lastImuStamp_ > 0.0)
 				{
@@ -1501,7 +1492,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 						getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
 						if(!imuTmp.empty())
 						{
-							UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
+							this->postInterIMU(imuTmp, stamps[i]/1000.0);
 							pub++;
 						}
 						else

corelib/src/camera/CameraStereoDC1394.cpp

@@ -396,7 +396,7 @@ std::string CameraStereoDC1394::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
+SensorData CameraStereoDC1394::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_DC1394

corelib/src/camera/CameraStereoFlyCapture2.cpp

@@ -260,7 +260,7 @@ std::string CameraStereoFlyCapture2::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
+SensorData CameraStereoFlyCapture2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FLYCAPTURE2

corelib/src/camera/CameraStereoImages.cpp

@@ -156,7 +156,7 @@ std::string CameraStereoImages::getSerial() const
 	return stereoModel_.name();
 }
 
-SensorData CameraStereoImages::captureImage(CameraInfo * info)
+SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraStereoTara.cpp

@@ -136,7 +136,7 @@ std::string CameraStereoTara::getSerial() const
 	return cameraName_;
 }
 
-SensorData CameraStereoTara::captureImage(CameraInfo * info)
+SensorData CameraStereoTara::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraStereoVideo.cpp

@@ -243,7 +243,7 @@ std::string CameraStereoVideo::getSerial() const
 	return cameraName_;
 }
 
-SensorData CameraStereoVideo::captureImage(CameraInfo * info)
+SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 

corelib/src/camera/CameraStereoZed.cpp

@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraStereoZed.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
 
 #ifdef RTABMAP_ZED
@@ -167,12 +166,13 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr
 class ZedIMUThread: public UThread
 {
 public:
-	ZedIMUThread(float rate, sl::Camera * zed, const Transform & imuLocalTransform, bool accurate)
+	ZedIMUThread(float rate, sl::Camera * zed, CameraStereoZed * camera, const Transform & imuLocalTransform, bool accurate)
 	{
 		UASSERT(rate > 0.0f);
-		UASSERT(zed != 0);
+		UASSERT(zed != 0 && camera != 0);
 		rate_ = rate;
 		zed_= zed;
+		camera_ = camera;
 		accurate_ = accurate;
 		imuLocalTransform_ = imuLocalTransform;
 	}
@@ -212,19 +212,20 @@ private:
 		bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
 		if(res == sl::SUCCESS && imudata.valid)
 		{
-			UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()));
+			this->postInterIMU(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now());
 		}
 #else
         sl::SensorsData sensordata;
-        sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE);
+        sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT);
         if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available)
         {
-            UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now()));
+        	camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp)/10e9);
         }
 #endif
 	}
 	float rate_;
 	sl::Camera * zed_;
+	CameraStereoZed * camera_;
 	bool accurate_;
 	Transform imuLocalTransform_;
 	UTimer frameRateTimer_;
@@ -279,7 +280,6 @@ CameraStereoZed::CameraStereoZed(
 	computeOdometry_(computeOdometry),
 	lost_(true),
 	force3DoF_(odomForce3DoF),
-	publishInterIMU_(false),
 	imuPublishingThread_(0)
 #endif
 {
@@ -345,7 +345,6 @@ CameraStereoZed::CameraStereoZed(
 	computeOdometry_(computeOdometry),
 	lost_(true),
 	force3DoF_(odomForce3DoF),
-	publishInterIMU_(false),
 	imuPublishingThread_(0)
 #endif
 {
@@ -386,13 +385,6 @@ CameraStereoZed::~CameraStereoZed()
 #endif
 }
 
-void CameraStereoZed::publishInterIMU(bool enabled)
-{
-#ifdef RTABMAP_ZED
-	publishInterIMU_ = enabled;
-#endif
-}
-
 bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -564,9 +556,9 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 			imuLocalTransform_.prettyPrint().c_str(),
 			zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
 #endif
-		if(publishInterIMU_)
+		if(isInterIMUPublishing())
 		{
-			imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
+			imuPublishingThread_ = new ZedIMUThread(200, zed_, this, imuLocalTransform_, true);
 			imuPublishingThread_->start();
 		}
 	}
@@ -607,7 +599,7 @@ bool CameraStereoZed::odomProvided() const
 #endif
 }
 
-bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance)
+bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
 {
 #ifdef RTABMAP_ZED
 
@@ -683,7 +675,7 @@ bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covarian
 	return false;
 }
 
-SensorData CameraStereoZed::captureImage(CameraInfo * info)
+SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZED
@@ -711,10 +703,12 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 #else
 
 		sl::ERROR_CODE res;
+        sl::Timestamp timestamp;
 		bool imuReceived = true;
 		do
 		{
 			res = zed_->grab(rparam);
+			timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE);
 
 			// If the sensor supports IMU, wait IMU to be available before sending data.
 			if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
@@ -752,8 +746,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
                 zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH);
 #endif
 				slMat2cvMat(tmp).copyTo(depth);
-
+#if ZED_SDK_MAJOR_VERSION < 3
 				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+#else
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), double(timestamp)/10e9);
+#endif
 			}
 			else
 			{
@@ -766,8 +763,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 				cv::Mat rgbaRight = slMat2cvMat(tmp);
 				cv::Mat right;
 				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
-			
+#if ZED_SDK_MAJOR_VERSION < 3
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+#else
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), double(timestamp)/10e9);
+#endif
 			}
 
 			if(imuPublishingThread_ == 0)
@@ -803,6 +803,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 					info->odomPose = zedPoseToTransform(pose);
 					if (!info->odomPose.isNull())
 					{
+#if ZED_SDK_MAJOR_VERSION >=3
+						if(pose.timestamp != timestamp)
+						{
+							UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp);
+						}
+#endif
+
 						//transform from:
 						// x->right, y->down, z->forward
 						//to:
@@ -858,4 +865,9 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 	return data;
 }
 
+void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
+{
+	postInterIMU(imu, stamp);
+}
+
 } // namespace rtabmap

corelib/src/camera/CameraStereoZedOC.cpp

@@ -710,7 +710,7 @@ std::string CameraStereoZedOC::getSerial() const
 	return "";
 }
 
-SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
+SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_ZEDOC

corelib/src/camera/CameraVideo.cpp

@@ -162,7 +162,7 @@ std::string CameraVideo::getSerial() const
 	return _guid;
 }
 
-SensorData CameraVideo::captureImage(CameraInfo * info)
+SensorData CameraVideo::captureImage(SensorCaptureInfo * info)
 {
 	cv::Mat img;
 	if(_capture.isOpened())

corelib/src/lidar/LidarVLP16.cpp

@@ -0,0 +1,378 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/lidar/LidarVLP16.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThread.h>
+#include <pcl/pcl_config.h>
+
+#if PCL_VERSION_COMPARE(<, 1, 9, 0)
+#define VLP_MAX_NUM_LASERS 16
+#define VLP_DUAL_MODE 0x39
+#endif
+
+namespace rtabmap {
+
+/** @brief Function used to check that hour assigned to timestamp in conversion is
+ * correct. Velodyne only returns time since the top of the hour, so if the computer clock
+ * and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong
+ * hour may be associated with the timestamp
+ *
+ * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
+ * Original license: BSD License 2.0
+ * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
+ *
+ * @param stamp timestamp recovered from velodyne
+ * @param nominal_stamp time coming from computer's clock
+ * @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments
+ * disagree by more than a half-hour.
+ */
+double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) {
+    const int HALFHOUR_TO_SEC = 1800;
+    double retval = stamp;
+    if (nominal_stamp > stamp) {
+        if (nominal_stamp - stamp > HALFHOUR_TO_SEC) {
+            retval = retval + 2*HALFHOUR_TO_SEC;
+        }
+    } else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) {
+        retval = retval - 2*HALFHOUR_TO_SEC;
+    }
+    return retval;
+}
+
+/*
+ * Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
+ * Original license: BSD License 2.0
+ * Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
+ */
+double rosTimeFromGpsTimestamp(const uint32_t data) {
+    const int HOUR_TO_SEC = 3600;
+    // time for each packet is a 4 byte uint
+    // It is the number of microseconds from the top of the hour
+    double time_nom = UTimer::now();
+    uint32_t cur_hour = time_nom / HOUR_TO_SEC;
+    double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000;
+    stamp = resolveHourAmbiguity(stamp, time_nom);
+    return stamp;
+}
+
+LidarVLP16::LidarVLP16(
+		const std::string& pcapFile,
+		bool organized,
+		bool stampLast,
+		float frameRate,
+		Transform localTransform) :
+	Lidar(frameRate, localTransform),
+	pcl::VLPGrabber(pcapFile),
+	timingOffsetsDualMode_(false),
+	startSweepTime_(0),
+	startSweepTimeHost_(0),
+	organized_(organized),
+	useHostTime_(false),
+	stampLast_(stampLast)
+{
+	UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str());
+}
+LidarVLP16::LidarVLP16(
+		const boost::asio::ip::address& ipAddress,
+		const std::uint16_t port,
+		bool organized,
+		bool useHostTime,
+		bool stampLast,
+		float frameRate,
+		Transform localTransform) :
+	Lidar(frameRate, localTransform),
+	pcl::VLPGrabber(ipAddress, port),
+	timingOffsetsDualMode_(false),
+	startSweepTime_(0),
+	startSweepTimeHost_(0),
+	organized_(organized),
+	useHostTime_(useHostTime),
+	stampLast_(stampLast)
+{
+	UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port);
+}
+
+LidarVLP16::~LidarVLP16()
+{
+	UDEBUG("Stopping lidar...");
+	stop();
+	scanReady_.release();
+	UDEBUG("Stopped lidar!");
+}
+
+void LidarVLP16::setOrganized(bool enable)
+{
+	organized_ = true;
+}
+
+bool LidarVLP16::init(const std::string &, const std::string &)
+{
+	UDEBUG("Init lidar");
+	if(isRunning())
+	{
+		UDEBUG("Stopping lidar...");
+		stop();
+		uSleep(2000); // make sure all callbacks are finished
+		UDEBUG("Stopped lidar!");
+	}
+	startSweepTime_ = 0.0;
+	startSweepTimeHost_ = 0.0;
+	accumulatedScans_.clear();
+	if(organized_)
+	{
+		accumulatedScans_.resize(16);
+	}
+	else
+	{
+		accumulatedScans_.resize(1);
+	}
+	buildTimings(false);
+	start();
+	UDEBUG("Lidar capture started");
+	return true;
+}
+
+/**
+ * Build a timing table for each block/firing. Stores in timing_offsets vector
+ */
+void LidarVLP16::buildTimings(bool dualMode)
+{
+	// vlp16
+	// timing table calculation, from velodyne user manual
+	timingOffsets_.resize(12);
+	for (size_t i=0; i < timingOffsets_.size(); ++i){
+		timingOffsets_[i].resize(32);
+	}
+	// constants
+	double full_firing_cycle = 55.296 * 1e-6; // seconds
+	double single_firing = 2.304 * 1e-6; // seconds
+	double dataBlockIndex, dataPointIndex;
+	// compute timing offsets
+	for (size_t x = 0; x < timingOffsets_.size(); ++x){
+		for (size_t y = 0; y < timingOffsets_[x].size(); ++y){
+			if (dualMode){
+				dataBlockIndex = (x - (x % 2)) + (y / 16);
+			}
+			else{
+				dataBlockIndex = (x * 2) + (y / 16);
+			}
+			dataPointIndex = y % 16;
+			//timing_offsets[block][firing]
+									timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
+		}
+	}
+	timingOffsetsDualMode_ = dualMode;
+}
+
+void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket)
+{
+	if (sizeof(HDLLaserReturn) != 3)
+		return;
+
+	double receivedHostTime = UTimer::now();
+	double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp);
+	if(startSweepTime_==0)
+	{
+		startSweepTime_ = packetStamp;
+		startSweepTimeHost_ = receivedHostTime;
+	}
+
+	bool dualMode = dataPacket->mode == VLP_DUAL_MODE;
+	if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode)
+	{
+		// reset everything
+		timingOffsets_.clear();
+		buildTimings(dualMode);
+		startSweepTime_ = packetStamp;
+		startSweepTimeHost_ = receivedHostTime;
+		for(size_t i=0; i<accumulatedScans_.size(); ++i)
+		{
+			accumulatedScans_[i].clear();
+		}
+	}
+
+	double interpolated_azimuth_delta;
+	std::uint8_t index = 1;
+	if (dualMode)
+	{
+		index = 2;
+	}
+	if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
+	{
+		interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
+	}
+	else
+	{
+		interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
+	}
+
+	for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i)
+	{
+		HDLFiringData firing_data = dataPacket->firingData[i];
+
+		for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++)
+		{
+			double current_azimuth = firing_data.rotationalPosition;
+			if (j >= VLP_MAX_NUM_LASERS)
+			{
+				current_azimuth += interpolated_azimuth_delta;
+			}
+			if (current_azimuth > 36000)
+			{
+				current_azimuth -= 36000;
+			}
+
+			double t = 0;
+			if (timingOffsets_.size())
+			  t = timingOffsets_[i][j];
+
+			if (current_azimuth < HDLGrabber::last_azimuth_)
+			{
+				if (!accumulatedScans_[0].empty())
+				{
+					UScopeMutex lock(lastScanMutex_);
+					bool notify = lastScan_.laserScanRaw().empty();
+					if(stampLast_)
+					{
+						double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t;
+						double diff = lastStamp - startSweepTime_;
+						lastScan_.setStamp(useHostTime_?startSweepTimeHost_+diff:lastStamp);
+						for(size_t r=0; r<accumulatedScans_.size(); ++r)
+						{
+							for(size_t k=0; k<accumulatedScans_[r].size(); ++k)
+							{
+								accumulatedScans_[r][k].t -= diff;
+							}
+						}
+					}
+					else
+					{
+						lastScan_.setStamp(useHostTime_?startSweepTimeHost_:startSweepTime_);
+					}
+					if(accumulatedScans_.size() > 1)
+					{
+						cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone();
+						for(size_t k=1; k<accumulatedScans_.size(); ++k)
+						{
+							UASSERT((int)accumulatedScans_[k].size() == organizedScan.cols);
+							organizedScan.push_back(cv::Mat(1, accumulatedScans_[k].size(), CV_32FC(5), accumulatedScans_[k].data()).clone());
+						}
+						lastScan_.setLaserScan(LaserScan(organizedScan, 0, 0, LaserScan::kXYZIT, getLocalTransform()));
+					}
+					else
+					{
+						lastScan_.setLaserScan(LaserScan(cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(), 0, 0, LaserScan::kXYZIT, getLocalTransform()));
+					}
+					if(notify)
+					{
+						scanReady_.release();
+					}
+
+					startSweepTime_ = packetStamp + t;
+					startSweepTimeHost_ = receivedHostTime + t;
+				}
+				for(size_t k=0; k<accumulatedScans_.size(); ++k)
+				{
+					accumulatedScans_[k].clear();
+				}
+			}
+
+			double timeSinceStartOfThisScan = packetStamp + t - startSweepTime_;
+
+			pcl::PointXYZI xyzi;
+			HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
+			PointXYZIT xyzit;
+			xyzit.x = xyzi.y;
+			xyzit.y = -xyzi.x;
+			xyzit.z = xyzi.z;
+			xyzit.i = xyzi.intensity;
+			xyzit.t = timeSinceStartOfThisScan;
+
+			if(accumulatedScans_.size()>1)
+			{
+				accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit);
+			}
+			else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z)))
+			{
+				accumulatedScans_[0].push_back (xyzit);
+			}
+			last_azimuth_ = current_azimuth;
+
+			if (dualMode)
+			{
+				pcl::PointXYZI dual_xyzi;
+				HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
+
+				if(accumulatedScans_.size()>1)
+				{
+					xyzit.x = dual_xyzi.y;
+					xyzit.y = -dual_xyzi.x;
+					xyzit.z = dual_xyzi.z;
+					xyzit.i = dual_xyzi.intensity;
+					xyzit.t = timeSinceStartOfThisScan;
+					accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit);
+				}
+				else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z)
+						&& ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z)))
+				{
+					xyzit.x = dual_xyzi.y;
+					xyzit.y = -dual_xyzi.x;
+					xyzit.z = dual_xyzi.z;
+					xyzit.i = dual_xyzi.intensity;
+					xyzit.t = timeSinceStartOfThisScan;
+					accumulatedScans_[0].push_back (xyzit);
+				}
+			}
+		}
+		if (dualMode)
+		{
+			i++;
+		}
+	}
+}
+
+SensorData LidarVLP16::captureData(SensorCaptureInfo * info)
+{
+	SensorData data;
+	if(scanReady_.acquire(1, 5000))
+	{
+		UScopeMutex lock(lastScanMutex_);
+		if(!lastScan_.laserScanRaw().empty())
+		{
+			data = lastScan_;
+			lastScan_ = SensorData();
+		}
+	}
+	else
+	{
+		UWARN("Did not receive any scans for the past 5 seconds.");
+	}
+	return data;
+}
+
+
+} /* namespace rtabmap */

corelib/src/odometry/OdometryF2M.cpp

@@ -156,11 +156,14 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	regPipeline_ = Registration::create(bundleParameters);
 	if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
 	{
-		UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
-				Parameters::kOdomF2MBundleAdjustment().c_str(),
-				bundleAdjustment_,
-				Parameters::kRegStrategy().c_str(),
-				uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
+		if(regPipeline_->isImageRequired())
+		{
+			UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
+					Parameters::kOdomF2MBundleAdjustment().c_str(),
+					bundleAdjustment_,
+					Parameters::kRegStrategy().c_str(),
+					uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
+		}
 		bundleAdjustment_ = 0;
 	}
 

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -975,10 +975,6 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh
     visible_faces.resize (cpt_visible_faces);
     mesh.tex_polygons[current_cam].clear ();
     mesh.tex_polygons[current_cam] = visible_faces;
-
-    int nb_faces = 0;
-    for (int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++)
-      nb_faces += static_cast<int> (mesh.tex_polygons[i].size ());
   }
 
   // we have been through all the cameras.

corelib/src/util3d.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
@@ -2238,7 +2239,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
@@ -2268,8 +2269,17 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.size(); ++i)
@@ -2286,8 +2296,17 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & lase
 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
 {
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	for(int i=0; i<laserScan.size(); ++i)
 	{
@@ -2303,8 +2322,17 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserSc
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.size(); ++i)
@@ -2321,8 +2349,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan
 pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform,  float intensity)
 {
 	pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.size(); ++i)
@@ -2339,8 +2376,17 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & la
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	for(int i=0; i<laserScan.size(); ++i)
 	{
@@ -2356,8 +2402,17 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
 pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform,  float intensity)
 {
 	pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
+	if(laserScan.isOrganized())
+	{
+		output->width = laserScan.data().cols;
+		output->height = laserScan.data().rows;
+		output->is_dense = false;
+	}
+	else
+	{
+		output->is_dense = true;
+	}
 	output->resize(laserScan.size());
-	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	for(int i=0; i<laserScan.size(); ++i)
 	{
@@ -2374,7 +2429,8 @@ pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointXYZ output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -2388,7 +2444,8 @@ pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointNormal output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -2409,7 +2466,8 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointXYZRGB output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -2448,7 +2506,8 @@ pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float i
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointXYZI output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -2473,7 +2532,8 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointXYZRGBNormal output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -2520,7 +2580,8 @@ pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int in
 {
 	UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
 	pcl::PointXYZINormal output;
-	const float * ptr = laserScan.data().ptr<float>(0, index);
+	int row = index / laserScan.data().cols;
+	const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
 	output.x = ptr[0];
 	output.y = ptr[1];
 	if(!laserScan.is2d())
@@ -3489,6 +3550,162 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
 	return util3d::transformPointCloud(cloud, transform);
 }
 
+LaserScan deskew(
+		const LaserScan & input,
+		double inputStamp,
+		const rtabmap::Transform & velocity)
+{
+	if(velocity.isNull())
+	{
+		UERROR("velocity should be valid!");
+		return LaserScan();
+	}
+
+	if(input.format() != LaserScan::kXYZIT)
+	{
+		UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
+		return LaserScan();
+	}
+
+	if(input.empty())
+	{
+		UERROR("input scan is empty!");
+		return LaserScan();
+	}
+
+	int offsetTime = input.getTimeOffset();
+
+	// Get latest timestamp
+	double firstStamp;
+	double lastStamp;
+	firstStamp = inputStamp + input.data().ptr<float>(0, 0)[offsetTime];
+	lastStamp = inputStamp + input.data().ptr<float>(0, input.size()-1)[offsetTime];
+
+	if(lastStamp <= firstStamp)
+	{
+		UERROR("First and last stamps in the scan are the same!");
+		return LaserScan();
+	}
+
+	rtabmap::Transform firstPose;
+	rtabmap::Transform lastPose;
+
+	float vx,vy,vz, vroll,vpitch,vyaw;
+	velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+
+	//  1- The pose of base frame in odom frame at first stamp
+	//  2- The pose of base frame in odom frame at last stamp
+	double dt1 = firstStamp - inputStamp;
+	double dt2 = lastStamp - inputStamp;
+
+	firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
+	lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
+
+	if(firstPose.isNull())
+	{
+		UERROR("Could not get transform between stamps %f and %f!",
+				firstStamp,
+				inputStamp);
+		return LaserScan();
+	}
+	if(lastPose.isNull())
+	{
+		UERROR("Could not get transform between stamps %f and %f!",
+				lastStamp,
+				inputStamp);
+		return LaserScan();
+	}
+
+	double stamp;
+	UTimer processingTime;
+	double scanTime = lastStamp - firstStamp;
+	cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
+	int offsetIntensity = input.getIntensityOffset();
+	bool isLocalTransformIdentity = input.localTransform().isIdentity();
+	Transform localTransformInv = input.localTransform().inverse();
+
+	bool timeOnColumns = input.data().cols > input.data().rows;
+	int oi = 0;
+	if(timeOnColumns)
+	{
+		// t1     t2    ...
+		// ring1  ring1 ...
+		// ring2  ring2 ...
+		// ring3  ring4 ...
+		// ring4  ring3 ...
+		for(int u=0; u<input.data().cols; ++u)
+		{
+			const float * inputPtr = input.data().ptr<float>(0, u);
+			stamp = inputStamp + inputPtr[offsetTime];
+			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+
+			for(int v=0; v<input.data().rows; ++v)
+			{
+				inputPtr = input.data().ptr<float>(v, u);
+				pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
+				if(pcl::isFinite(pt))
+				{
+					if(!isLocalTransformIdentity)
+					{
+						pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
+					}
+					pt = rtabmap::util3d::transformPoint(pt, transform);
+					if(!isLocalTransformIdentity)
+					{
+						pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
+					}
+					float * dataPtr = output.ptr<float>(0, oi++);
+					dataPtr[0] = pt.x;
+					dataPtr[1] = pt.y;
+					dataPtr[2] = pt.z;
+					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+				}
+			}
+		}
+	}
+	else // time on rows
+	{
+		// t1     ring1 ring2 ring3 ring4
+		// t2     ring1 ring2 ring3 ring4
+		// t3     ring1 ring2 ring3 ring4
+		// t4     ring1 ring2 ring3 ring4
+		// ...    ...   ...   ...   ...
+		for(int v=0; v<input.data().rows; ++v)
+		{
+			const float * inputPtr = input.data().ptr<float>(v, 0);
+			stamp = inputStamp + inputPtr[offsetTime];
+			rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
+
+			for(int u=0; u<input.data().cols; ++u)
+			{
+				inputPtr = input.data().ptr<float>(v, u);
+				pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
+				if(pcl::isFinite(pt))
+				{
+					if(!isLocalTransformIdentity)
+					{
+						pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
+					}
+					pt = rtabmap::util3d::transformPoint(pt, transform);
+					if(!isLocalTransformIdentity)
+					{
+						pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
+					}
+					float * dataPtr = output.ptr<float>(0, oi++);
+					dataPtr[0] = pt.x;
+					dataPtr[1] = pt.y;
+					dataPtr[2] = pt.z;
+					dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
+				}
+			}
+		}
+	}
+	output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
+	UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
+	return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
+}
+
+
 }
 
 }

corelib/src/util3d_filtering.cpp

@@ -82,8 +82,8 @@ LaserScan commonFiltering(
 		float groundNormalsUp)
 {
 	LaserScan scan = scanIn;
-	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
-			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
+	UDEBUG("scan size=%d format=%d, organized=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
+			scan.size(), (int)scan.format(), scan.isOrganized()?1:0, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
 	if(!scan.isEmpty())
 	{
 		// combined downsampling and range filtering step
@@ -99,34 +99,44 @@ LaserScan commonFiltering(
 			int oi = 0;
 			float rangeMinSqrd = rangeMin * rangeMin;
 			float rangeMaxSqrd = rangeMax * rangeMax;
-			for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
+			int downsamplingRows = scan.data().rows > scan.data().cols?downsamplingStep:1;
+			int downsamplingCols = scan.data().cols > scan.data().rows?downsamplingStep:1;
+			for(int j=0; j<scan.data().rows-downsamplingRows+1; j+=downsamplingRows)
 			{
-				const float * ptr = scan.data().ptr<float>(0, i);
-
-				if(rangeMin>0.0f || rangeMax>0.0f)
+				for(int i=0; i<scan.data().cols-downsamplingCols+1; i+=downsamplingCols)
 				{
-					float r;
-					if(is2d)
+					const float * ptr = scan.data().ptr<float>(j, i);
+
+					if(rangeMin>0.0f || rangeMax>0.0f)
 					{
-						r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
-					}
-					else
-					{
-						r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
+						float r;
+						if(is2d)
+						{
+							r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
+						}
+						else
+						{
+							r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
+						}
+
+						if(!uIsFinite(r))
+						{
+							continue;
+						}
+
+						if(rangeMin > 0.0f && r < rangeMinSqrd)
+						{
+							continue;
+						}
+						if(rangeMax > 0.0f && r > rangeMaxSqrd)
+						{
+							continue;
+						}
 					}
 
-					if(rangeMin > 0.0f && r < rangeMinSqrd)
-					{
-						continue;
-					}
-					if(rangeMax > 0.0f && r > rangeMaxSqrd)
-					{
-						continue;
-					}
+					cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
+					++oi;
 				}
-
-				cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
-				++oi;
 			}
 			int previousSize = scan.size();
 			int scanMaxPtsTmp = scan.maxPoints();

corelib/src/util3d_mapping.cpp

@@ -655,7 +655,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 		map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
 		UDEBUG("map size = %dx%d", map.cols, map.rows);
 
-		int j=0;
 		float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
 		for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
 		{
@@ -741,14 +740,12 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 					}
 				}
 			}
-			++j;
 		}
 		UDEBUG("Ray trace known space=%fs", timer.ticks());
 
 		// now fill unknown spaces
 		if(unknownSpaceFilled && scanMaxRange > 0)
 		{
-			j=0;
 			float angleIncrement = CV_PI/90.0f; // angle increment
 			for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
 			{
@@ -813,7 +810,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 						}
 					}
 				}
-				++j;
 			}
 			UDEBUG("Fill empty space=%fs", timer.ticks());
 			//cv::imwrite("map.png", util3d::convertMap2Image8U(map));

corelib/src/util3d_surface.cpp

@@ -3518,22 +3518,25 @@ LaserScan adjustNormalsToViewPoint(
 		int nz = ny+1;
 		cv::Mat output = scan.data().clone();
 		#pragma omp parallel for
-		for(int i=0; i<scan.size(); ++i)
+		for(int j=0; j<scan.data().rows; ++j)
 		{
-			float * ptr = output.ptr<float>(0, i);
-			if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
+			for(int i=0; i<scan.data().cols; ++i)
 			{
-				Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
-				Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
-
-				float result = v.dot(n);
-				if(result < 0
-				 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+				float * ptr = output.ptr<float>(j, i);
+				if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
 				{
-					//reverse normal
-					ptr[nx] *= -1.0f;
-					ptr[ny] *= -1.0f;
-					ptr[nz] *= -1.0f;
+					Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
+					Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
+
+					float result = v.dot(n);
+					if(result < 0
+					 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+					{
+						//reverse normal
+						ptr[nx] *= -1.0f;
+						ptr[ny] *= -1.0f;
+						ptr[nz] *= -1.0f;
+					}
 				}
 			}
 		}