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

rtabmap 0.15.3 now required. icp_odometry: added scan_downsampling_step parameter. OdometryROS: fixed too old tf stamps when guess and minimum motion are set.

Browse Source
This commit is contained in:
matlabbe
2017-12-13 18:16:07 -05:00
parent 5115b9a96b
commit ea5707e267
4 changed files with 68 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -18,7 +18,7 @@ find_package(rviz)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.14.0 REQUIRED)
find_package(RTABMap 0.15.3 REQUIRED)
find_package(OpenCV REQUIRED)
include/rtabmap_ros/OdometryROS.h
+2
View File
@@ -127,6 +127,8 @@ private:
bool stereoParams_;
bool visParams_;
bool icpParams_;
rtabmap::Transform guess_;
double guessStamp_;
};
}
src/OdometryROS.cpp
+24 -9
View File
@@ -77,7 +77,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
resetCurrentCount_(0),
stereoParams_(stereoParams),
visParams_(visParams),
icpParams_(icpParams)
icpParams_(icpParams),
guessStamp_(0.0)
{
}
@@ -397,20 +398,25 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
}
Transform guess;
Transform guessCurrentPose;
if(!guessFrameId_.empty())
{
Transform previousPose = this->getTransform(guessFrameId_, frameId_, odometry_->previousStamp()>0.0?ros::Time(odometry_->previousStamp()):stamp);
Transform previousPose = this->getTransform(guessFrameId_, frameId_, guessStamp_>0.0?ros::Time(guessStamp_):stamp);
guessCurrentPose = this->getTransform(guessFrameId_, frameId_, stamp);
if(!previousPose.isNull() && !guessCurrentPose.isNull())
{
guess = previousPose.inverse() * guessCurrentPose;
if(odometry_->previousStamp()>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
if(guess_.isNull())
{
guess_ = previousPose.inverse() * guessCurrentPose;
}
else
{
guess_ = guess_ * previousPose.inverse() * guessCurrentPose;
}
if(guessStamp_>0.0 && (guessMinTranslation_ > 0.0 || guessMinRotation_ > 0.0))
{
float x,y,z,roll,pitch,yaw;
guess.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) &&
(guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_))
{
@@ -421,13 +427,15 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
correctionMsg.child_frame_id = guessFrameId_;
correctionMsg.header.frame_id = odomFrameId_;
correctionMsg.header.stamp = stamp;
Transform correction = odometry_->getPose() * guess * guessCurrentPose.inverse();
Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
tfBroadcaster_.sendTransform(correctionMsg);
}
guessStamp_ = stamp.toSec();
return;
}
}
guessStamp_ = stamp.toSec();
}
else
{
@@ -440,7 +448,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, guess, &info);
rtabmap::Transform pose = odometry_->process(dataCpy, guess_, &info);
guess_.setNull();
if(!pose.isNull())
{
resetCurrentCount_ = resetCountdown_;
@@ -682,6 +691,9 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
NODELET_INFO( "visual_odometry: reset odom!");
odometry_->reset();
guess_.setNull();
guessStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
this->flushCallbacks();
return true;
}
@@ -691,6 +703,9 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
guess_.setNull();
guessStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
this->flushCallbacks();
return true;
}
src/nodelets/icp_odometry.cpp
+41 -4
View File
@@ -58,6 +58,7 @@ public:
ICPOdometry() :
OdometryROS(false, false, true),
scanCloudMaxPoints_(0),
scanDownsamplingStep_(1),
scanVoxelSize_(0.0),
scanNormalK_(0),
scanNormalRadius_(0.0)
@@ -76,6 +77,7 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle();
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
@@ -86,10 +88,11 @@ private:
}
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_downsampling_step = %d", scanDownsamplingStep_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -106,6 +109,24 @@ private:
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "1"));
ros::NodeHandle & pnh = getPrivateNodeHandle();
iter = parameters.find(Parameters::kIcpDownsamplingStep());
if(iter != parameters.end())
{
int value = uStr2Int(iter->second);
if(value > 1)
{
if(!pnh.hasParam("scan_downsampling_step"))
{
ROS_WARN("IcpOdometry: Transferring value %s of \"%s\" to ros parameter \"scan_downsampling_step\" for convenience. \"%s\" is set to 0.", iter->second.c_str(), iter->first.c_str(), iter->first.c_str());
scanDownsamplingStep_ = value;
iter->second = "1";
}
else
{
ROS_WARN("IcpOdometry: Both parameter \"%s\" and ros parameter \"scan_downsampling_step\" are set.", iter->first.c_str());
}
}
}
iter = parameters.find(Parameters::kIcpVoxelSize());
if(iter != parameters.end())
{
@@ -176,6 +197,11 @@ private:
int maxLaserScans = (int)scanMsg->ranges.size();
if(pclScan->size())
{
if(scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
@@ -245,6 +271,11 @@ private:
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
@@ -255,6 +286,11 @@ private:
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *pclScan);
if(pclScan->size() && scanDownsamplingStep_ > 1)
{
pclScan = util3d::downsample(pclScan, scanDownsamplingStep_);
maxLaserScans /= scanDownsamplingStep_;
}
if(!pclScan->is_dense)
{
pclScan = util3d::removeNaNFromPointCloud(pclScan);
@@ -306,6 +342,7 @@ private:
ros::Subscriber scan_sub_;
ros::Subscriber cloud_sub_;
int scanCloudMaxPoints_;
int scanDownsamplingStep_;
double scanVoxelSize_;
int scanNormalK_;
double scanNormalRadius_;