Working rtabmap_lidar-mapping example (live and pcap)

app/android/jni/CameraARCore.cpp

@@ -334,7 +334,7 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
 	}
 }
 
-SensorData CameraARCore::captureImage(CameraInfo * info)
+SensorData CameraARCore::captureImage(SensorCaptureInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");

app/android/jni/CameraARCore.h

@@ -78,7 +78,7 @@ class CameraARCore : public CameraMobile {
 	void imageCallback(AImageReader *reader);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0); // should be called in opengl thread
 	virtual void capturePoseOnly();
 
 private:

app/android/jni/CameraAREngine.cpp

@@ -169,7 +169,7 @@ void CameraAREngine::close()
 	CameraMobile::close();
 }
 
-SensorData CameraAREngine::captureImage(CameraInfo * info)
+SensorData CameraAREngine::captureImage(SensorCaptureInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");

app/android/jni/CameraAREngine.h

@@ -53,7 +53,7 @@ class CameraAREngine : public CameraMobile {
 	virtual std::string getSerial() const;
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 	virtual void capturePoseOnly();
 
 private:

app/android/jni/CameraMobile.cpp

@@ -241,7 +241,7 @@ void CameraMobile::mainLoopBegin()
 
 void CameraMobile::mainLoop()
 {
-	CameraInfo info;
+	SensorCaptureInfo info;
 	SensorData data = this->captureImage(&info);
 
 	if(data.isValid() && !info.odomPose.isNull())
@@ -393,7 +393,7 @@ void CameraMobile::mainLoop()
 	}
 }
 
-SensorData CameraMobile::captureImage(CameraInfo * info)
+SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 {
 	if(info)
 	{

app/android/jni/CameraMobile.h

@@ -122,7 +122,7 @@ class CameraMobile : public Camera, public UThread, public UEventsSender {
     const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 	virtual void capturePoseOnly() {}
 
 	virtual void mainLoopBegin();

app/android/jni/CameraTango.cpp

@@ -775,7 +775,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	return pose;
 }
 
-SensorData CameraTango::captureImage(CameraInfo * info)
+SensorData CameraTango::captureImage(SensorCaptureInfo * info)
 {
 	//LOGI("Capturing image...");
 

app/android/jni/CameraTango.h

@@ -61,7 +61,7 @@ class CameraTango : public CameraMobile {
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);

corelib/include/rtabmap/core/Camera.h

@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -28,17 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/CameraInfo.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-class UTimer;
+#include <rtabmap/core/SensorCapture.h>
 
 namespace rtabmap
 {
@@ -47,50 +37,23 @@ namespace rtabmap
  * Class Camera
  *
  */
-class RTABMAP_EXP Camera
+class RTABMAP_EXP Camera : public SensorCapture
 {
 public:
-	virtual ~Camera();
-	SensorData takeImage(CameraInfo * info = 0);
+	virtual ~Camera() {}
 
 	bool initFromFile(const std::string & calibrationPath);
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
-	virtual std::string getSerial() const = 0;
-	virtual bool odomProvided() const { return false; }
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
-
-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
 
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-
-	void resetTimer();
 protected:
 	/**
 	 * Constructor
 	 *
 	 * @param imageRate the frame rate (Hz), 0 for fast as the camera can
 	 * @param localTransform the transform from base frame to camera frame (without optical rotation)
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
-
-	/**
-	 * returned rgb and depth images should be already rectified if calibration was loaded
-	 */
-	virtual SensorData captureImage(CameraInfo * info = 0) = 0;
-
-	int getNextSeqID() {return ++_seq;}
-
-private:
-	float _imageRate;
-	Transform _localTransform;
-	cv::Size _targetImageSize;
-	UTimer * _frameRateTimer;
-	int _seq;
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()) {}
 };
 
 

corelib/include/rtabmap/core/DBReader.h

@@ -84,10 +84,10 @@ class RTABMAP_EXP DBReader : public Camera {
 	const DBDriver * driver() const {return _dbDriver;}
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
 private:
-	SensorData getNextData(CameraInfo * info = 0);
+	SensorData getNextData(SensorCaptureInfo * info = 0);
 
 private:
 	std::list<std::string> _paths;

corelib/include/rtabmap/core/LaserScan.h

@@ -47,14 +47,16 @@ class RTABMAP_EXP LaserScan
 		kXYZRGB=7,
 		kXYZNormal=8,
 		kXYZINormal=9,
-		kXYZRGBNormal=10};
+		kXYZRGBNormal=10,
+		kXYZIT=11};
 
 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
 	static bool isScan2d(const Format & format);
 	static bool isScanHasNormals(const Format & format);
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
+	static bool isScanHasTime(const Format & format);
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
@@ -123,18 +125,22 @@ class RTABMAP_EXP LaserScan
 
 	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
-	int size() const {return data_.cols;}
+	int size() const {return data_.total();}
 	int dataType() const {return data_.type();}
 	bool is2d() const {return isScan2d(format_);}
 	bool hasNormals() const {return isScanHasNormals(format_);}
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
+	bool hasTime() const {return isScanHasTime(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
+	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
+	LaserScan densify() const;
 
 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+	int getTimeOffset() const {return hasTime()?4:-1;}
 
 	float & field(unsigned int pointIndex, unsigned int channelOffset);
 

corelib/include/rtabmap/core/Odometry.h

@@ -110,6 +110,7 @@ class RTABMAP_EXP Odometry
 	bool _alignWithGround;
 	bool _publishRAMUsage;
 	bool _imagesAlreadyRectified;
+	bool _deskewing;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/Parameters.h

@@ -455,6 +455,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
     RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation if provided).");
 
     // Odometry Frame-to-Map
     RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -667,6 +668,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
     RTABMAP_PARAM(Icp, Force4DoF,                 bool, false,   uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM(Icp, FiltersEnabled,            int, 3,       "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
 #ifdef RTABMAP_POINTMATCHER
     RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -68,6 +68,7 @@ class RTABMAP_EXP RegistrationIcp : public Registration
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _force4DoF;
+	int _filtersEnabled;
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;

corelib/include/rtabmap/core/SensorCapture.h

@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include "rtabmap/core/SensorData.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_EXP SensorCapture
+{
+public:
+	virtual ~SensorCapture();
+	SensorData takeImage(SensorCaptureInfo * info = 0);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
+
+	//getters
+	float getImageRate() const {return _imageRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+	//setters
+	void setImageRate(float imageRate) {_imageRate = imageRate;}
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
+
+	void resetTimer();
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param imageRate the frame rate (Hz), 0 for fast as the camera can
+	 * @param localTransform the transform from base frame to sensor frame
+	 */
+	SensorCapture(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;
+
+	int getNextSeqID() {return ++_seq;}
+
+private:
+	float _imageRate;
+	Transform _localTransform;
+	UTimer * _frameRateTimer;
+	int _seq;
+};
+
+
+} // namespace rtabmap

corelib/include/rtabmap/core/CameraInfo.h → corelib/include/rtabmap/core/SensorCaptureInfo.h

@@ -27,16 +27,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
+#include "rtabmap/core/Transform.h"
 #include <string>
 
 namespace rtabmap
 {
 
-class CameraInfo
+class SensorCaptureInfo
 {
 
 public:
-	CameraInfo() :
+	SensorCaptureInfo() :
 		cameraName(""),
 		id(0),
 		stamp(0.0),
@@ -52,7 +53,7 @@ class CameraInfo
 		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
 	{
 	}
-	virtual ~CameraInfo() {}
+	virtual ~SensorCaptureInfo() {}
 
 	std::string cameraName;
 	int id;