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

2
CMakeLists.txt
View File

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
####################### #######################
SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 11) SET(RTABMAP_MINOR_VERSION 11)
SET(RTABMAP_PATCH_VERSION 8) SET(RTABMAP_PATCH_VERSION 9)
SET(RTABMAP_VERSION SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -499,7 +499,7 @@ public:
static ParametersMap deserialize(const std::string & parameters); static ParametersMap deserialize(const std::string & parameters);
static bool isFeatureParameter(const std::string & param); static bool isFeatureParameter(const std::string & param);
static ParametersMap getDefaultOdometryParameters(bool stereo = false); static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
static ParametersMap getDefaultParameters(const std::string & group); static ParametersMap getDefaultParameters(const std::string & group);
static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group); static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);

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) Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
{ {
UASSERT(!data.imageRaw().empty());
// Ground alignment // Ground alignment
if(_pose.isIdentity() && _alignWithGround) 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; double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess = dt && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform(); 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()); 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( output = registrationPipeline_->computeTransformationMod(
tmpRefFrame, tmpRefFrame,
newFrame, 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); &regInfo);
if(info && this->isInfoDataFilled()) if(info && this->isInfoDataFilled())

3
corelib/src/OdometryF2M.cpp
View File

@@ -213,7 +213,8 @@ Transform OdometryF2M::computeTransform(
Transform transform = regPipeline_->computeTransformationMod( Transform transform = regPipeline_->computeTransformationMod(
tmpMap, tmpMap,
*lastFrame_, *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); &regInfo);
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors()); 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; 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 odomParameters;
rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters(); rtabmap::ParametersMap defaultParameters = rtabmap::Parameters::getDefaultParameters();
@@ -168,9 +168,10 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo)
std::string group = uSplit(iter->first, '/').front(); std::string group = uSplit(iter->first, '/').front();
if(uStrContains(group, "Odom") || if(uStrContains(group, "Odom") ||
(stereo && group.compare("Stereo") == 0) || (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("Reg") == 0 ||
group.compare("Vis") == 0) (vis && group.compare("Vis") == 0))
{ {
if(stereo) if(stereo)
{ {

2
package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>rtabmap</name> <name>rtabmap</name>
<version>0.11.8</version> <version>0.11.9</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="matlabbe@gmail.com">Mathieu Labbe</maintainer> <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <author>Mathieu Labbe</author>