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

Fixed variance conversion from double to float with large numbers (over float max but not inf) (ref: http://answers.ros.org/question/221550/problem-running-rtabmap_ros-against-a-bag-file/)

Browse Source
This commit is contained in:
matlabbe
2015-11-27 11:54:18 -05:00
parent 542437c135
commit 8eea06c32b
2 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+4 -4
View File
@@ -629,8 +629,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
lastPose_ = odom;
lastPoseStamp_ = odomMsg->header.stamp;
double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
float transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
{
rotVariance_ = rotVariance;
@@ -1217,8 +1217,8 @@ void CoreWrapper::process(
const SensorData & data,
const Transform & odom,
const std::string & odomFrameId,
double odomRotationalVariance,
double odomTransitionalVariance)
float odomRotationalVariance,
float odomTransitionalVariance)
{
UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0)
src/CoreWrapper.h
+4 -4
View File
@@ -183,8 +183,8 @@ private:
const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
double odomRotationalVariance = 1.0,
double odomTransitionalVariance = 1.0);
float odomRotationalVariance = 1.0,
float odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -229,8 +229,8 @@ private:
bool paused_;
rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_;
double rotVariance_;
double transVariance_;
float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_;
bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_;