fix build errors with opencv2, added Transform::getClosestTransform() function for convenience.

corelib/include/rtabmap/core/IMU.h

@@ -65,7 +65,6 @@ public:
 		return localTransform_.isNull();
 	}
 
-
 private:
 	cv::Vec4d orientation_;
 	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set

corelib/include/rtabmap/core/Transform.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/RtabmapExp.h>
 #include <vector>
 #include <string>
+#include <map>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <opencv2/core/core.hpp>
@@ -149,6 +150,11 @@ public:
 	static Transform fromString(const std::string & string);
 	static bool canParseString(const std::string & string);
 
+	static Transform getClosestTransform(
+				const std::map<double, Transform> & tfBuffer,
+				const double & stamp,
+				double * stampDiff = 0);
+
 private:
 	cv::Mat data_;
 };

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

@@ -56,7 +56,6 @@ public:
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
-	Transform getClosestIMU(const double & stamp, double & stampDiff) const;
 
 private:
 	//Parameters

corelib/src/Transform.cpp

@@ -467,4 +467,41 @@ bool Transform::canParseString(const std::string & string)
 	return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
 }
 
+Transform Transform::getClosestTransform(
+				const std::map<double, Transform> & tfBuffer,
+				const double & stamp,
+				double * stampDiff)
+{
+	UASSERT(!tfBuffer.empty());
+	std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
+	std::map<double, Transform>::const_iterator imuIterA = imuIterB;
+	if(imuIterA != tfBuffer.begin())
+	{
+		imuIterA = --imuIterA;
+	}
+	if(imuIterB == tfBuffer.end())
+	{
+		imuIterB = --imuIterB;
+	}
+	Transform imuT;
+	if(imuIterB->first == stamp || imuIterA == imuIterB)
+	{
+		imuT = imuIterB->second;
+		if(stampDiff)
+		{
+			*stampDiff = fabs(imuIterB->first - stamp);
+		}
+	}
+	else if(imuIterA != imuIterB)
+	{
+		//interpolate:
+		imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
+		if(stampDiff)
+		{
+			*stampDiff = 0.0;
+		}
+	}
+	return imuT;
+}
+
 }

corelib/src/camera/CameraVideo.cpp

@@ -30,6 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #if CV_MAJOR_VERSION > 3
 #include <opencv2/videoio/videoio_c.h>
+#if CV_MAJOR_VERSION > 4
+#include <opencv2/videoio/legacy/constants_c.h>
+#endif
 #endif
 
 namespace rtabmap
@@ -131,13 +134,13 @@ bool CameraVideo::init(const std::string & calibrationFolder, const std::string
 		{
 			if(_model.isValidForProjection())
 			{
-				_capture.set(cv::CAP_PROP_FRAME_WIDTH, _model.imageWidth());
+				_capture.set(CV_CAP_PROP_FRAME_WIDTH, _model.imageWidth());
-				_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
+				_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _model.imageHeight());
 			}
 			else if(_width > 0 && _height > 0)
 			{
-				_capture.set(cv::CAP_PROP_FRAME_WIDTH, _width);
+				_capture.set(CV_CAP_PROP_FRAME_WIDTH, _width);
-				_capture.set(cv::CAP_PROP_FRAME_HEIGHT, _height);
+				_capture.set(CV_CAP_PROP_FRAME_HEIGHT, _height);
 			}
 		}
 		if(_rectifyImages && !_model.isValidForRectification())

corelib/src/odometry/OdometryF2M.cpp

@@ -335,8 +335,7 @@ Transform OdometryF2M::computeTransform(
 					Transform imuT;
 					if(!imus_.empty())
 					{
-						double stampDiff = 0.0;
+						imuT = Transform::getClosestTransform(imus_, lastFrame_->getStamp());
-						imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff);
 					}
 
 					// local bundle adjustment
@@ -1304,43 +1303,4 @@ Transform OdometryF2M::computeTransform(
 	return output;
 }
 
-Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) const
-{
-	UASSERT(!imus_.empty());
-	std::map<double, Transform>::const_iterator imuIterB = imus_.lower_bound(stamp);
-	std::map<double, Transform>::const_iterator imuIterA = imuIterB;
-	if(imuIterA != imus_.begin())
-	{
-		imuIterA = --imuIterA;
-	}
-	if(imuIterB == imus_.end())
-	{
-		imuIterB = --imuIterB;
-	}
-	Transform imuT;
-	stampDiff = 0.0;
-	if(imuIterB->first == lastFrame_->getStamp() || imuIterA == imuIterB)
-	{
-		imuT = imuIterB->second;
-		stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
-	}
-	else if(imuIterA != imuIterB)
-	{
-		if(fabs(imuIterA->first - lastFrame_->getStamp()) <
-		   fabs(imuIterB->first - lastFrame_->getStamp()))
-		{
-			//imuT = imuIterA->second;
-			stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
-		}
-		else
-		{
-			//imuT = imuIterB->second;
-			stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
-		}
-		//interpolate:
-		imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
-	}
-	return imuT;
-}
-
 } // namespace rtabmap

tools/EurocDataset/main.cpp

@@ -439,9 +439,7 @@ int main(int argc, char * argv[])
 
 						SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu);
 						cameraThread.postUpdate(&dataImu);
-						UDEBUG("");
 						odom->process(dataImu);
-						UDEBUG("");
 					}
 
 				} while (t_imu <= data.stamp());