i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-11 04:19:50 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bump 0.20.19. OdomF2M: scale x0.1 covariance when local bundle adjustment is done. Rtabmap: fixed GT localization error when RGBD/MaxOdomCacheSize is used.

Browse Source
This commit is contained in:
matlabbe
2022-02-06 17:41:53 -05:00
parent 9ad6b626e4
commit b44b212218
8 changed files with 55 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
####################### #######################
SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 20) SET(RTABMAP_MINOR_VERSION 20)
SET(RTABMAP_PATCH_VERSION 18) SET(RTABMAP_PATCH_VERSION 19)
SET(RTABMAP_VERSION SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/Registration.h
+2 -1
View File
@@ -45,7 +45,8 @@ public:
kTypeIcp = 1, kTypeIcp = 1,
kTypeVisIcp = 2 kTypeVisIcp = 2
}; };
static double COVARIANCE_EPSILON; static double COVARIANCE_LINEAR_EPSILON;
static double COVARIANCE_ANGULAR_EPSILON;
public: public:
static Registration * create(const ParametersMap & parameters); static Registration * create(const ParametersMap & parameters);
corelib/src/Registration.cpp
+14 -13
View File
@@ -33,7 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap { namespace rtabmap {
double Registration::COVARIANCE_EPSILON = 0.000000001; double Registration::COVARIANCE_LINEAR_EPSILON = 0.00000001; // 0.1 mm
double Registration::COVARIANCE_ANGULAR_EPSILON = 0.00000003; // 0.01 deg
Registration * Registration::create(const ParametersMap & parameters) Registration * Registration::create(const ParametersMap & parameters)
{ {
@@ -237,18 +238,18 @@ Transform Registration::computeTransformationMod(
info.covariance = cv::Mat::eye(6,6,CV_64FC1); info.covariance = cv::Mat::eye(6,6,CV_64FC1);
} }
if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON) if(info.covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform info.covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(infoOut) if(infoOut)
corelib/src/RegistrationVis.cpp
+12 -12
View File
@@ -1668,18 +1668,18 @@ Transform RegistrationVis::computeTransformationImpl(
for(int i=0;i<2;++i) for(int i=0;i<2;++i)
{ {
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1); UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON) if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
} }
cv::Mat cov = covariances[0].clone(); cv::Mat cov = covariances[0].clone();
corelib/src/Rtabmap.cpp
+4
View File
@@ -3225,6 +3225,10 @@ bool Rtabmap::process(
UDEBUG(" to %s", newT.prettyPrint().c_str()); UDEBUG(" to %s", newT.prettyPrint().c_str());
iter->second.setTransform(newT); iter->second.setTransform(newT);
// Update link in the referred signatures
if(iter->first > 0)
_memory->updateLink(iter->second, false);
_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second)); _odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
} }
_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end()); _odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
corelib/src/odometry/OdometryF2M.cpp
+17
View File
@@ -494,6 +494,23 @@ Transform OdometryF2M::computeTransform(
info->gravityRollError = fabs(rollImu - roll); info->gravityRollError = fabs(rollImu - roll);
info->gravityPitchError = fabs(pitchImu - pitch); info->gravityPitchError = fabs(pitchImu - pitch);
} }
// With bundle adjustment, scale down covariance by 10
UASSERT(regInfo.covariance.cols==6 && regInfo.covariance.rows == 6 && regInfo.covariance.type() == CV_64FC1);
double thrLin = Registration::COVARIANCE_LINEAR_EPSILON*10.0;
double thrAng = Registration::COVARIANCE_ANGULAR_EPSILON*10.0;
if(regInfo.covariance.at<double>(0,0)>thrLin)
regInfo.covariance.at<double>(0,0) *= 0.1;
if(regInfo.covariance.at<double>(1,1)>thrLin)
regInfo.covariance.at<double>(1,1) *= 0.1;
if(regInfo.covariance.at<double>(2,2)>thrLin)
regInfo.covariance.at<double>(2,2) *= 0.1;
if(regInfo.covariance.at<double>(3,3)>thrAng)
regInfo.covariance.at<double>(3,3) *= 0.1;
if(regInfo.covariance.at<double>(4,4)>thrAng)
regInfo.covariance.at<double>(4,4) *= 0.1;
if(regInfo.covariance.at<double>(5,5)>thrAng)
regInfo.covariance.at<double>(5,5) *= 0.1;
} }
} }
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str()); UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
guilib/src/MainWindow.cpp
+4 -4
View File
@@ -607,8 +607,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false); _ui->statsToolBox->updateStat("Odometry/ICPStructuralDistribution/", false);
_ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false); _ui->statsToolBox->updateStat("Odometry/ICPCorrespondences/", false);
_ui->statsToolBox->updateStat("Odometry/ICPRMS/", false); _ui->statsToolBox->updateStat("Odometry/ICPRMS/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false); _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false); _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", false);
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false); _ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false); _ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
@@ -1718,9 +1718,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().timeParticleFiltering>0.0f) if(odom.info().timeParticleFiltering>0.0f)
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>rtabmap</name> <name>rtabmap</name>
<version>0.20.18</version> <version>0.20.19</version>
<description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description> <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer> <maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <author>Mathieu Labbe</author>