Ouster SDK integration

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 @@ class RTABMAP_CORE_EXPORT IMUFilter
 		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