Fixed Semaphore::value() on Windows.

CMakeLists.txt

@@ -205,6 +205,7 @@ option(WITH_REALSENSE2    "Include RealSense support"            ON)
 option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_DEPTHAI       "Include depthai-core support"         OFF)
 option(WITH_XVSDK         "Include XVisio SDK support"           OFF)
+option(WITH_OUSTER        "Include Ouster SDK support"           ON)
 option(WITH_OCTOMAP       "Include OctoMap support"              ON)
 option(WITH_GRIDMAP       "Include GridMap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              OFF)
@@ -671,6 +672,13 @@ IF(WITH_XVSDK)
     ENDIF(xvsdk_FOUND)
 ENDIF(WITH_XVSDK)
 
+IF(WITH_OUSTER)
+    FIND_PACKAGE(OusterSDK QUIET)
+    IF(OusterSDK_FOUND)
+       MESSAGE(STATUS "Found OusterSDK (targets)")
+    ENDIF(OusterSDK_FOUND)
+ENDIF(WITH_OUSTER)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -1018,6 +1026,9 @@ IF(NOT xvsdk_FOUND)
 ELSE()
    SET(CONF_WITH_XVSDK 1)
 ENDIF()
+IF(NOT OusterSDK_FOUND)
+   SET(OUSTER "//")
+ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
    SET(CONF_WITH_OCTOMAP 0)
@@ -1674,6 +1685,21 @@ ELSE()
 MESSAGE(STATUS "  With XVisio SDK           = NO (xvsdk not found)")
 ENDIF()
 
+MESSAGE(STATUS "")
+MESSAGE(STATUS " LiDAR Drivers:")
+IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8.0")
+MESSAGE(STATUS "  With Velodyne VLP16       = YES")
+ELSE()
+MESSAGE(STATUS "  With Velodyne VLP16       = NO (PCL>=1.8 required)")
+ENDIF()
+IF(OusterSDK_FOUND)
+MESSAGE(STATUS "  With Ouster SDK ${OusterSDK_VERSION}  = YES")
+ELSEIF(NOT WITH_OUSTER)
+MESSAGE(STATUS "  With Ouster SDK           = NO (WITH_OUSTER=OFF)")
+ELSE()
+MESSAGE(STATUS "  With Ouster SDK           = NO (OusterSDK not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)

Version.h.in

@@ -70,6 +70,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @MYNTEYE@#define RTABMAP_MYNTEYE
 @DEPTHAI@#define RTABMAP_DEPTHAI
 @XVSDK@#define RTABMAP_XVSDK
+@OUSTER@#define RTABMAP_OUSTER
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @GRIDMAP@#define RTABMAP_GRIDMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF

corelib/include/rtabmap/core/IMUFilter.h

@@ -41,7 +41,6 @@ public:
 		kMadgwick=0,
 		kComplementaryFilter=1};
 public:
-	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
 	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
 
 public:

corelib/include/rtabmap/core/lidar/LidarOuster.h

@@ -0,0 +1,72 @@
+/*
+Copyright (c) 2010-2024, 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.
+*/
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_
+
+#include <rtabmap/core/Lidar.h>
+
+namespace rtabmap {
+
+class IMUFilter;
+class OusterCaptureThread;
+
+class RTABMAP_CORE_EXPORT LidarOuster :public Lidar {
+public:
+	static bool available();
+public:
+	LidarOuster(
+		const std::string& ipOrHostnameOrPcapOrOsf,
+		const std::string& dataDestinationOrJson = "",
+		int lidarMode = 0,
+		int timestampMode = 0,
+		bool useReflectivityForIntensityChannel = true,
+		bool publishIMU = false,
+		float frameRate = 0.0f,
+		Transform localTransform = Transform::getIdentity());
+	virtual ~LidarOuster();
+
+	SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override;
+	virtual std::string getSerial() const override;
+
+protected:
+    virtual SensorData captureData(SensorCaptureInfo * info = 0) override;
+
+private:
+	OusterCaptureThread * ousterCaptureThread_;
+	bool imuPublished_;
+	bool useReflectivityForIntensityChannel_;
+	std::string ipOrHostnameOrPcapOrOsf_;
+	std::string dataDestinationOrJson_;
+	int lidarMode_;
+	int timestampMode_;
+
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDAROUSTER_H_ */

corelib/include/rtabmap/core/lidar/LidarVLP16.h

@@ -43,6 +43,8 @@ struct PointXYZIT {
 };
 
 class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber {
+public:
+	static bool available();
 public:
 	LidarVLP16(
 			const std::string& pcapFile,

corelib/src/CMakeLists.txt

@@ -41,6 +41,8 @@ SET(SRC_FILES
     camera/CameraMyntEye.cpp
     camera/CameraDepthAI.cpp
 	camera/CameraSeerSense.cpp
+
+	lidar/LidarOuster.cpp
     
     EpipolarGeometry.cpp
     VisualWord.cpp
@@ -391,6 +393,15 @@ IF(xvsdk_FOUND)
 	)
 ENDIF(xvsdk_FOUND)
 
+IF(OusterSDK_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		OusterSDK::ouster_client
+		OusterSDK::ouster_pcap
+		OusterSDK::ouster_osf
+	)
+ENDIF(OusterSDK_FOUND)
+
 IF(TARGET OpenMP::OpenMP_CXX)
 	SET(LIBRARIES
 		${LIBRARIES}

corelib/src/IMUFilter.cpp

@@ -35,13 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-IMUFilter * IMUFilter::create(const ParametersMap & parameters)
-{
-	int type = Parameters::defaultKpDetectorStrategy();
-	Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
-	return create((IMUFilter::Type)type, parameters);
-}
-
 IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parameters)
 {
 #ifndef RTABMAP_MADGWICK
@@ -62,7 +55,6 @@ IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parame
 #endif
 	default:
 		filter = new ComplementaryFilter(parameters);
-		type = IMUFilter::kComplementaryFilter;
 		break;
 
 	}

corelib/src/Odometry.cpp

@@ -646,7 +646,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		dt > 0 &&
 		!guess.isNull())
 	{
-		UDEBUG("Deskewing begin");
+		UDEBUG("Deskewing begin (with imu=%d)", !imus_.empty()?1:0);
 		// Recompute velocity
 		float vx,vy,vz, vroll,vpitch,vyaw;
 		guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
@@ -672,7 +672,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			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())
+			if(!imuFirstScan.isNull() || !imuLastScan.isNull())
 			{
 				Transform orientation = imuFirstScan.inverse() * imuLastScan;
 				orientation.getEulerAngles(vroll, vpitch, vyaw);
@@ -689,6 +689,18 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					vyaw /= scanTime;
 				}
 			}
+			if(imuFirstScan.isNull())
+			{
+				UWARN("We are receiving IMUs but we could not find one for the "
+				      "first lidar stamp %f to be used for deskewing.", 
+					  data.stamp() + data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
+			}
+			if(imuLastScan.isNull())
+			{
+				UWARN("We are receiving IMUs but we could not find one for the "
+				      "last lidar stamp %f to be used for deskewing.", 
+					  data.stamp() + data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
+			}
 		}
 
 		Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
@@ -703,6 +715,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	if(data.laserScanRaw().isOrganized())
 	{
 		// Laser scans should be dense passing this point
+		UDEBUG("Densify scan");
 		data.setLaserScan(data.laserScanRaw().densify());
 	}
 

corelib/src/Parameters.cpp

@@ -814,6 +814,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With Ouster SDK:";
+#ifdef RTABMAP_OUSTER
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With libpointmatcher:";
 #ifdef RTABMAP_POINTMATCHER

corelib/src/lidar/LidarOuster.cpp

@@ -0,0 +1,700 @@
+/*
+Copyright (c) 2010-2024, 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/Version.h>
+#ifdef RTABMAP_OUSTER
+// Ouster should be included first to avoid conflicts 
+// with defines set by Windows.h included indirectly below
+#include "ouster/client.h"
+#include "ouster/os_pcap.h"
+#include "ouster/types.h"
+#include "ouster/impl/build.h"
+#include "ouster/lidar_scan.h"
+#include "ouster/osf/reader.h"
+#include "ouster/osf/stream_lidar_scan.h"
+#include "ouster/osf/meta_lidar_sensor.h"
+#include "ouster/osf/meta_extrinsics.h"
+#include "ouster/osf/meta_streaming_info.h"
+#endif
+
+#include <rtabmap/core/lidar/LidarOuster.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/core/IMUFilter.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+
+namespace rtabmap {
+
+#ifdef RTABMAP_OUSTER
+class OusterCaptureThread : public UThread {
+  public:
+	OusterCaptureThread(
+		std::shared_ptr<ouster::sensor::client> & clientHandle,
+		const ouster::sensor::sensor_info & info,
+		bool imuPublished,
+		bool useReflectivityForIntensityChannel) :
+			scanBatcher_(info),
+			pf_(ouster::sensor::get_format(info)),
+			clientHandle_(clientHandle),
+			info_(info),
+			intensityChannel_(useReflectivityForIntensityChannel?
+				ouster::sensor::ChanField::REFLECTIVITY:
+				ouster::sensor::ChanField::SIGNAL),
+			endOfFileReached_(false)
+	{
+		UDEBUG("");
+		init(imuPublished);
+	}
+	OusterCaptureThread(
+		std::shared_ptr<ouster::sensor_utils::playback_handle> & pcapHandle,
+		const ouster::sensor::sensor_info & info,
+		bool imuPublished,
+		bool useReflectivityForIntensityChannel) :
+			scanBatcher_(info),
+			pf_(ouster::sensor::get_format(info)),
+			pcapHandle_(pcapHandle),
+			info_(info),
+			intensityChannel_(useReflectivityForIntensityChannel?
+				ouster::sensor::ChanField::REFLECTIVITY:
+				ouster::sensor::ChanField::SIGNAL),
+			endOfFileReached_(false)
+	{
+		UDEBUG("");
+		init(imuPublished);
+	}
+
+	OusterCaptureThread(
+		std::shared_ptr<ouster::osf::Reader> & osfReader,
+		const ouster::sensor::sensor_info & info,
+		bool imuPublished,
+		bool useReflectivityForIntensityChannel) :
+			scanBatcher_(info),
+			pf_(ouster::sensor::get_format(info)),
+			osfReader_(osfReader),
+			osfIter_(osfReader_->messages().begin()),
+			info_(info),
+			intensityChannel_(useReflectivityForIntensityChannel?
+				ouster::sensor::ChanField::REFLECTIVITY:
+				ouster::sensor::ChanField::SIGNAL),
+			endOfFileReached_(false)
+	{
+		UDEBUG("");
+		init(imuPublished);
+	}
+
+	virtual ~OusterCaptureThread()
+	{
+		this->join(true);
+		delete imuFilter_;
+	}
+
+	const ouster::sensor::sensor_info & getInfo() const {
+		return info_;
+	}
+
+	bool getScan(LaserScan & output, double & stamp)
+	{
+		if(clientHandle_.get() == 0)
+		{
+			// We are reading from file, call mainLoop till a scan is received or end of stream
+			while(scanReady_.value() == 0 && !endOfFileReached_)
+				this->mainLoop();
+		}
+	
+		if(!endOfFileReached_ && !scanReady_.acquire(1, 5000))
+		{
+			UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
+		}
+		else if(!endOfFileReached_ || this->isRunning())
+		{
+			ouster::LidarScan scan;
+			{
+				UScopeMutex s(scanMutex_);
+				scan = lastScan_;
+				lastScan_ = ouster::LidarScan();
+			}
+
+			UASSERT(scan.has_field(ouster::sensor::ChanField::RANGE));
+			UASSERT(scan.has_field(intensityChannel_));
+
+			ouster::LidarScan::Points cloud = ouster::cartesian(scan, lut_);
+
+			// Convert to our LaserScan format XYZIT
+			output = LaserScan(cv::Mat(cv::Size(scan.w,scan.h), CV_32FC(5)), LaserScan::kXYZIT, 0, 0, 0, 0, 0, lidarSensorT_);
+			auto timestamp = scan.timestamp();
+			auto scan_ts = timestamp[scan.w-1]; // stamp with last reading
+
+			//UWARN("Prepare scan %f -> %f", double(timestamp[0])/10e8, double(timestamp[scan.w-1])/10e8);
+			auto intensityOffset = output.getIntensityOffset();
+			auto timeOffset = output.getTimeOffset();
+			float bad_point = std::numeric_limits<float>::quiet_NaN ();
+			ouster::FieldType intensityType = scan.field_type(intensityChannel_==ouster::sensor::ChanField::REFLECTIVITY?"REFLECTIVITY":"SIGNAL");
+			for (size_t u = 0; u < scan.h; u++) {
+				for (size_t v = 0; v < scan.w; v++) {
+					auto ts = scan_ts-timestamp[v];
+					auto index = u*scan.w+v;
+					if(cloud(index, 0) != 0.0)
+					{
+						output.field(index, 0) = cloud(index, 0);
+						output.field(index, 1) = cloud(index, 1);
+						output.field(index, 2) = cloud(index, 2);
+					}
+					else
+					{
+						output.field(index, 0) = output.field(index, 1) = output.field(index, 2) = bad_point;
+					}
+					if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT32)
+					{
+						output.field(index, intensityOffset) = scan.field<uint32_t>(intensityChannel_).data()[index];
+					}
+					else if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT16)
+					{
+						output.field(index, intensityOffset) = scan.field<uint16_t>(intensityChannel_).data()[index];
+					}
+					else if(intensityType.element_type == ouster::sensor::ChanFieldType::UINT8)
+					{
+						output.field(index, intensityOffset) = scan.field<uint8_t>(intensityChannel_).data()[index];
+					}
+					else
+					{
+						UFATAL("Intensity format %d not supported!", intensityType.element_type);
+					}
+					output.field(index, timeOffset) = -float(ts)/10e8;
+				}
+			}
+			stamp = double(scan_ts)/10e8;
+			return true;
+		}
+		return false;
+	}
+
+	IMU getIMU(double stamp, int maxWaitTimeMs = 100)
+	{
+		IMU imu;
+		if(!imuBuffer_.empty())
+		{
+			imuMutex_.lock();
+			int waitTry = 0;
+			while(maxWaitTimeMs>0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+			{
+				imuMutex_.unlock();
+				++waitTry;
+				uSleep(1);
+				imuMutex_.lock();
+			}
+
+			if(imuBuffer_.rbegin()->first < stamp)
+			{
+				if(maxWaitTimeMs > 0)
+				{
+					UWARN("Could not find imus to interpolate at scan time %f after waiting %d ms (last is %f)...", stamp/1000.0, maxWaitTimeMs, imuBuffer_.rbegin()->first/1000.0);
+				}
+			}
+			else
+			{
+				std::map<double, IMU>::const_iterator iterB = imuBuffer_.lower_bound(stamp);
+				std::map<double, IMU>::const_iterator iterA = iterB;
+				if(iterA != imuBuffer_.begin())
+				{
+					iterA = --iterA;
+				}
+				if(iterB == imuBuffer_.end())
+				{
+					iterB = --iterB;
+				}
+				if(stamp >= iterA->first && stamp <= iterB->first)
+				{
+					float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+
+					cv::Vec4d quatA = iterA->second.orientation();
+					cv::Vec3d accA = iterA->second.linearAcceleration();
+					cv::Vec3d gyroA = iterA->second.angularVelocity();
+					cv::Vec4d quatB = iterB->second.orientation();
+					cv::Vec3d accB = iterB->second.linearAcceleration();
+					cv::Vec3d gyroB = iterB->second.angularVelocity();
+
+					Eigen::Quaterniond qa(quatA[3], quatA[0], quatA[1], quatA[2]);
+					Eigen::Quaterniond qb(quatB[3], quatB[0], quatB[1], quatB[2]);
+					Eigen::Quaterniond qres = qa.slerp(t, qb);
+
+					accA[0] = accA[0] + t*(accB[0] - accA[0]);
+					accA[1] = accA[1] + t*(accB[1] - accA[1]);
+					accA[2] = accA[2] + t*(accB[2] - accA[2]);
+
+					gyroA[0] = gyroA[0] + t*(gyroB[0] - gyroA[0]);
+					gyroA[1] = gyroA[1] + t*(gyroB[1] - gyroA[1]);
+					gyroA[2] = gyroA[2] + t*(gyroB[2] - gyroA[2]);
+
+					imu = IMU(cv::Vec4d(qres.x(), qres.y(), qres.z(), qres.w()), iterA->second.orientationCovariance(),
+							gyroA, iterA->second.angularVelocityCovariance(),
+							accA, iterA->second.linearAccelerationCovariance(),
+							imuLocalTransform_);
+				}
+			}
+			imuMutex_.unlock();
+		}
+		return imu;
+	}
+
+private:
+	void init(bool imuPublished)
+	{
+		imuFilter_ = 0;
+		
+		UINFO("imuPublished=%d", imuPublished?1:0);
+		size_t w = info_.format.columns_per_frame;
+		size_t h = info_.format.pixels_per_column;
+
+		lut_ = ouster::make_xyz_lut(info_);
+
+		// Buffer to store raw packet data
+		lidarPacket_ = ouster::sensor::LidarPacket (pf_.lidar_packet_size);
+		imuPacket_ = ouster::sensor::ImuPacket(pf_.imu_packet_size);
+
+		const std::array<ouster::FieldType, 2> reducedSlots{
+			{{ouster::sensor::ChanField::RANGE, ouster::sensor::ChanFieldType::UINT32},
+			{intensityChannel_, ouster::sensor::ChanFieldType::UINT32}}};
+		scanBuffer_ = ouster::LidarScan(w, h, reducedSlots.begin(), reducedSlots.end());
+
+		lidarSensorT_ = Transform::fromEigen4d(info_.lidar_to_sensor_transform);
+		lidarSensorT_.x() /= 1000;
+		lidarSensorT_.y() /= 1000;
+		lidarSensorT_.z() /= 1000;
+		UINFO("Lidar to sensor: %s", lidarSensorT_.prettyPrint().c_str());
+
+		if(imuPublished)
+		{
+			imuLocalTransform_ = Transform::fromEigen4d(info_.imu_to_sensor_transform);
+			imuLocalTransform_.x() /= 1000;
+			imuLocalTransform_.y() /= 1000;
+			imuLocalTransform_.z() /= 1000;
+			UINFO("IMU to sensor: %s", imuLocalTransform_.prettyPrint().c_str());
+
+			imuFilter_ = IMUFilter::create(IMUFilter::kMadgwick);
+		}
+	}
+
+	virtual void mainLoop()
+	{
+		bool isLidarPacket = false;
+		bool isImuPacket = false;
+		
+		if(clientHandle_)
+		{
+			// wait until sensor data is available
+			ouster::sensor::client_state st = ouster::sensor::poll_client(*clientHandle_);
+			// check for timeout
+			if (st == ouster::sensor::TIMEOUT){
+				UERROR("Client has timed out");
+				this->kill();
+				return;
+			}
+
+			if (st & ouster::sensor::EXIT){
+				UERROR("Exit was requested");
+				this->kill();
+				return;
+			}
+
+			// check for error status
+			if (st & ouster::sensor::CLIENT_ERROR) {
+				UERROR("Sensor client returned error state!");
+				this->kill();
+				return;
+			}
+
+			if (st & ouster::sensor::LIDAR_DATA) {
+				if (!ouster::sensor::read_lidar_packet(*clientHandle_, lidarPacket_)) {
+					UERROR("Failed to read a lidar packet of the expected size!");
+					this->kill();
+					return;
+				}
+				isLidarPacket = true;
+			}
+
+			if (imuFilter_ && st & ouster::sensor::IMU_DATA) {
+				if (!ouster::sensor::read_imu_packet(*clientHandle_, imuPacket_)) {
+					UERROR("Failed to read an imu packet of the expected size!");
+					this->kill();
+					return;
+				}
+				isImuPacket = true;
+			}
+		}
+		else if(pcapHandle_) //pcap
+		{
+			ouster::sensor_utils::packet_info packet_info;
+			if(!ouster::sensor_utils::next_packet_info(*pcapHandle_, packet_info))
+			{
+				UWARN("No more data");
+				endOfFileReached_ = true;
+				return;
+			}
+
+			if(packet_info.dst_port == info_.config.udp_port_lidar)
+			{
+				auto packet_size = ouster::sensor_utils::read_packet(
+					*pcapHandle_, lidarPacket_.buf.data(), lidarPacket_.buf.size());
+
+				if (packet_size == pf_.lidar_packet_size) {
+					isLidarPacket = true;
+				}
+			}
+			else if(imuFilter_ && packet_info.dst_port == info_.config.udp_port_imu)
+			{
+				// async IMU, can be used for deskewing in Odometry
+				auto packet_size = ouster::sensor_utils::read_packet(
+					*pcapHandle_, imuPacket_.buf.data(), imuPacket_.buf.size());
+				if (packet_size == pf_.imu_packet_size) {
+					isImuPacket = true;
+				}
+			}
+		}
+		else if(osfReader_.get())
+		{
+			// read next message from OSF in timestamp order
+			if(osfIter_ == osfReader_->messages().end())
+			{
+				UWARN("No more data");
+				endOfFileReached_ = true;
+				return;
+			}
+			if(osfIter_->is<ouster::osf::LidarScanStream>())
+			{
+				// Decoding LidarScan messages
+				auto ls = osfIter_->decode_msg<ouster::osf::LidarScanStream>();
+				{
+					UScopeMutex s(scanMutex_);
+					bool notify = lastScan_.w == 0;
+					lastScan_ = *ls;
+					if(lastScan_.w != 0 && notify)
+					{
+						scanReady_.release();
+					}
+				}
+			}
+			//else if(m.is<ouster::osf::LidarImuStream>()) // Seems not implemented in ouster-sdk
+
+			++osfIter_;
+		}
+		else {
+			UFATAL("");
+		}
+
+		if (isLidarPacket) {
+			// batcher will return "true" when the current scan is complete
+            if (scanBatcher_(lidarPacket_, scanBuffer_)) {
+                // retry until we receive a full set of valid measurements
+                // (accounting for azimuth_window settings if any)
+                if (scanBuffer_.complete(info_.format.column_window))
+				{
+					{
+						UScopeMutex s(scanMutex_);
+						bool notify = lastScan_.w == 0;
+						lastScan_ = scanBuffer_;
+						if(lastScan_.w != 0 && notify)
+						{
+							scanReady_.release();
+						}
+					}
+                }
+            }
+		}
+
+		// async IMU, can be used for deskewing in Odometry
+		if (isImuPacket) {
+			static const double standard_g = 9.80665;
+			uchar* buf = imuPacket_.buf.data();
+			uint64_t ts = pf_.imu_gyro_ts(buf);
+			double imuStamp = double(ts)/10e8;
+			cv::Vec3d linearAcc;
+			cv::Vec3d angularVel;
+			linearAcc[0] = pf_.imu_la_x(buf) * standard_g;
+			linearAcc[1] = pf_.imu_la_y(buf) * standard_g;
+			linearAcc[2] = pf_.imu_la_z(buf) * standard_g;
+			angularVel[0] = pf_.imu_av_x(buf) * M_PI / 180.0;
+			angularVel[1] = pf_.imu_av_y(buf) * M_PI / 180.0;
+			angularVel[2] = pf_.imu_av_z(buf) * M_PI / 180.0;
+			imuFilter_->update(
+				angularVel[0], angularVel[1], angularVel[2],
+				linearAcc[0], linearAcc[1], linearAcc[2],
+				imuStamp);
+			cv::Vec4d quat;
+			imuFilter_->getOrientation(quat[0], quat[1], quat[2], quat[3]);
+			IMU imu(quat, cv::Mat::eye(3,3,CV_64FC1)*6e-4,
+					angularVel, cv::Mat::eye(3,3,CV_64FC1)*6e-4,
+					linearAcc, cv::Mat::eye(3,3,CV_64FC1)*0.01,
+					imuLocalTransform_);
+			UEventsManager::post(new IMUEvent(imu, imuStamp));
+			UScopeMutex s(imuMutex_);
+			imuBuffer_.insert(imuBuffer_.end(), std::make_pair(imuStamp, imu));
+			if(imuBuffer_.size() > 1000)
+			{
+				imuBuffer_.erase(imuBuffer_.begin());
+			}
+		}
+	}
+
+	virtual void mainLoopEnd()
+	{
+		scanReady_.release();
+	}
+
+  private:
+	UMutex scanMutex_;
+	UMutex imuMutex_;
+	USemaphore scanReady_;
+	ouster::ScanBatcher scanBatcher_;
+	ouster::sensor::packet_format pf_;
+	ouster::LidarScan scanBuffer_;
+	ouster::LidarScan lastScan_;
+	ouster::sensor::LidarPacket lidarPacket_;
+	ouster::sensor::ImuPacket imuPacket_;
+	std::shared_ptr<ouster::sensor::client> clientHandle_;
+	std::shared_ptr<ouster::sensor_utils::playback_handle> pcapHandle_;
+	std::shared_ptr<ouster::osf::Reader> osfReader_;
+	ouster::osf::MessagesStreamingIter osfIter_;
+	ouster::sensor::sensor_info info_;
+	ouster::sensor::cf_type intensityChannel_;
+	ouster::XYZLut lut_;
+	Transform lidarSensorT_;
+	Transform imuLocalTransform_;
+	IMUFilter * imuFilter_;
+	std::map<double, IMU> imuBuffer_;
+	bool endOfFileReached_;
+};
+#endif
+
+LidarOuster::LidarOuster(
+		const std::string& ipOrHostnameOrPcapOrOsf,
+		const std::string& dataDestinationOrJson,
+		int lidarMode,
+		int timestampMode,
+		bool useReflectivityForIntensityChannel,
+		bool publishIMU,
+		float frameRate,
+		Transform localTransform) :
+	Lidar(frameRate, localTransform),
+	ousterCaptureThread_(0),
+	imuPublished_(publishIMU),
+	useReflectivityForIntensityChannel_(useReflectivityForIntensityChannel),
+	ipOrHostnameOrPcapOrOsf_(ipOrHostnameOrPcapOrOsf),
+	dataDestinationOrJson_(dataDestinationOrJson),
+	lidarMode_(lidarMode),
+	timestampMode_(timestampMode)
+{
+	UASSERT(!ipOrHostnameOrPcapOrOsf.empty());
+#ifdef RTABMAP_OUSTER
+	UASSERT(lidarMode>=ouster::sensor::lidar_mode::MODE_UNSPEC && lidarMode<=ouster::sensor::lidar_mode::MODE_4096x5);
+	UASSERT(timestampMode>=ouster::sensor::timestamp_mode::TIME_FROM_UNSPEC && timestampMode<=ouster::sensor::timestamp_mode::TIME_FROM_PTP_1588);
+#endif
+}
+
+LidarOuster::~LidarOuster()
+{
+#ifdef RTABMAP_OUSTER
+	if(ousterCaptureThread_)
+		ousterCaptureThread_->join(true);
+	delete ousterCaptureThread_;
+#endif
+}
+
+bool LidarOuster::available()
+{
+#ifdef RTABMAP_OUSTER
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::string LidarOuster::getSerial() const
+{
+#ifdef RTABMAP_OUSTER
+	if(ousterCaptureThread_) {
+		return ousterCaptureThread_->getInfo().sn.c_str();
+	}
+#endif
+	return "";
+}
+
+bool LidarOuster::init(const std::string &, const std::string &)
+{
+#ifdef RTABMAP_OUSTER
+	if(ousterCaptureThread_)
+		ousterCaptureThread_->join(true);
+	delete ousterCaptureThread_;
+	ousterCaptureThread_ = 0;
+
+	bool readingFromFile = false;
+	ouster::sensor::sensor_info info;
+	std::string ext = uToLowerCase(UFile::getExtension(ipOrHostnameOrPcapOrOsf_));
+	if(ext == "pcap")
+	{
+		UINFO("Using PCAP file \"%s\" and JSON file \"%s\"", ipOrHostnameOrPcapOrOsf_.c_str(), dataDestinationOrJson_.c_str());
+		if(dataDestinationOrJson_.empty())
+		{
+			UERROR("A JSON path should be provided when a PCAP path is used.");
+			return false;
+		}
+
+		UDEBUG("");
+		auto pcapHandle = ouster::sensor_utils::replay_initialize(ipOrHostnameOrPcapOrOsf_);
+		if(pcapHandle.get() == 0) {
+			UERROR("Failed to open pcap file \"%s\"!", ipOrHostnameOrPcapOrOsf_.c_str());
+			return false;
+		}
+    	info = ouster::sensor::metadata_from_json(dataDestinationOrJson_);
+
+		ousterCaptureThread_ = new OusterCaptureThread(
+			pcapHandle,
+			info,
+			imuPublished_,
+			useReflectivityForIntensityChannel_);
+		readingFromFile = true;
+	}
+	else if(ext == "osf")
+	{
+		UINFO("Using OSF file \"%s\"", ipOrHostnameOrPcapOrOsf_.c_str());
+		if(imuPublished_)
+		{
+			UWARN("Imu publishing it not supported for OSF files.");
+			imuPublished_ = false;
+		}
+		try {
+			std::shared_ptr<ouster::osf::Reader> osfReader(new ouster::osf::Reader(ipOrHostnameOrPcapOrOsf_));
+			auto sensors = osfReader->meta_store().find<ouster::osf::LidarSensor>();
+			UASSERT(sensors.size() >= 1);
+
+			// Use first sensor and get its sensor_info
+			info = sensors.begin()->second->info();
+
+			ousterCaptureThread_ = new OusterCaptureThread(
+				osfReader,
+				info,
+				imuPublished_,
+				useReflectivityForIntensityChannel_);
+			
+			readingFromFile = true;
+		}
+		catch(std::exception & e) {
+			UERROR("Failed creating OSF reader: %s", e.what());
+			return false;
+		}
+	}
+	else // ip / hostname
+	{
+		UINFO("Using sensor hostname \"%s\" and destination (optional) \"%s\"",
+			ipOrHostnameOrPcapOrOsf_.c_str(), dataDestinationOrJson_.c_str());
+		auto clientHandle = ouster::sensor::init_client(
+			ipOrHostnameOrPcapOrOsf_, 
+			dataDestinationOrJson_,
+			(ouster::sensor::lidar_mode)lidarMode_,
+			(ouster::sensor::timestamp_mode)timestampMode_);
+		
+		if(!clientHandle) {
+			UERROR("Failed to connect to sensor! Verify that the Ouster can be reached on the network at this address \"%s\".",
+				ipOrHostnameOrPcapOrOsf_.c_str());
+			return false;
+		}
+		
+		UINFO("Connection to sensor succeeded");
+
+		auto metadata = ouster::sensor::get_metadata(*clientHandle);
+
+		// Raw metadata can be parsed into a `sensor_info` struct
+		info = ouster::sensor::sensor_info(metadata);
+
+		ousterCaptureThread_ = new OusterCaptureThread(
+			clientHandle,
+			info,
+			imuPublished_,
+			useReflectivityForIntensityChannel_);
+	}
+	
+	UINFO("Firmware version: %s", info.fw_rev.c_str());
+	UINFO("Serial number:    %s", info.sn.c_str());
+	UINFO("Product line:     %s", info.prod_line.c_str());
+	UINFO("Scan dimensions:  %ld x %ld", info.format.columns_per_frame, info.format.pixels_per_column);
+	//UINFO("Column window:    [%d,%d]", info.column_window.first, info.column_window.second);
+
+	if(!readingFromFile)
+	{
+		ousterCaptureThread_->start();
+	}
+	else if(this->getFrameRate() == 0.0f)
+	{ 
+		std::string lidarMode = ouster::sensor::to_string(info.config.lidar_mode.value_or(ouster::sensor::lidar_mode::MODE_UNSPEC));
+		std::list<std::string> lidarModeSplit = uSplit(lidarMode, 'x');
+		if(lidarModeSplit.size() == 2) {
+			int rate = uStr2Int(*lidarModeSplit.rbegin());
+			UASSERT(rate > 0);
+			this->setFrameRate(rate);
+			UINFO("Setting frame rate to %d Hz (lidar mode = %s)", rate, lidarMode.c_str());
+		}
+		else
+		{
+			UERROR("Unknown lidar mode \"%s\", framerate is set to 10.", lidarMode.c_str());
+			this->setFrameRate(10);
+		}
+	}
+
+	return true;
+#else
+	UERROR("RTAB-Map is not built with OusterSDK");
+	return false;
+#endif
+}
+
+SensorData LidarOuster::captureData(SensorCaptureInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_OUSTER
+	double stamp = 0.0;
+	LaserScan scan;
+	if(!ousterCaptureThread_->getScan(scan, stamp))
+	{
+		// End of stream
+		return data;
+	}
+
+	data.setLaserScan(scan);
+	data.setStamp(stamp);
+
+	IMU imu = ousterCaptureThread_->getIMU(stamp);
+	if(!imu.empty())
+	{
+		data.setIMU(imu);
+	}
+#else
+	UERROR("RTAB-Map is not built with OusterSDK");
+#endif
+	return data;
+}
+
+
+} /* namespace rtabmap */

corelib/src/lidar/LidarVLP16.cpp

@@ -124,6 +124,15 @@ LidarVLP16::~LidarVLP16()
 	UDEBUG("Stopped lidar!");
 }
 
+bool LidarVLP16::available()
+{
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+	return true;
+#else
+	return false;
+#endif
+}
+
 void LidarVLP16::setOrganized(bool enable)
 {
 	organized_ = true;

corelib/src/odometry/OdometryF2M.cpp

@@ -1148,6 +1148,7 @@ Transform OdometryF2M::computeTransform(
 		}
 		else
 		{
+			UDEBUG("Initialize first key frame");
 			// Just generate keypoints for the new signature
 			// For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
 			Signature dummy;

corelib/src/util3d_filtering.cpp

@@ -328,6 +328,7 @@ LaserScan commonFiltering(
 			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
 		}
 	}
+	UDEBUG("scan size=%d format=%d, organized=%d", scan.size(), (int)scan.format(), scan.isOrganized()?1:0);
 	return scan;
 }
 

data/presets/lidar3d_icp.ini

@@ -1,3 +1,5 @@
+# Kept for backward compatibility, see also lidar3d_icp_indoor.ini or lidar3d_icp_outdoor.ini
+
 # Could be used with LiDARs like Velodyne, RoboSense, Ouster
 
 [Camera]

data/presets/lidar3d_icp_indoor.ini

@@ -0,0 +1,46 @@
+# Could be used with LiDARs like Velodyne, RoboSense, Ouster
+
+[Camera]
+Scan\downsampleStep = 1
+Scan\fromDepth = false
+Scan\normalsK = 0
+Scan\normalsRadius = 0
+Scan\normalsUp = false
+Scan\rangeMax = 0
+Scan\rangeMin = 0
+Scan\voxelSize = 0
+
+[Gui]
+General\showClouds0 = false
+General\showClouds1 = false
+
+[Core]
+# Would be 0.01 for odom and 0.2 for mapping:
+Icp\CorrespondenceRatio = 0.1
+Icp\Epsilon = 0.001
+Icp\FiltersEnabled = 2
+Icp\Iterations = 10
+Icp\OutlierRatio = 0.7
+# ~10x the voxel size:
+Icp\MaxCorrespondenceDistance = 1
+Icp\MaxTranslation = 2
+# Uncomment if lidar can see ground most of the time (on a car or wheeled robot):
+#Icp\PointToPlaneGroundNormalsUp = 0.8
+Icp\PointToPlaneK = 20
+Icp\PointToPlaneMinComplexity = 0
+# Make sure PM is used:
+Icp\Strategy = 1
+Icp\VoxelSize = 0.1
+Mem\NotLinkedNodesKept = false
+Mem\STMSize = 30
+Odom\Deskewing = true
+Odom\GuessSmoothingDelay = 0.3
+Odom\ScanKeyFrameThr = 0.6
+OdomF2M\ScanMaxSize = 15000
+# Match voxel size:
+OdomF2M\ScanSubtractRadius = 0.1
+RGBD\AngularUpdate = 0.05
+RGBD\LinearUpdate = 0.05
+RGBD\ProximityMaxGraphDepth = 0
+RGBD\ProximityPathMaxNeighbors = 1
+Reg\Strategy = 1

data/presets/lidar3d_icp_outdoor.ini

@@ -0,0 +1,46 @@
+# Could be used with LiDARs like Velodyne, RoboSense, Ouster
+
+[Camera]
+Scan\downsampleStep = 1
+Scan\fromDepth = false
+Scan\normalsK = 0
+Scan\normalsRadius = 0
+Scan\normalsUp = false
+Scan\rangeMax = 0
+Scan\rangeMin = 0
+Scan\voxelSize = 0
+
+[Gui]
+General\showClouds0 = false
+General\showClouds1 = false
+
+[Core]
+# Would be 0.01 for odom and 0.2 for mapping:
+Icp\CorrespondenceRatio = 0.1
+Icp\Epsilon = 0.001
+Icp\FiltersEnabled = 2
+Icp\Iterations = 10
+Icp\OutlierRatio = 0.7
+# ~10x the voxel size:
+Icp\MaxCorrespondenceDistance = 5
+Icp\MaxTranslation = 5
+# Uncomment if lidar can see ground most of the time (on a car or wheeled robot):
+#Icp\PointToPlaneGroundNormalsUp = 0.8
+Icp\PointToPlaneK = 20
+Icp\PointToPlaneMinComplexity = 0
+# Make sure PM is used:
+Icp\Strategy = 1
+Icp\VoxelSize = 0.5
+Mem\NotLinkedNodesKept = false
+Mem\STMSize = 30
+Odom\Deskewing = true
+Odom\GuessSmoothingDelay = 0.3
+Odom\ScanKeyFrameThr = 0.6
+OdomF2M\ScanMaxSize = 15000
+# Match voxel size:
+OdomF2M\ScanSubtractRadius = 0.5
+RGBD\AngularUpdate = 0.05
+RGBD\LinearUpdate = 0.05
+RGBD\ProximityMaxGraphDepth = 0
+RGBD\ProximityPathMaxNeighbors = 1
+Reg\Strategy = 1

examples/LidarMapping/MapBuilder.h

@@ -190,9 +190,21 @@ protected Q_SLOTS:
 			{
 				// update camera position
 				cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
+
+				if( odom.data().imu().orientation().val[0]!=0 ||
+					odom.data().imu().orientation().val[1]!=0 ||
+					odom.data().imu().orientation().val[2]!=0 ||
+					odom.data().imu().orientation().val[3]!=0)
+				{
+					Eigen::Vector3f gravity(0,0,-1);
+					Transform orientation(0,0,0, odom.data().imu().orientation()[0], odom.data().imu().orientation()[1], odom.data().imu().orientation()[2], odom.data().imu().orientation()[3]);
+					gravity = (orientation* odom.data().imu().localTransform().inverse()*(odometryCorrection_*odom.pose()).rotation().inverse()).toEigen3f()*gravity;
+					cloudViewer_->addOrUpdateLine("odom_imu_orientation", odometryCorrection_*odom.pose(), (odometryCorrection_*odom.pose()).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*odom.pose().rotation().inverse(), Qt::yellow, true, true);
+				}
 			}
 		}
 		cloudViewer_->update();
+		cloudViewer_->refreshView();
 
 		lastOdometryProcessed_ = true;
 	}
@@ -295,6 +307,7 @@ protected Q_SLOTS:
 		odometryCorrection_ = stats.mapCorrection();
 
 		cloudViewer_->update();
+		cloudViewer_->refreshView();
 
 		processingStatistics_ = false;
 	}

examples/LidarMapping/main.cpp

@@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 // Should be first on windows to avoid "WinSock.h has already been included" error
 #include "rtabmap/core/lidar/LidarVLP16.h"
+#include "rtabmap/core/lidar/LidarOuster.h"
 
 #include <rtabmap/core/Odometry.h>
 #include "rtabmap/core/Rtabmap.h"
@@ -35,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UEventsManager.h"
 #include "rtabmap/utilite/UStl.h"
 #include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
 #include <QApplication>
 #include <stdio.h>
 #include <pcl/io/pcd_io.h>
@@ -47,11 +49,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-lidar_mapping IP PORT\n"
-			"rtabmap-lidar_mapping PCAP_FILEPATH\n"
+			"rtabmap-lidar_mapping [OPTIONS] DRIVER ...\n"
+			"rtabmap-lidar_mapping [OPTIONS] 0 IP PORT (with VLP16 LiDAR)\n"
+			"rtabmap-lidar_mapping [OPTIONS] 1 IP      (with Ouster LiDAR)\n"
+			"rtabmap-lidar_mapping [OPTIONS] DRIVER PCAP_FILEPATH [JSON_FILEPATH]\n"
 			"\n"
-			"Example:"
-			"  rtabmap-lidar_mapping 192.168.1.201 2368\n\n");
+			"DRIVER: 0=VLP16, 1=Ouster\n"
+			"JSON_FILEPATH: should be set if DRIVER=1\n"
+			"Options:\n"
+			"  --resolution #          Resolution of the map (default 0.05 m), indoor: 0.05-0.3, outdoor: 0.3-0.5\n"
+			"\n"
+			"Examples:\n"
+			"  rtabmap-lidar_mapping 0 192.168.1.201 2368\n"
+			"  rtabmap-lidar_mapping 0 scan.pcap\n"
+			"  rtabmap-lidar_mapping 1 scan.pcap config.json\n\n");
 	exit(1);
 }
 
@@ -59,42 +70,109 @@ using namespace rtabmap;
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kWarning);
+	ULogger::setLevel(ULogger::kInfo);
 
-	std::string filepath;
+	std::string pcapFile;
+	std::string jsonFile;
 	std::string ip;
 	int port = 2368;
-	if(argc < 2)
+	int driver = 0;
+	float resolution = 0.05;
+	if(argc < 3)
 	{
 		showUsage();
 	}
-	else if(argc == 2)
-	{
-		filepath = argv[1];
-	}
 	else
 	{
-		ip = argv[1];
-		port = uStr2Int(argv[2]);
+		int i=1;
+		while(i<argc-3)
+		{
+			if(strcmp("--resolution", argv[i]) == 0)
+			{
+				++i;
+				resolution = uStr2Float(argv[i]);
+				++i;
+			}
+			else
+			{
+				break;
+			}
+		}
+		driver = uStr2Int(argv[i++]);
+		if(driver <0 || driver>1)
+		{
+			printf("Not supported driver %d!\n", driver);
+			showUsage();
+		}
+		std::string ext = UFile::getExtension(uToLowerCase(argv[i]));
+		if(ext == "pcap" || ext == "osf")
+		{
+			pcapFile = argv[i++];
+			if(driver == 1 && ext == "pcap")
+			{
+				if(i<argc) {
+					jsonFile=argv[i];
+				}
+				else {
+					printf("Missing JSON file\n");
+					showUsage();
+				}
+			}
+		}
+		else if(i<argc-1) {
+			ip = argv[i++];
+			if(i<argc) {
+				port = uStr2Int(argv[i]);
+			}
+			else {
+				printf("Missing port for driver 0\n");
+				showUsage();
+			}
+		}
+		else {
+			printf("Only %d argument(s) provided \n", argc-1);
+			showUsage();
+		}
 	}
+	printf("Using driver=%s\n", driver==0?"VLP16":"Ouster");
 
 	// Here is the pipeline that we will use:
 	// LidarVLP16 -> "SensorEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"
 
 	// Create the Lidar sensor, it will send a SensorEvent
-	LidarVLP16 * lidar;
+	Lidar * lidar = 0;
 	if(!ip.empty())
 	{
-		printf("Using ip=%s port=%d\n", ip.c_str(), port);
-		lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+		if(driver == 0)
+		{
+			printf("Using ip=%s port=%d\n", ip.c_str(), port);
+			lidar = new LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
+			((LidarVLP16*)lidar)->setOrganized(true); //faster deskewing
+		}
+		else if(driver == 1)
+		{
+			printf("Using ip=%s\n", ip.c_str());
+			lidar = new LidarOuster(ip, 0, 0);
+		}
 	}
 	else
 	{
-		filepath = uReplaceChar(filepath, '~', UDirectory::homeDir());
-		printf("Using file=%s\n", filepath.c_str());
-		lidar = new LidarVLP16(filepath);
+		pcapFile = uReplaceChar(pcapFile, '~', UDirectory::homeDir());
+		printf("Using file=%s\n", pcapFile.c_str());
+		if(driver == 0)
+		{
+			lidar = new LidarVLP16(pcapFile);
+			((LidarVLP16*)lidar)->setOrganized(true); //faster deskewing
+			lidar->setFrameRate(10); // default
+		}
+		else if(driver == 1)
+		{
+			jsonFile = uReplaceChar(jsonFile, '~', UDirectory::homeDir());
+			printf("Using config=%s\n", jsonFile.c_str());
+			lidar = new LidarOuster(pcapFile, jsonFile);
+			// frame rate is set based on the lidar mode
+		}
 	}
-	lidar->setOrganized(true); //faster deskewing
 
 	if(!lidar->init())
 	{
@@ -112,8 +190,6 @@ int main(int argc, char * argv[])
 
 	ParametersMap params;
 
-	float resolution = 0.05;
-
 	// ICP parameters
 	params.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
 	params.insert(ParametersPair(Parameters::kIcpFiltersEnabled(), "2"));
@@ -211,16 +287,15 @@ int main(int argc, char * argv[])
 	}
 	if(cloud->size())
 	{
-		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)cloud->size());
-		cloud = util3d::voxelize(cloud, 0.01f);
+		printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", resolution, (int)cloud->size());
+		cloud = util3d::voxelize(cloud, resolution);
 
-		printf("Saving rtabmap_cloud.pcd... done! (%d points)\n", (int)cloud->size());
-		pcl::io::savePCDFile("rtabmap_cloud.pcd", *cloud);
-		//pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+		pcl::io::savePLYFile("rtabmap_cloud.ply", *cloud); // to save in PLY format
+		printf("Saving rtabmap_cloud.ply... done! (%d points)\n", (int)cloud->size());
 	}
 	else
 	{
-		printf("Saving rtabmap_cloud.pcd... failed! The cloud is empty.\n");
+		printf("Saving point cloud failed! The cloud is empty.\n");
 	}
 
 	// Save trajectory

guilib/include/rtabmap/gui/MainWindow.h

@@ -194,6 +194,7 @@ protected Q_SLOTS:
 	void selectDepthAIOAKDPro();
 	void selectXvisioSeerSense();
 	void selectVLP16();
+	void selectOuster();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -121,6 +121,7 @@ public:
 
 		kSrcLidar           = 400,
 		kSrcLidarVLP16      = 400,
+		kSrcLidarOuster     = 401,
 	};
 
 public:
@@ -403,6 +404,8 @@ private Q_SLOTS:
 	void selectSourceRealsense2JsonPath();
 	void selectSourceDepthaiBlobPath();
 	void selectVlp16PcapPath();
+	void selectOusterPcapPath();
+	void selectOusterJsonPath();
 	void updateSourceGrpVisibility();
 	void testOdometry();
 	void testCamera();

guilib/src/AboutDialog.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraStereo.h"
+#include "rtabmap/core/lidar/LidarOuster.h"
 #include "rtabmap/core/Optimizer.h"
 #include "ui_aboutDialog.h"
 #include <opencv2/core/version.hpp>
@@ -158,6 +159,8 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_depthai->setText(CameraDepthAI::available() ? "Yes" : "No");
 	_ui->label_depthai_license->setEnabled(CameraDepthAI::available());
 	_ui->label_xvsdk->setText(CameraSeerSense::available() ? "Yes" : "No");
+	_ui->label_ouster->setText(LidarOuster::available() ? "Yes" : "No");
+	_ui->label_ouster_license->setEnabled(LidarOuster::available());
 
 	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
 	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);

guilib/src/CMakeLists.txt

@@ -181,6 +181,8 @@ add_definitions(${PCL_DEFINITIONS})
 SET(RESOURCES
 	${PROJECT_SOURCE_DIR}/data/presets/camera_tof_icp.ini
 	${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp.ini
+	${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp_indoor.ini
+	${PROJECT_SOURCE_DIR}/data/presets/lidar3d_icp_outdoor.ini
 )
 
 foreach(arg ${RESOURCES})

guilib/src/GuiLib.qrc

@@ -46,5 +46,7 @@
         <file>images/astra.png</file>
         <file>images/oakdpro.png</file>
         <file>images/seer_sense_DS80.png</file>
+        <file>images/ouster.png</file>
+        <file>images/vlp16.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -25,6 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+// Should be first on windows to avoid "WinSock.h has already been included" error
+#include "rtabmap/core/lidar/LidarVLP16.h"
+
 #include "rtabmap/gui/MainWindow.h"
 
 #include "ui_mainWindow.h"
@@ -32,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGB.h"
 #include "rtabmap/core/CameraStereo.h"
 #include "rtabmap/core/Lidar.h"
+#include "rtabmap/core/lidar/LidarOuster.h"
 #include "rtabmap/core/IMUThread.h"
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/Parameters.h"
@@ -471,6 +475,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionDepthAI_oakdpro, SIGNAL(triggered()), this, SLOT(selectDepthAIOAKDPro()));
 	connect(_ui->actionXvisio_SeerSense, SIGNAL(triggered()), this, SLOT(selectXvisioSeerSense()));
 	connect(_ui->actionVelodyne_VLP_16, SIGNAL(triggered()), this, SLOT(selectVLP16()));
+	connect(_ui->actionOuster_SDK, SIGNAL(triggered()), this, SLOT(selectOuster()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_PCL->setEnabled(CameraOpenni::available());
 	_ui->actionOpenNI_PCL_ASUS->setEnabled(CameraOpenni::available());
@@ -499,6 +504,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
     _ui->actionDepthAI_oakdlite->setEnabled(CameraDepthAI::available());
     _ui->actionDepthAI_oakdpro->setEnabled(CameraDepthAI::available());
 	_ui->actionXvisio_SeerSense->setEnabled(CameraSeerSense::available());
+	_ui->actionVelodyne_VLP_16->setEnabled(LidarVLP16::available());
+	_ui->actionOuster_SDK->setEnabled(LidarOuster::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -5305,6 +5312,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionDepthAI_oakdpro->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoDepthAI);
 	_ui->actionXvisio_SeerSense->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcSeerSense);
 	_ui->actionVelodyne_VLP_16->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarVLP16);
+	_ui->actionOuster_SDK->setChecked(_preferencesDialog->getLidarSourceDriver() == PreferencesDialog::kSrcLidarOuster);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -7248,6 +7256,11 @@ void MainWindow::selectVLP16()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarVLP16);
 }
 
+void MainWindow::selectOuster()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcLidarOuster);
+}
+
 void MainWindow::dumpTheMemory()
 {
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));

guilib/src/OdometryViewer.cpp

@@ -185,6 +185,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 	bool lost = false;
 	bool lostStateChanged = false;
 
+	imageView_->setVisible(!odom.data().imageRaw().empty() || !odom.data().depthOrRightRaw().empty());
+
 	if(odom.pose().isNull())
 	{
 		UDEBUG("odom lost"); // use last pose
@@ -211,7 +213,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		cloudView_->setBackgroundColor(Qt::black);
 	}
 
-	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
+	timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
 
 	if(cloudShown_->isChecked() &&
 		!odom.data().imageRaw().empty() &&
@@ -219,8 +221,8 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		(odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
 	{
 		UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
-
-		if(!odom.data().depthRaw().empty())
+
+		if(!odom.data().depthRaw().empty())
 		{
 			if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
 			   odom.data().imageRaw().rows % decimationSpin_->value() == 0)
@@ -238,14 +240,14 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 			}
 		}
 		else
-		{
-			validDecimationValue_ = decimationSpin_->value();
+		{
+			validDecimationValue_ = decimationSpin_->value();
 		}
 
 		// visualization: buffering the clouds
 		// Create the new cloud
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		pcl::IndicesPtr validIndices(new std::vector<int>);
+		pcl::IndicesPtr validIndices(new std::vector<int>);
 		cloud = util3d::cloudRGBFromSensorData(
 				odom.data(),
 				validDecimationValue_,
@@ -257,7 +259,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 		if(voxelSpin_->value())
 		{
 			cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
-		}
+		}
 
 		if(cloud->size())
 		{
@@ -489,6 +491,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
 
 	imageView_->update();
 	cloudView_->update();
+	cloudView_->refreshView();
 	QApplication::processEvents();
 	processingData_ = false;
 }

guilib/src/PreferencesDialog.cpp

@@ -27,10 +27,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <pcl/pcl_config.h>
-#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 // Should be first on windows to avoid "WinSock.h has already been included" error
 #include "rtabmap/core/lidar/LidarVLP16.h"
-#endif
+#include "rtabmap/core/lidar/LidarOuster.h"
 
 #include "rtabmap/gui/PreferencesDialog.h"
 #include "rtabmap/gui/DatabaseViewer.h"
@@ -93,6 +92,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // Presets
 #include "camera_tof_icp_ini.h"
 #include "lidar3d_icp_ini.h"
+#include "lidar3d_icp_indoor_ini.h"
+#include "lidar3d_icp_outdoor_ini.h"
 
 #include <opencv2/opencv_modules.hpp>
 #include <rtabmap/core/SensorCaptureThread.h>
@@ -430,6 +431,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
+	 //LiDARs
+	if (!LidarVLP16::available())
+	{
+		_ui->comboBox_lidar_src->setItemData(kSrcLidarVLP16 - kSrcLidar, 0, Qt::UserRole - 1);
+	}
+	if (!LidarOuster::available())
+	{
+		_ui->comboBox_lidar_src->setItemData(kSrcLidarOuster - kSrcLidar, 0, Qt::UserRole - 1);
+	}
+
 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
 	_ui->checkBox_showFrustums->setEnabled(false);
 	_ui->checkBox_showFrustums->setChecked(false);
@@ -480,6 +491,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->pushButton_resetConfig, SIGNAL(clicked()), this, SLOT(resetConfig()));
 	connect(_ui->pushButton_presets_camera_tof_icp, SIGNAL(clicked()), this, SLOT(loadPreset()));
 	connect(_ui->pushButton_presets_lidar_3d_icp, SIGNAL(clicked()), this, SLOT(loadPreset()));
+	connect(_ui->pushButton_presets_lidar_3d_icp_indoor, SIGNAL(clicked()), this, SLOT(loadPreset()));
+	connect(_ui->pushButton_presets_lidar_3d_icp_outdoor, SIGNAL(clicked()), this, SLOT(loadPreset()));
 	connect(_ui->radioButton_basic, SIGNAL(toggled(bool)), this, SLOT(setupTreeView()));
 	connect(_ui->pushButton_testOdometry, SIGNAL(clicked()), this, SLOT(testOdometry()));
 	connect(_ui->pushButton_test_camera, SIGNAL(clicked()), this, SLOT(testCamera()));
@@ -917,6 +930,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_vlp16_organized, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_vlp16_hostTime, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkBox_vlp16_stamp_last, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_ouster_ip_hostname, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_ouster_lidar_mode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_ouster_timestamp, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->toolButton_ouster_pcap_path, SIGNAL(clicked()), this, SLOT(selectOusterPcapPath()));
+	connect(_ui->toolButton_ouster_json_path, SIGNAL(clicked()), this, SLOT(selectOusterJsonPath()));
+	connect(_ui->lineEdit_ouster_pcap_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->lineEdit_ouster_json_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -2286,6 +2306,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_vlp16_organized->setChecked(false);
 		_ui->checkBox_vlp16_hostTime->setChecked(true);
 		_ui->checkBox_vlp16_stamp_last->setChecked(true);
+		_ui->lineEdit_ouster_ip_hostname->clear();
+		_ui->comboBox_ouster_lidar_mode->setCurrentIndex(0);
+		_ui->comboBox_ouster_timestamp->setCurrentIndex(0);
+		_ui->lineEdit_ouster_pcap_path->clear();
+		_ui->lineEdit_ouster_json_path->clear();
+		_ui->checkBox_ouster_reflectivity->setChecked(true);
+		_ui->checkBox_ouster_imu->setChecked(false);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -2850,6 +2877,16 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->checkBox_vlp16_stamp_last->setChecked(settings.value("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked()).toBool());
 	settings.endGroup(); // VLP16
 
+	settings.beginGroup("Ouster");
+	_ui->lineEdit_ouster_ip_hostname->setText(settings.value("ip",_ui->lineEdit_ouster_ip_hostname->text()).toString());
+	_ui->comboBox_ouster_lidar_mode->setCurrentIndex(settings.value("lidarMode", _ui->comboBox_ouster_lidar_mode->currentIndex()).toInt());
+	_ui->comboBox_ouster_timestamp->setCurrentIndex(settings.value("timestamp", _ui->comboBox_ouster_timestamp->currentIndex()).toInt());
+	_ui->lineEdit_ouster_pcap_path->setText(settings.value("pcapPath",_ui->lineEdit_ouster_pcap_path->text()).toString());
+	_ui->lineEdit_ouster_json_path->setText(settings.value("jsonPath",_ui->lineEdit_ouster_json_path->text()).toString());
+	_ui->checkBox_ouster_reflectivity->setChecked(settings.value("reflectivity",_ui->checkBox_ouster_reflectivity->isChecked()).toBool());
+	_ui->checkBox_ouster_imu->setChecked(settings.value("imu",_ui->checkBox_ouster_imu->isChecked()).toBool());
+	settings.endGroup(); // Ouster
+	
 	settings.endGroup(); // Lidar
 
 	_calibrationDialog->loadSettings(settings, "CalibrationDialog");
@@ -3004,6 +3041,14 @@ void PreferencesDialog::loadPreset()
 	{
 		loadPreset(LIDAR3D_ICP_INI);
 	}
+	else if(sender() == _ui->pushButton_presets_lidar_3d_icp_indoor)
+	{
+		loadPreset(LIDAR3D_ICP_INDOOR_INI);
+	}
+	else if(sender() == _ui->pushButton_presets_lidar_3d_icp_outdoor)
+	{
+		loadPreset(LIDAR3D_ICP_OUTDOOR_INI);
+	}
 	else
 	{
 		UERROR("Unknown sender!");
@@ -3448,6 +3493,16 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("stampLast", _ui->checkBox_vlp16_stamp_last->isChecked());
 	settings.endGroup(); // VLP16
 
+	settings.beginGroup("Ouster");
+	settings.setValue("ip",_ui->lineEdit_ouster_ip_hostname->text());
+	settings.setValue("lidarMode", _ui->comboBox_ouster_lidar_mode->currentIndex());
+	settings.setValue("timestamp", _ui->comboBox_ouster_timestamp->currentIndex());
+	settings.setValue("pcapPath",_ui->lineEdit_ouster_pcap_path->text());
+	settings.setValue("jsonPath",_ui->lineEdit_ouster_json_path->text());
+	settings.setValue("reflectivity",_ui->checkBox_ouster_reflectivity->isChecked());
+	settings.setValue("imu",_ui->checkBox_ouster_imu->isChecked());
+	settings.endGroup(); // Ouster
+
 	settings.endGroup(); // Lidar
 
 	_calibrationDialog->saveSettings(settings, "CalibrationDialog");
@@ -4263,7 +4318,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
 	}
 	else if(src >= kSrcLidar)
 	{
-		_ui->comboBox_lidar_src->setCurrentIndex(kSrcLidarVLP16 - kSrcLidar + 1);
+		_ui->comboBox_lidar_src->setCurrentIndex(src - kSrcLidar + 1);
 	}
 
 	if(previousCameraSrc == kSrcUndef && src < kSrcDatabase &&
@@ -4277,7 +4332,7 @@ void PreferencesDialog::selectSourceDriver(Src src, int variant)
 	}
 	else if(previousLidarSrc== kSrcUndef && src >= kSrcLidar &&
 			QMessageBox::question(this, tr("LiDAR Source..."),
-			tr("Do you want to use \"LiDAR 3D ICP\" preset?"),
+			tr("Do you want to use \"LiDAR 3D ICP\" preset? You can open Preferences->General Settings to select indoor or outdoor presets afterwards."),
 			QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes) == QMessageBox::Yes)
 	{
 		loadPreset(LIDAR3D_ICP_INI);
@@ -4748,6 +4803,38 @@ void PreferencesDialog::selectVlp16PcapPath()
 	}
 }
 
+void PreferencesDialog::selectOusterPcapPath()
+{
+	QString dir = _ui->lineEdit_ouster_pcap_path->text();
+	if (dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ouster recording (*.pcap *.osf)"));
+	if (!path.isEmpty())
+	{
+		_ui->lineEdit_ouster_pcap_path->setText(path);
+		if(QFileInfo(path).suffix() == "pcap")
+		{
+			selectOusterJsonPath();
+		}
+	}
+}
+
+void PreferencesDialog::selectOusterJsonPath()
+{
+	QString dir = _ui->lineEdit_ouster_json_path->text();
+	if (dir.isEmpty())
+	{
+		dir = getWorkingDirectory();
+	}
+	QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ouster recording config (*.json)"));
+	if (!path.isEmpty())
+	{
+		_ui->lineEdit_ouster_json_path->setText(path);
+	}
+}
+
 void PreferencesDialog::setParameter(const std::string & key, const std::string & value)
 {
 	UDEBUG("%s=%s", key.c_str(), value.c_str());
@@ -5698,6 +5785,7 @@ void PreferencesDialog::updateSourceGrpVisibility()
 	}
 	_ui->stackedWidget_lidar_src->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0);
 	_ui->groupBox_vlp16->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarVLP16-kSrcLidar);
+	_ui->groupBox_ouster->setVisible(_ui->comboBox_lidar_src->currentIndex()-1 == kSrcLidarOuster-kSrcLidar);
 	_ui->frame_lidar_sensor->setVisible(_ui->comboBox_lidar_src->currentIndex() > 0 || _ui->checkBox_source_scanFromDepth->isChecked()); // Not Lidar None or database input
 	_ui->pushButton_test_lidar->setEnabled(_ui->comboBox_lidar_src->currentIndex() > 0);
 
@@ -6987,9 +7075,8 @@ Lidar * PreferencesDialog::createLidar()
 {
 	Lidar * lidar = 0;
 	Src driver = getLidarSourceDriver();
-	if(driver == kSrcLidarVLP16)
+	if(driver >= kSrcLidarVLP16 && driver <= kSrcLidarOuster)
 	{
-#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
 		Transform localTransform = Transform::fromString(_ui->lineEdit_lidar_local_transform->text().replace("PI_2", QString::number(3.141592/2.0)).toStdString());
 		if(localTransform.isNull())
 		{
@@ -6997,33 +7084,73 @@ Lidar * PreferencesDialog::createLidar()
 					_ui->lineEdit_lidar_local_transform->text().toStdString().c_str());
 			localTransform = Transform::getIdentity();
 		}
-		if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty())
+		if(driver == kSrcLidarVLP16)
 		{
-			// PCAP mode
-			lidar = new LidarVLP16(
-					_ui->lineEdit_vlp16_pcap_path->text().toStdString(),
-					_ui->checkBox_vlp16_organized->isChecked(),
-					_ui->checkBox_vlp16_stamp_last->isChecked(),
-					this->getGeneralInputRate(),
-					localTransform);
+			if(!_ui->lineEdit_vlp16_pcap_path->text().isEmpty())
+			{
+				// PCAP mode
+				lidar = new LidarVLP16(
+						_ui->lineEdit_vlp16_pcap_path->text().toStdString(),
+						_ui->checkBox_vlp16_organized->isChecked(),
+						_ui->checkBox_vlp16_stamp_last->isChecked(),
+						this->getGeneralInputRate(),
+						localTransform);
+			}
+			else
+			{
+				// Connect to sensor
+
+				lidar = new LidarVLP16(
+						boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld",
+								(size_t)_ui->spinBox_vlp16_ip1->value(),
+								(size_t)_ui->spinBox_vlp16_ip2->value(),
+								(size_t)_ui->spinBox_vlp16_ip3->value(),
+								(size_t)_ui->spinBox_vlp16_ip4->value())),
+						_ui->spinBox_vlp16_port->value(),
+						_ui->checkBox_vlp16_organized->isChecked(),
+						_ui->checkBox_vlp16_hostTime->isChecked(),
+						_ui->checkBox_vlp16_stamp_last->isChecked(),
+						this->getGeneralInputRate(),
+						localTransform);
+
+			}
 		}
-		else
+		else // Ouster
 		{
-			// Connect to sensor
-
-			lidar = new LidarVLP16(
-					boost::asio::ip::address_v4::from_string(uFormat("%ld.%ld.%ld.%ld",
-							(size_t)_ui->spinBox_vlp16_ip1->value(),
-							(size_t)_ui->spinBox_vlp16_ip2->value(),
-							(size_t)_ui->spinBox_vlp16_ip3->value(),
-							(size_t)_ui->spinBox_vlp16_ip4->value())),
-					_ui->spinBox_vlp16_port->value(),
-					_ui->checkBox_vlp16_organized->isChecked(),
-					_ui->checkBox_vlp16_hostTime->isChecked(),
-					_ui->checkBox_vlp16_stamp_last->isChecked(),
-					this->getGeneralInputRate(),
-					localTransform);
+			if(!_ui->lineEdit_ouster_pcap_path->text().isEmpty())
+			{
+				// PCAP/OSF mode
+				lidar = new LidarOuster(
+						_ui->lineEdit_ouster_pcap_path->text().toStdString(),
+						_ui->lineEdit_ouster_json_path->text().toStdString(),
+						0, // lidar mode ignored
+						0, // timestamp mode ignored
+						_ui->checkBox_ouster_reflectivity->isChecked(),
+						_ui->checkBox_ouster_imu->isChecked(),
+						this->getGeneralInputRate(),
+						localTransform);
+			}
+			else if(!_ui->lineEdit_ouster_ip_hostname->text().isEmpty())
+			{
+				// Connect to sensor
 
+				lidar = new LidarOuster(
+						_ui->lineEdit_ouster_ip_hostname->text().toStdString(),
+						"", // data destination ignored
+						_ui->comboBox_ouster_lidar_mode->currentIndex(),
+						_ui->comboBox_ouster_timestamp->currentIndex(),
+						_ui->checkBox_ouster_reflectivity->isChecked(),
+						_ui->checkBox_ouster_imu->isChecked(),
+						this->getGeneralInputRate(),
+						localTransform);
+			}
+			else
+			{
+				QMessageBox::warning(this,
+					   tr("RTAB-Map"),
+					   tr("Ouster: An IP/hostname or PCAP path should be set..."));
+				return lidar; // 0
+			}
 		}
 		if(!lidar->init())
 		{
@@ -7034,12 +7161,6 @@ Lidar * PreferencesDialog::createLidar()
 			delete lidar;
 			lidar = 0;
 		}
-#else
-		UWARN("Lidar cannot be used with rtabmap built with PCL < 1.8... ");
-		QMessageBox::warning(this,
-				   tr("RTAB-Map"),
-				   tr("Lidar initialization failed..."));
-#endif
 	}
 	return lidar;
 }
@@ -7182,8 +7303,9 @@ void PreferencesDialog::setSLAMMode(bool enabled)
 
 void PreferencesDialog::testOdometry()
 {
-	Camera * camera = this->createCamera();
-	if(!camera)
+	Camera * camera = this->getSourceDriver()!=-1?this->createCamera():0;
+	Lidar * lidar = this->getLidarSourceDriver()!=-1?this->createLidar():0;
+	if(!camera && !lidar)
 	{
 		return;
 	}
@@ -7241,18 +7363,30 @@ void PreferencesDialog::testOdometry()
 	odomViewer->resize(1280, 480+QPushButton().minimumHeight());
 	odomViewer->registerToEventsManager();
 
-	SensorCaptureThread cameraThread(camera, this->getAllParameters()); // take ownership of camera
-	cameraThread.setMirroringEnabled(isSourceMirroring());
-	cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
-	cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
-	cameraThread.setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex());
+	SensorCaptureThread  * cameraThread;
+	if(lidar && camera)
+	{
+		cameraThread = new SensorCaptureThread(lidar, camera, this->getAllParameters()); // take ownership of lidar and camera
+	}
+	else if(camera)
+	{
+		cameraThread = new SensorCaptureThread(camera, this->getAllParameters()); // take ownership of camera
+	}
+	else // lidar 
+	{
+		cameraThread = new SensorCaptureThread(lidar, this->getAllParameters()); // take ownership of lidar
+	}
+	cameraThread->setMirroringEnabled(isSourceMirroring());
+	cameraThread->setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
+	cameraThread->setImageDecimation(_ui->spinBox_source_imageDecimation->value());
+	cameraThread->setHistogramMethod(_ui->comboBox_source_histogramMethod->currentIndex());
 	if(_ui->checkbox_source_feature_detection->isChecked())
 	{
-		cameraThread.enableFeatureDetection(this->getAllParameters());
+		cameraThread->enableFeatureDetection(this->getAllParameters());
 	}
-	cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
-	cameraThread.setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
-	cameraThread.setScanParameters(
+	cameraThread->setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
+	cameraThread->setStereoExposureCompensation(_ui->checkBox_stereo_exposureCompensation->isChecked());
+	cameraThread->setScanParameters(
 			_ui->checkBox_source_scanFromDepth->isChecked(),
 			_ui->spinBox_source_scanDownsampleStep->value(),
 			_ui->doubleSpinBox_source_scanRangeMin->value(),
@@ -7264,23 +7398,23 @@ void PreferencesDialog::testOdometry()
 			_ui->checkBox_source_scanDeskewing->isChecked());
 	if(_ui->comboBox_imuFilter_strategy->currentIndex()>0 && dynamic_cast<DBReader*>(camera) == 0)
 	{
-		cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked());
+		cameraThread->enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters(), _ui->checkBox_imuFilter_baseFrameConversion->isChecked());
 	}
 	if(isDepthFilteringAvailable())
 	{
 		if(_ui->groupBox_bilateral->isChecked())
 		{
-			cameraThread.enableBilateralFiltering(
+			cameraThread->enableBilateralFiltering(
 					_ui->doubleSpinBox_bilateral_sigmaS->value(),
 					_ui->doubleSpinBox_bilateral_sigmaR->value());
 		}
 		if(!_ui->lineEdit_source_distortionModel->text().isEmpty())
 		{
-			cameraThread.setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString());
+			cameraThread->setDistortionModel(_ui->lineEdit_source_distortionModel->text().toStdString());
 		}
 	}
 
-	UEventsManager::createPipe(&cameraThread, &odomThread, "SensorEvent");
+	UEventsManager::createPipe(cameraThread, &odomThread, "SensorEvent");
 	if(imuThread)
 	{
 		UEventsManager::createPipe(imuThread, &odomThread, "IMUEvent");
@@ -7289,7 +7423,7 @@ void PreferencesDialog::testOdometry()
 	UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
 
 	odomThread.start();
-	cameraThread.start();
+	cameraThread->start();
 
 	if(imuThread)
 	{
@@ -7304,8 +7438,9 @@ void PreferencesDialog::testOdometry()
 		imuThread->join(true);
 		delete imuThread;
 	}
-	cameraThread.join(true);
+	cameraThread->join(true);
 	odomThread.join(true);
+	delete cameraThread;
 }
 
 void PreferencesDialog::testCamera()

guilib/src/ui/mainWindow.ui

@@ -356,7 +356,32 @@
       <property name="title">
        <string>LiDAR</string>
       </property>
-      <addaction name="actionVelodyne_VLP_16"/>
+      <property name="icon">
+       <iconset resource="../GuiLib.qrc">
+        <normaloff>:/images/vlp16.png</normaloff>:/images/vlp16.png</iconset>
+      </property>
+      <widget class="QMenu" name="menuVelodyne">
+       <property name="title">
+        <string>Velodyne</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/vlp16.png</normaloff>:/images/vlp16.png</iconset>
+       </property>
+       <addaction name="actionVelodyne_VLP_16"/>
+      </widget>
+      <widget class="QMenu" name="menuOuster">
+       <property name="title">
+        <string>Ouster</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/ouster.png</normaloff>:/images/ouster.png</iconset>
+       </property>
+       <addaction name="actionOuster_SDK"/>
+      </widget>
+      <addaction name="menuVelodyne"/>
+      <addaction name="menuOuster"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuStereo_camera"/>
@@ -1749,6 +1774,14 @@
     <string>Xvisio</string>
    </property>
   </action>
+  <action name="actionOuster_SDK">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>Ouster SDK</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-3879</y>
             <width>713</width>
-            <height>4653</height>
+            <height>4737</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>7</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -369,10 +369,16 @@
                           </item>
                           <item>
                            <layout class="QGridLayout" name="gridLayout_135" columnstretch="0,1">
-                            <item row="0" column="0">
-                             <widget class="QPushButton" name="pushButton_presets_camera_tof_icp">
+                            <item row="1" column="1">
+                             <widget class="QLabel" name="label_753">
                               <property name="text">
-                               <string>TOF Camera ICP</string>
+                               <string>e.g., Velodyne, RoboSense and Ouster LiDARs.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                               </property>
                              </widget>
                             </item>
@@ -395,12 +401,46 @@
                             <item row="1" column="0">
                              <widget class="QPushButton" name="pushButton_presets_lidar_3d_icp">
                               <property name="text">
-                               <string>LiDAR 3D ICP</string>
+                               <string>LiDAR 3D ICP Legacy (5 cm voxel)</string>
                               </property>
                              </widget>
                             </item>
-                            <item row="1" column="1">
-                             <widget class="QLabel" name="label_753">
+                            <item row="2" column="0">
+                             <widget class="QPushButton" name="pushButton_presets_lidar_3d_icp_indoor">
+                              <property name="text">
+                               <string>LiDAR 3D ICP Indoor (10 cm voxel)</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="0" column="0">
+                             <widget class="QPushButton" name="pushButton_presets_camera_tof_icp">
+                              <property name="text">
+                               <string>TOF Camera ICP</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="0">
+                             <widget class="QPushButton" name="pushButton_presets_lidar_3d_icp_outdoor">
+                              <property name="text">
+                               <string>LiDAR 3D ICP Oudoor (50 cm voxel)</string>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="2" column="1">
+                             <widget class="QLabel" name="label_763">
+                              <property name="text">
+                               <string>e.g., Velodyne, RoboSense and Ouster LiDARs.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="3" column="1">
+                             <widget class="QLabel" name="label_764">
                               <property name="text">
                                <string>e.g., Velodyne, RoboSense and Ouster LiDARs.</string>
                               </property>
@@ -8465,6 +8505,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <string>VLP-16</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>Ouster SDK</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="0" column="1">
@@ -8761,7 +8806,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item>
                         <widget class="QStackedWidget" name="stackedWidget_lidar_src">
                          <property name="currentIndex">
-                          <number>1</number>
+                          <number>2</number>
                          </property>
                          <widget class="QWidget" name="page_98">
                           <layout class="QVBoxLayout" name="verticalLayout_177">
@@ -9033,6 +9078,243 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </item>
                           </layout>
                          </widget>
+                         <widget class="QWidget" name="page_100">
+                          <layout class="QVBoxLayout" name="verticalLayout_181">
+                           <item>
+                            <widget class="QGroupBox" name="groupBox_ouster">
+                             <property name="title">
+                              <string>Ouster SDK</string>
+                             </property>
+                             <layout class="QGridLayout" name="gridLayout_136" columnstretch="0,0,1">
+                              <item row="4" column="0">
+                               <widget class="QToolButton" name="toolButton_ouster_pcap_path">
+                                <property name="text">
+                                 <string>...</string>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="6" column="1">
+                               <widget class="QCheckBox" name="checkBox_ouster_reflectivity">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="5" column="2">
+                               <widget class="QLabel" name="label_760">
+                                <property name="text">
+                                 <string>Path to a *.JSON config file corresponding to PCAP recording.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="2">
+                               <widget class="QLabel" name="label_758">
+                                <property name="text">
+                                 <string>IP Address or Hostname.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="2" column="2">
+                               <widget class="QLabel" name="label_7482">
+                                <property name="toolTip">
+                                 <string>This can speedup deskewing.</string>
+                                </property>
+                                <property name="text">
+                                 <string>Lidar mode.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="1">
+                               <widget class="QLineEdit" name="lineEdit_ouster_ip_hostname"/>
+                              </item>
+                              <item row="3" column="1">
+                               <widget class="QComboBox" name="comboBox_ouster_timestamp">
+                                <property name="toolTip">
+                                 <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;TIME_FROM_INTERNAL_OSC: Use the internal clock.&lt;/p&gt;&lt;p&gt;TIME_FROM_SYNC_PULSE_IN: A free running counter synced to the SYNC_PULSE_IN input counts seconds (# of pulses) and nanoseconds since sensor turn on.&lt;/p&gt;&lt;p&gt;TIME_FROM_PTP_1588: Synchronize with an external PTP master.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                </property>
+                                <item>
+                                 <property name="text">
+                                  <string>Default</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>From Internal OSC</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>From Sync Pulse In</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>From PTP 1588</string>
+                                 </property>
+                                </item>
+                               </widget>
+                              </item>
+                              <item row="8" column="1">
+                               <spacer name="verticalSpacer_32">
+                                <property name="orientation">
+                                 <enum>Qt::Vertical</enum>
+                                </property>
+                                <property name="sizeHint" stdset="0">
+                                 <size>
+                                  <width>20</width>
+                                  <height>40</height>
+                                 </size>
+                                </property>
+                               </spacer>
+                              </item>
+                              <item row="4" column="2">
+                               <widget class="QLabel" name="label_759">
+                                <property name="text">
+                                 <string>Path to a *.PCAP or *.OSF file. IP is ignored if PCAP/OSF file is used. A JSON file should be provided if PCAP is used, see below.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="3" column="2">
+                               <widget class="QLabel" name="label_755">
+                                <property name="toolTip">
+                                 <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;If the lidar is not connected to a GPS, the lidar's clock may be not in sync with the host computer's clock. By enabling this, as soon as a packet is received, it will be stamped with host time &amp;quot;now&amp;quot;. Note that it affects only the final scan timsestamp, not the individual relative timestamp for each point. It is recommended to enable this if you combine it with other sensors above and it is not connected to a GPS.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                </property>
+                                <property name="text">
+                                 <string>Timestamp mode.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="2" column="1">
+                               <widget class="QComboBox" name="comboBox_ouster_lidar_mode">
+                                <property name="toolTip">
+                                 <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;MODE_512x10: 10 scans of 512 columns per second&lt;/p&gt;&lt;p&gt;MODE_512x20: 20 scans of 512 columns per second&lt;/p&gt;&lt;p&gt;MODE_1024x10: 10 scans of 1024 columns per second&lt;/p&gt;&lt;p&gt;MODE_1024x20: 20 scans of 1024 columns per second&lt;/p&gt;&lt;p&gt;MODE_2048x10: 10 scans of 2048 columns per second&lt;/p&gt;&lt;p&gt;MODE_4096x5: 5 scans of 4096 columns per second. Only available on select sensors&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                                </property>
+                                <item>
+                                 <property name="text">
+                                  <string>Default</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>512x10</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>512x20</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>1024x10</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>1024x20</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>2048x10</string>
+                                 </property>
+                                </item>
+                                <item>
+                                 <property name="text">
+                                  <string>4096x5</string>
+                                 </property>
+                                </item>
+                               </widget>
+                              </item>
+                              <item row="5" column="1">
+                               <widget class="QLineEdit" name="lineEdit_ouster_json_path">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="6" column="2">
+                               <widget class="QLabel" name="label_761">
+                                <property name="text">
+                                 <string>Use Reflectivity instead of Signal for intensity channel.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="4" column="1">
+                               <widget class="QLineEdit" name="lineEdit_ouster_pcap_path">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="5" column="0">
+                               <widget class="QToolButton" name="toolButton_ouster_json_path">
+                                <property name="text">
+                                 <string>...</string>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="7" column="1">
+                               <widget class="QCheckBox" name="checkBox_ouster_imu">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="7" column="2">
+                               <widget class="QLabel" name="label_762">
+                                <property name="text">
+                                 <string>Publish IMU.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                             </layout>
+                            </widget>
+                           </item>
+                          </layout>
+                         </widget>
                         </widget>
                        </item>
                       </layout>

tools/CMakeLists.txt

@@ -22,9 +22,7 @@ ENDIF(OPENCV_NONFREE_FOUND)
 
 IF(TARGET rtabmap_gui)
 	ADD_SUBDIRECTORY( CameraRGBD )
-	IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
-		ADD_SUBDIRECTORY( LidarViewer )
-	ENDIF()
+	ADD_SUBDIRECTORY( LidarViewer )
 	ADD_SUBDIRECTORY( DatabaseViewer )
 	ADD_SUBDIRECTORY( EpipolarGeometry )
 	ADD_SUBDIRECTORY( OdometryViewer )

tools/LidarViewer/main.cpp

@@ -37,7 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UConversion.h"
-#include <pcl/visualization/cloud_viewer.h>
 #include <stdio.h>
 #include <signal.h>
 
@@ -49,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/visualization/image_viewer.h>
 #include <pcl/console/parse.h>
 #include <rtabmap/core/lidar/LidarVLP16.h>
+#include <rtabmap/core/lidar/LidarOuster.h>
 
 using namespace pcl;
 using namespace pcl::console;
@@ -57,8 +57,14 @@ using namespace pcl::visualization;
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-lidar_viewer IP PORT driver\n"
-			"  driver       Driver number to use: 0=VLP16 (default IP and port are 192.168.1.201 2368)\n");
+			"rtabmap-lidar_viewer DRIVER IP [PORT]\n"
+			"\n"
+			"DRIVER: 0=VLP16, 1=Ouster\n"
+			"PORT:   should be set if DRIVER=0\n"
+			"\n"
+			"Examples:\n"
+			"  rtabmap-lidar_viewer 0 192.168.1.201 2368\n"
+			"  rtabmap-lidar_viewer 1 192.168.1.2\n\n");
 	exit(1);
 }
 
@@ -80,28 +86,45 @@ int main(int argc, char * argv[])
 	int driver = 0;
 	std::string ip;
 	int port = 2368;
-	if(argc < 4)
+	if(argc < 3)
 	{
+		printf("Not enough arguments.\n");
 		showUsage();
 	}
 	else
 	{
-		ip = argv[1];
-		port = uStr2Int(argv[2]);
-		driver = atoi(argv[3]);
-		if(driver < 0 || driver > 0)
+		driver = uStr2Int(argv[1]);
+		if(driver < 0 || driver > 1)
 		{
-			UERROR("driver should be 0.");
+			printf("driver should be 0 or 1.\n");
 			showUsage();
 		}
-	}
-	printf("Using driver %d (ip=%s port=%d)\n", driver, ip.c_str(), port);
 
-	rtabmap::LidarVLP16 * lidar = 0;
+		ip = argv[2];
+		if(driver==0)
+		{
+			if(argc>=3)
+			{
+				port = uStr2Int(argv[3]);
+			}
+			else
+			{
+				printf("IP and PORT should be set with VLP16 driver\n");
+				showUsage();
+			}
+		}
+	}
+	printf("Using driver %d (ip=%s%s)\n", driver, ip.c_str(), driver==0?uFormat(" port=%d", port).c_str():"");
+
+	rtabmap::Lidar * lidar = 0;
 	if(driver == 0)
 	{
 		lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string(ip), port);
 	}
+	else if(driver == 1)
+	{
+		lidar = new rtabmap::LidarOuster(ip);
+	}
 	else
 	{
 		UFATAL("");
@@ -109,7 +132,7 @@ int main(int argc, char * argv[])
 
 	if(!lidar->init())
 	{
-		printf("Lidar init failed! Please select another driver (see \"--help\").\n");
+		printf("Lidar init failed!\n");
 		delete lidar;
 		exit(1);
 	}
@@ -121,18 +144,18 @@ int main(int argc, char * argv[])
 	signal(SIGTERM, &sighandler);
 	signal(SIGINT, &sighandler);
 
-	rtabmap::SensorData data = lidar->takeScan();
+	rtabmap::SensorData data = lidar->takeData();
 	while(!data.laserScanRaw().empty() && (viewer==0 || !viewer->wasStopped()) && running)
 	{
 		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud = rtabmap::util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
-		viewer->showCloud(cloud, "cloud");
 		printf("Scan size: %ld points\n", cloud->size());
+		viewer->showCloud(cloud, "cloud");
 
 		int c = cv::waitKey(10); // wait 10 ms or for key stroke
 		if(c == 27)
 			break; // if ESC, break and quit
 
-		data = lidar->takeScan();
+		data = lidar->takeData();
 	}
 	printf("Closing...\n");
 	if(viewer)

utilite/include/rtabmap/utilite/USemaphore.h

@@ -60,10 +60,10 @@ public:
 	 */
 	USemaphore( int initValue = 0 )
 	{
-#ifdef _WIN32
-		S = CreateSemaphore(0,initValue,SEM_VALUE_MAX,0);
-#else
 		_available = initValue;
+#ifdef _WIN32
+		S = CreateSemaphoreW(0,initValue,SEM_VALUE_MAX,0);
+#else
 		pthread_mutex_init(&_waitMutex, NULL);
 		pthread_cond_init(&_cond, NULL);
 #endif
@@ -88,12 +88,15 @@ public:
 	 * @return true on success, false on error/timeout
 	 */
 #ifdef _WIN32
-	bool acquire(int n = 1, int ms = 0) const
+	bool acquire(int n = 1, int ms = 0)
 	{
 		int rt = 0;
 		while(n-- > 0 && rt==0)
 		{
 			rt = WaitForSingleObject((HANDLE)S, ms<=0?INFINITE:ms);
+			if (rt == 0) {
+				_available -= 1;
+			}
 		}
 		return rt == 0;
 	}
@@ -136,12 +139,17 @@ public:
 	 * @return false if the semaphore can't be taken without waiting (value <= 0), true otherwise
 	 */
 #ifdef _WIN32
-	int acquireTry() const
+	bool acquireTry()
 	{
-		return ((WaitForSingleObject((HANDLE)S,INFINITE)==WAIT_OBJECT_0)?0:EAGAIN);
+		if(WaitForSingleObject((HANDLE)S, INFINITE) == WAIT_OBJECT_0)
+		{
+			_available -= 1;
+			return true;
+		}
+		return false;
 	}
 #else
-	int acquireTry(int n)
+	bool acquireTry(int n)
 	{
 		pthread_mutex_lock(&_waitMutex);
 		if(n > _available)
@@ -160,9 +168,11 @@ public:
 	 * signaling waiting threads (which called acquire()).
 	 */
 #ifdef _WIN32
-	int release(int n = 1) const
+	void release(int n = 1)
 	{
-		return (ReleaseSemaphore((HANDLE)S,n,0)?0:ERANGE);
+		if (ReleaseSemaphore((HANDLE)S, n, 0)) {
+			_available += n;
+		}
 	}
 #else
 	void release(int n = 1)
@@ -181,7 +191,7 @@ public:
 #ifdef _WIN32
 	int value() const
 	{
-		LONG V = -1; ReleaseSemaphore((HANDLE)S,0,&V); return V;
+		return _available;
 	}
 #else
 	int value()
@@ -204,20 +214,21 @@ public:
 	{
 		CloseHandle(S);
 		S = CreateSemaphore(0,init,SEM_VALUE_MAX,0);
+		_available = init;
 	}
 #endif
 
 private:
 	void operator=(const USemaphore &){}
 #ifdef _WIN32
-	USemaphore(const USemaphore &S){}
+	USemaphore(const USemaphore &S) :S(0), _available(0) {}
 	HANDLE S;
 #else
 	USemaphore(const USemaphore &):_available(0){}
 	pthread_mutex_t _waitMutex;
 	pthread_cond_t _cond;
-	int _available;
 #endif
+	int _available;
 };
 
 #endif // USEMAPHORE_H