Integrated Fovis (Odom/Strategy=2)

corelib/include/rtabmap/core/Odometry.h

@@ -45,7 +45,8 @@ public:
 	enum Type {
 		kTypeUndef = -1,
 		kTypeF2M = 0,
-		kTypeF2F = 1
+		kTypeF2F = 1,
+		kTypeFovis = 2
 	};
 
 public:

corelib/include/rtabmap/core/OdometryFovis.h

@@ -0,0 +1,67 @@
+/*
+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 ODOMETRYFOVIS_H_
+#define ODOMETRYFOVIS_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace fovis {
+class VisualOdometry;
+class Rectification;
+class StereoCalibration;
+class DepthImage;
+class StereoDepth;
+}
+
+namespace rtabmap {
+
+class Registration;
+
+class RTABMAP_EXP OdometryFovis : public Odometry
+{
+public:
+	OdometryFovis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryFovis();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeFovis;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	fovis::VisualOdometry * fovis_;
+	fovis::Rectification * rect_;
+	fovis::StereoCalibration * stereoCalib_;
+	fovis::DepthImage * depthImage_;
+	fovis::StereoDepth * stereoDepth_;
+};
+
+}
+
+#endif /* ODOMETRYFOVIS_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -363,7 +363,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)");
+	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis");
 	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).");