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

Fixed ICP-only odometry when guess from motion is enabled

Browse Source
This commit is contained in:
matlabbe
2016-08-07 20:54:56 -04:00
parent 93f1ae501c
commit 6ef9034a67
7 changed files with 11 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/Odometry.cpp
View File

@@ -200,8 +200,6 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
{
UASSERT(!data.imageRaw().empty());
// Ground alignment
if(_pose.isIdentity() && _alignWithGround)
{
@@ -258,13 +256,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if(!data.stereoCameraModel().isValidForProjection() &&
(data.cameraModels().size() == 0 || !data.cameraModels()[0].isValidForProjection()))
{
UERROR("Rectified images required! Calibrate your camera.");
return Transform();
}
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
UASSERT_MSG(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()), uFormat("dt=%f previous transform=%s", dt, previousVelocityTransform_.prettyPrint().c_str()).c_str());

3
corelib/src/OdometryF2F.cpp
View File

@@ -96,7 +96,8 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod(
tmpRefFrame,
newFrame,
!guess.isNull()?motionSinceLastKeyFrame*guess:Transform(),
// special case for ICP-only odom, set guess to identity if we just started
!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
&regInfo);
if(info && this->isInfoDataFilled())

3
corelib/src/OdometryF2M.cpp
View File

@@ -213,7 +213,8 @@ Transform OdometryF2M::computeTransform(
Transform transform = regPipeline_->computeTransformationMod(
tmpMap,
*lastFrame_,
guess.isNull()?Transform():this->getPose()*guess,
// special case for ICP-only odom, set guess to identity if we just started
!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
&regInfo);
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());

7
corelib/src/Parameters.cpp
View File

@@ -159,7 +159,7 @@ bool Parameters::isFeatureParameter(const std::string & parameter)
group.compare("BRISK") == 0;
}
rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo)
rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, bool vis, bool icp)
{
rtabmap::ParametersMap odomParameters;
rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters();
@@ -168,9 +168,10 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo)
std::string group = uSplit(iter->first, '/').front();
if(uStrContains(group, "Odom") ||
(stereo && group.compare("Stereo") == 0) ||
Parameters::isFeatureParameter(iter->first) ||
(icp && group.compare("Icp") == 0) ||
(vis && Parameters::isFeatureParameter(iter->first)) ||
group.compare("Reg") == 0 ||
group.compare("Vis") == 0)
(vis && group.compare("Vis") == 0))
{
if(stereo)
{