Fixed changing local transform not handled correctly when changing for third party odometries

CMakeLists.txt

@@ -369,7 +369,7 @@ IF(WITH_DVO)
     ENDIF(dvo_core_FOUND)
 ENDIF(WITH_DVO)
 
-IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
+IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM2 QUIET)
     IF(ORB_SLAM2_FOUND)
        MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_INCLUDE_DIRS}")
@@ -381,12 +381,10 @@ IF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
           MESSAGE(STATUS "Found Pangolin: ${Pangolin_INCLUDE_DIRS}")
           SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
           SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARIES} ${Pangolin_LIBRARIES})
-          set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -march=native")
+          MESSAGE(WARNING "Don't forget to build ORB_SLAM2 (and included g2o) without \"-march=native\" to avoid crash when ORB_SLAM2 starts.")
-          set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
-          # There could be crash issues if built with RealSense
        ENDIF()
     ENDIF(ORB_SLAM2_FOUND)
-ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND AND NOT RealSense_FOUND)
+ENDIF(WITH_ORB_SLAM2 AND NOT G2O_FOUND)
 
 IF(G2O_FOUND OR GTSAM_FOUND OR ZED_FOUND OR ANDROID OR RealSense_FOUND OR ORB_SLAM2_FOUND)
    #Newest versions require std11

cmake_modules/FindORB_SLAM2.cmake

@@ -9,12 +9,13 @@
 
 find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
 find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib)
 
-IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
    SET(ORB_SLAM2_FOUND TRUE)
    SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
-   SET(ORB_SLAM2_LIBRARIES ${ORB_SLAM2_LIBRARY})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY})
-ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND g2o_LIBRARY)
 
 IF (ORB_SLAM2_FOUND)
    # show which ORB_SLAM2 was found only if not quiet

corelib/include/rtabmap/core/OdometryDVO.h

@@ -58,6 +58,7 @@ private:
 	dvo::core::RgbdCameraPyramid * camera_;
 	bool lost_;
 	Transform motionFromKeyFrame_;
+	Transform previousLocalTransform_;
 
 };
 

corelib/include/rtabmap/core/OdometryFovis.h

@@ -60,6 +60,7 @@ private:
 	fovis::StereoDepth * stereoDepth_;
 	ParametersMap fovisParameters_;
 	bool lost_;
+	Transform previousLocalTransform_;
 };
 
 }

corelib/include/rtabmap/core/OdometryORBSLAM2.h

@@ -54,6 +54,7 @@ private:
 	ORBSLAM2System * orbslam2_;
 	ORB_SLAM2::System * system_;
 	bool firstFrame_;
+	Transform originLocalTransform_;
 
 };
 

corelib/src/OdometryDVO.cpp

@@ -90,6 +90,7 @@ void OdometryDVO::reset(const Transform & initialPose)
 	}
 	lost_ = false;
 	motionFromKeyFrame_.setIdentity();
+	previousLocalTransform_.setNull();
 #endif
 }
 
@@ -192,6 +193,7 @@ Transform OdometryDVO::computeTransform(
 
 	dvo::core::RgbdImagePyramid * current = new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
 
+	const Transform & localTransform = data.cameraModels()[0].localTransform();
 	cv::Mat covariance;
 	if(reference_ == 0)
 	{
@@ -245,16 +247,24 @@ Transform OdometryDVO::computeTransform(
 			reference_ = 0; // this will make restart from the next frame
 			motionFromKeyFrame_.setIdentity();
 			t.setNull();
+			previousLocalTransform_.setNull();
 			covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
 			UWARN("dvo failed to estimate motion, tracking will be reinitialized on next frame.");
 		}
-	}
 
-	const Transform & localTransform = data.cameraModels()[0].localTransform();
+		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
-	if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
+		{
-	{
+			// from camera frame to base frame
-		// from camera frame to base frame
+			if(!previousLocalTransform_.isNull())
-		t = localTransform * t * localTransform.inverse();
+			{
+				t = previousLocalTransform_ * t * localTransform.inverse();
+			}
+			else
+			{
+				t = localTransform * t * localTransform.inverse();
+			}
+			previousLocalTransform_ = localTransform;
+		}
 	}
 
 	if(info)

corelib/src/OdometryFovis.cpp

@@ -111,6 +111,7 @@ void OdometryFovis::reset(const Transform & initialPose)
 		stereoDepth_ = 0;
 	}
 	lost_ = false;
+	previousLocalTransform_.setNull();
 #endif
 }
 
@@ -341,12 +342,14 @@ Transform OdometryFovis::computeTransform(
 		t.setNull();
 		lost_ = true;
 		covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
+		previousLocalTransform_.setNull();
 	}
 	else if(lost_)
 	{
 		lost_ = false;
 		// we are not lost anymore but we don't know where we are now according to last valid pose
 		covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
+		previousLocalTransform_.setNull();
 	}
 	else
 	{
@@ -362,7 +365,15 @@ Transform OdometryFovis::computeTransform(
 	if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
 	{
 		// from camera frame to base frame
-		t = localTransform * t * localTransform.inverse();
+		if(!previousLocalTransform_.isNull())
+		{
+			t = previousLocalTransform_ * t * localTransform.inverse();
+		}
+		else
+		{
+			t = localTransform * t * localTransform.inverse();
+		}
+		previousLocalTransform_ = localTransform;
 	}
 
 	if(info)

corelib/src/OdometryORBSLAM2.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UDirectory.h"
 
 #ifdef RTABMAP_ORB_SLAM2
 #include <System.h>
@@ -495,6 +496,7 @@ public:
 		if(!vocabularyPath.empty())
 		{
 			//Load ORB Vocabulary
+			vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
 			UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
 			mpVocabulary = new ORB_SLAM2::ORBVocabulary();
 			bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
@@ -773,6 +775,7 @@ void OdometryORBSLAM2::reset(const Transform & initialPose)
 		orbslam2_->shutdown();
 	}
 	firstFrame_ = true;
+	originLocalTransform_.setNull();
 #endif
 }
 
@@ -845,14 +848,17 @@ Transform OdometryORBSLAM2::computeTransform(
 	}
 	else if(Tcw.cols == 4 && Tcw.rows == 4)
 	{
-		t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)).clone());
+		t = Transform(cv::Mat(Tcw, cv::Range(0,3), cv::Range(0,4)));
 
 		if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
 		{
-			// from camera frame to base frame
+			if(originLocalTransform_.isNull())
-			t = localTransform * t.inverse() * localTransform.inverse();
+			{
+				originLocalTransform_ = localTransform;
+			}
+			t = originLocalTransform_ * t.inverse() * localTransform.inverse();
+			t = this->getPose().inverse() * t;
 		}
-		t = this->getPose().inverse() * t;
 
 		if(firstFrame_)
 		{
@@ -916,12 +922,13 @@ Transform OdometryORBSLAM2::computeTransform(
 			}
 			info->wordMatches.resize(oi);
 			info->wordInliers.resize(oi);
+			info->inliers = oi;
 
 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
 			{
 				cv::Mat pt = mapPoints[i]->GetWorldPos();
-				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), localTransform)));
+				info->localMap.insert(std::make_pair(mapPoints[i]->mnId, util3d::transformPoint(cv::Point3f(pt), originLocalTransform_)));
 			}
 		}
 	}

guilib/src/PreferencesDialog.cpp

@@ -1603,6 +1603,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
 		{
 			_ui->odom_registration->setCurrentIndex(3);
+			updateOdometryVisibility();
 		}
 	}
 }