LiDAR capture support in standalone library (#1264)

* Working rtabmap_lidar-mapping example (live and pcap)

* finalizing merge, added some deprecated

* fixed build

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

* backward compatibility

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

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

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

* Working external lidar on iOS

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

* fixed winsock include order

* reverted camera tool

* disable imu filtering when odom sensor is used

* Updated package version

* fixed windows build

* fixing more windows build erros

corelib/src/camera/CameraDepthAI.cpp

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

corelib/src/camera/CameraFreenect.cpp

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

corelib/src/camera/CameraFreenect2.cpp

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

corelib/src/camera/CameraImages.cpp

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

corelib/src/camera/CameraK4A.cpp

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

corelib/src/camera/CameraK4W2.cpp

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

corelib/src/camera/CameraMyntEye.cpp

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

corelib/src/camera/CameraOpenNI2.cpp

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

corelib/src/camera/CameraOpenNICV.cpp

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

corelib/src/camera/CameraOpenni.cpp

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

corelib/src/camera/CameraRGBDImages.cpp

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

corelib/src/camera/CameraRealSense.cpp

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

corelib/src/camera/CameraRealSense2.cpp

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

corelib/src/camera/CameraStereoDC1394.cpp

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

corelib/src/camera/CameraStereoFlyCapture2.cpp

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

corelib/src/camera/CameraStereoImages.cpp

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

corelib/src/camera/CameraStereoTara.cpp

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

corelib/src/camera/CameraStereoVideo.cpp

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

corelib/src/camera/CameraStereoZed.cpp

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

corelib/src/camera/CameraStereoZedOC.cpp

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

corelib/src/camera/CameraVideo.cpp

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