i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2017-07-07 19:41:11 -04:00
parent f9ed36ed54
commit 9e011d6ad9
9 changed files with 51 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+3 -5
View File
@@ -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")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native")
# There could be crash issues if built with RealSense
MESSAGE(WARNING "Don't forget to build ORB_SLAM2 (and included g2o) without \"-march=native\" to avoid crash when ORB_SLAM2 starts.")
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
+4 -3
View File
@@ -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})
ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY)
SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${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
+1
View File
@@ -58,6 +58,7 @@ private:
dvo::core::RgbdCameraPyramid * camera_;
bool lost_;
Transform motionFromKeyFrame_;
Transform previousLocalTransform_;
};
corelib/include/rtabmap/core/OdometryFovis.h
+1
View File
@@ -60,6 +60,7 @@ private:
fovis::StereoDepth * stereoDepth_;
ParametersMap fovisParameters_;
bool lost_;
Transform previousLocalTransform_;
};
}
corelib/include/rtabmap/core/OdometryORBSLAM2.h
+1
View File
@@ -54,6 +54,7 @@ private:
ORBSLAM2System * orbslam2_;
ORB_SLAM2::System * system_;
bool firstFrame_;
Transform originLocalTransform_;
};
corelib/src/OdometryDVO.cpp
+16 -6
View File
@@ -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())
{
// from camera frame to base frame
t = localTransform * t * localTransform.inverse();
if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
{
// from camera frame to base frame
if(!previousLocalTransform_.isNull())
{
t = previousLocalTransform_ * t * localTransform.inverse();
}
else
{
t = localTransform * t * localTransform.inverse();
}
previousLocalTransform_ = localTransform;
}
}
if(info)
corelib/src/OdometryFovis.cpp
+12 -1
View File
@@ -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
+12 -5
View File
@@ -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
t = localTransform * t.inverse() * localTransform.inverse();
if(originLocalTransform_.isNull())
{
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
+1
View File
@@ -1603,6 +1603,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
{
_ui->odom_registration->setCurrentIndex(3);
updateOdometryVisibility();
}
}
}