i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

27
corelib/src/Registration.cpp
View File

@@ -33,7 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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)
{
@@ -237,18 +238,18 @@ Transform Registration::computeTransformationMod(
info.covariance = cv::Mat::eye(6,6,CV_64FC1);
}
if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
info.covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(info.covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
info.covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(infoOut)

24
corelib/src/RegistrationVis.cpp
View File

@@ -1668,18 +1668,18 @@ Transform RegistrationVis::computeTransformationImpl(
for(int i=0;i<2;++i)
{
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
}
cv::Mat cov = covariances[0].clone();

4
corelib/src/Rtabmap.cpp
View File

@@ -3225,6 +3225,10 @@ bool Rtabmap::process(
UDEBUG(" to %s", newT.prettyPrint().c_str());
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(selfLinks.begin(), selfLinks.end());

17
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -494,6 +494,23 @@ Transform OdometryF2M::computeTransform(
info->gravityRollError = fabs(rollImu - roll);
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());