Added OdometryFLOAM (Odom/Strategy=11)

corelib/include/rtabmap/core/Odometry.h

@@ -54,7 +54,8 @@ public:
 		kTypeLOAM = 7,
 		kTypeMSCKF = 8,
 		kTypeVINS = 9,
-		kTypeOpenVINS = 10
+		kTypeOpenVINS = 10,
+		kTypeFLOAM = 11
 	};
 
 public:

corelib/include/rtabmap/core/Parameters.h

@@ -432,7 +432,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
 
     // Odometry
-    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion");
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM");
     RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
     RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
     RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");

corelib/include/rtabmap/core/odometry/OdometryFLOAM.h

@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYFLOAM_H_
+#define ODOMETRYFLOAM_H_
+
+#include <rtabmap/core/Odometry.h>
+
+class LaserProcessingClass;
+class OdomEstimationClass;
+
+namespace rtabmap {
+
+class RTABMAP_EXP OdometryFLOAM : public Odometry
+{
+public:
+	OdometryFLOAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryFLOAM();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeFLOAM;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+#ifdef RTABMAP_FLOAM
+	LaserProcessingClass * laserProcessing_;
+	OdomEstimationClass * odomEstimation_;
+
+	Transform lastPose_;
+	bool lost_;
+	float linVar_;
+	float angVar_;
+#endif
+};
+
+}
+
+#endif /* ODOMETRYFLOAM_H_ */

corelib/src/CMakeLists.txt

@@ -89,6 +89,7 @@ SET(SRC_FILES
     odometry/OdometryOkvis.cpp
     odometry/OdometryORBSLAM.cpp
     odometry/OdometryLOAM.cpp
+    odometry/OdometryFLOAM.cpp
     odometry/OdometryMSCKF.cpp
     odometry/OdometryVINS.cpp
     odometry/OdometryOpenVINS.cpp
@@ -489,6 +490,17 @@ IF(loam_velodyne_FOUND)
 	)
 ENDIF(loam_velodyne_FOUND)
 
+IF(floam_FOUND)
+    SET(INCLUDE_DIRS 
+        ${INCLUDE_DIRS} 
+        ${floam_INCLUDE_DIRS}
+    )
+    SET(LIBRARIES
+        ${LIBRARIES}
+        ${floam_LIBRARIES}
+    )
+ENDIF(floam_FOUND)
+
 IF(ZED_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}

corelib/src/Odometry.cpp

@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/odometry/OdometryOkvis.h"
 #include "rtabmap/core/odometry/OdometryORBSLAM.h"
 #include "rtabmap/core/odometry/OdometryLOAM.h"
+#include "rtabmap/core/odometry/OdometryFLOAM.h"
 #include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/odometry/OdometryVINS.h"
 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
@@ -90,6 +91,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeLOAM:
 		odometry = new OdometryLOAM(parameters);
 		break;
+	case Odometry::kTypeFLOAM:
+		odometry = new OdometryFLOAM(parameters);
+		break;
 	case Odometry::kTypeMSCKF:
 		odometry = new OdometryMSCKF(parameters);
 		break;

corelib/src/camera/CameraStereoImages.cpp

@@ -83,6 +83,8 @@ CameraStereoImages::~CameraStereoImages()
 
 bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
+	UINFO("Calibration folder: \"%s\", name=\"%s\"", calibrationFolder.c_str(), cameraName.c_str());
+
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
@@ -105,8 +107,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	stereoModel_.setName(cameraName);
 	if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
 	{
-		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
-		return false;
+		UWARN("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid for rectification. This can be ignored if input images are already rectified.");
 	}
 
 	//desactivate before init as we will do it in this class instead for convenience

corelib/src/odometry/OdometryFLOAM.cpp

@@ -0,0 +1,194 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/odometry/OdometryFLOAM.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util2d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/core/util3d.h"
+#include <pcl/common/transforms.h>
+
+#ifdef RTABMAP_FLOAM
+#include <laserProcessingClass.h>
+#include <odomEstimationClass.h>
+#endif
+
+namespace rtabmap {
+
+/**
+ * https://github.com/wh200720041/floam
+ */
+
+OdometryFLOAM::OdometryFLOAM(const ParametersMap & parameters) :
+	Odometry(parameters)
+#ifdef RTABMAP_FLOAM
+	,laserProcessing_(new LaserProcessingClass())
+	,odomEstimation_(new OdomEstimationClass())
+	,lastPose_(Transform::getIdentity())
+	,lost_(false)
+#endif
+{
+#ifdef RTABMAP_FLOAM
+	int sensor = Parameters::defaultOdomLOAMSensor();
+	double vertical_angle = 2.0; // seems not used by floam (https://github.com/wh200720041/floam/issues/31)
+	float scan_period= Parameters::defaultOdomLOAMScanPeriod();
+	float max_dis = Parameters::defaultIcpRangeMax();
+	float min_dis = Parameters::defaultIcpRangeMin();
+	float map_resolution = Parameters::defaultIcpVoxelSize();
+	linVar_ = Parameters::defaultOdomLOAMLinVar();
+	angVar_ = Parameters::defaultOdomLOAMAngVar();
+
+	Parameters::parse(parameters, Parameters::kOdomLOAMSensor(), sensor);
+	Parameters::parse(parameters, Parameters::kOdomLOAMScanPeriod(), scan_period);
+	Parameters::parse(parameters, Parameters::kIcpRangeMax(), max_dis);
+	Parameters::parse(parameters, Parameters::kIcpRangeMin(), min_dis);
+	Parameters::parse(parameters, Parameters::kIcpVoxelSize(), map_resolution);
+
+	UASSERT(scan_period>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMLinVar(), linVar_);
+	UASSERT(linVar_>0.0f);
+	Parameters::parse(parameters, Parameters::kOdomLOAMAngVar(), angVar_);
+	UASSERT(angVar_>0.0f);
+
+	lidar::Lidar lidar_param;
+	lidar_param.setScanPeriod(scan_period);
+	lidar_param.setVerticalAngle(vertical_angle);
+	lidar_param.setLines(sensor==2?64:sensor==1?32:sensor);
+	lidar_param.setMaxDistance(max_dis<=0?200:max_dis);
+	lidar_param.setMinDistance(min_dis);
+
+	laserProcessing_->init(lidar_param);
+	odomEstimation_->init(lidar_param, map_resolution);
+#endif
+}
+
+OdometryFLOAM::~OdometryFLOAM()
+{
+#ifdef RTABMAP_FLOAM
+	delete laserProcessing_;
+	delete odomEstimation_;
+#endif
+}
+
+void OdometryFLOAM::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_FLOAM
+	lastPose_.setIdentity();
+	lost_ = false;
+#endif
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryFLOAM::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	Transform t;
+#ifdef RTABMAP_FLOAM
+	UTimer timer;
+
+	if(data.laserScanRaw().isEmpty())
+	{
+		UERROR("LOAM works only with laser scans and the current input is empty. Aborting odometry update...");
+		return t;
+	}
+	else if(data.laserScanRaw().is2d())
+	{
+		UERROR("LOAM version used works only with 3D laser scans from Velodyne. Aborting odometry update...");
+		return t;
+	}
+
+	cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
+	if(!lost_)
+	{
+		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudInPtr = util3d::laserScanToPointCloudI(data.laserScanRaw(), data.laserScanRaw().localTransform());
+
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_edge(new pcl::PointCloud<pcl::PointXYZI>());
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_surf(new pcl::PointCloud<pcl::PointXYZI>());
+
+		laserProcessing_->featureExtraction(laserCloudInPtr,pointcloud_edge,pointcloud_surf);
+
+		if(this->framesProcessed() == 0){
+			odomEstimation_->initMapWithPoints(pointcloud_edge, pointcloud_surf);
+		}else{
+			odomEstimation_->updatePointsToMap(pointcloud_edge, pointcloud_surf);
+		}
+
+		Transform pose = Transform::fromEigen3d(odomEstimation_->odom);
+
+		if(!pose.isNull())
+		{
+			covariance = cv::Mat::eye(6,6,CV_64FC1);
+			covariance(cv::Range(0,3), cv::Range(0,3)) *= 0.01;
+			covariance(cv::Range(3,6), cv::Range(3,6)) *= 0.01;
+
+			t = lastPose_.inverse() * pose; // incremental
+			lastPose_ = pose;
+
+			const Transform & localTransform = data.laserScanRaw().localTransform();
+			if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+			{
+				// from laser frame to base frame
+				t = localTransform * t * localTransform.inverse();
+			}
+
+			if(info)
+			{
+				info->type = (int)kTypeLOAM;
+				if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
+				{
+					info->reg.covariance = covariance;
+				}
+
+				if(this->isInfoDataFilled())
+				{
+					pcl::PointCloud<pcl::PointXYZI>::Ptr localMap(new pcl::PointCloud<pcl::PointXYZI>());
+					odomEstimation_->getMap(localMap);
+					info->localScanMapSize = localMap->size();
+					info->localScanMap = LaserScan(util3d::laserScanFromPointCloud(*localMap), 0, data.laserScanRaw().rangeMax());
+				}
+			}
+		}
+		else
+		{
+			lost_ = true;
+			UWARN("FLOAM failed to register the latest scan, odometry should be reset.");
+		}
+	}
+	UINFO("Odom update time = %fs, lost=%s", timer.elapsed(), lost_?"true":"false");
+
+#else
+	UERROR("RTAB-Map is not built with FLOAM support! Select another odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap

corelib/src/odometry/OdometryLOAM.cpp

@@ -34,8 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include <pcl/common/transforms.h>
 
-float SCAN_PERIOD = 0.1f;
-
 namespace rtabmap {
 
 /**
@@ -177,7 +175,7 @@ std::vector<pcl::PointCloud<pcl::PointXYZI> > OdometryLOAM::segmentScanRings(con
 		}
 
 		// calculate relative scan time based on point orientation
-		float relTime = SCAN_PERIOD * (ori - startOri) / (endOri - startOri);
+		float relTime = scanPeriod_ * (ori - startOri) / (endOri - startOri);
 		point.intensity = scanID + relTime;
 
 		// imu not used...