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

Fixed build with changes from 0.11.13

Browse Source
This commit is contained in:
matlabbe
2017-01-03 23:26:26 -05:00
parent f6a29a3e64
commit 3fb6a82825
5 changed files with 25 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+4 -2
View File
@@ -879,7 +879,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize;
info.timeEstimation = msg.timeEstimation;
info.variance = msg.variance;
info.varianceLin = msg.varianceLin;
info.varianceAng = msg.varianceAng;
info.timeParticleFiltering = msg.timeParticleFiltering;
info.stamp = msg.stamp;
info.interval = msg.interval;
@@ -923,7 +924,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localMapSize = info.localMapSize;
msg.localScanMapSize = info.localScanMapSize;
msg.timeEstimation = info.timeEstimation;
msg.variance = info.variance;
msg.varianceLin = info.varianceLin;
msg.varianceAng = info.varianceAng;
msg.timeParticleFiltering = info.timeParticleFiltering;
msg.stamp = info.stamp;
msg.interval = info.interval;
src/OdometryROS.cpp
+15 -15
View File
@@ -443,12 +443,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
//set covariance
// libviso2 uses approximately vel variance * 2
odom.pose.covariance.at(0) = info.variance*2; // xx
odom.pose.covariance.at(7) = info.variance*2; // yy
odom.pose.covariance.at(14) = info.variance*2; // zz
odom.pose.covariance.at(21) = info.variance*2; // rr
odom.pose.covariance.at(28) = info.variance*2; // pp
odom.pose.covariance.at(35) = info.variance*2; // yawyaw
odom.pose.covariance.at(0) = info.varianceLin*2; // xx
odom.pose.covariance.at(7) = info.varianceLin*2; // yy
odom.pose.covariance.at(14) = info.varianceLin*2; // zz
odom.pose.covariance.at(21) = info.varianceAng*2; // rr
odom.pose.covariance.at(28) = info.varianceAng*2; // pp
odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw
//set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -464,12 +464,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.twist.twist.angular.z = yaw;
}
odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw
//publish the message
odomPub_.publish(odom);
@@ -620,16 +620,16 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(icpParams_)
{
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.inliers, info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
}
else
{
NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
}
}
else
{
NODELET_INFO( "Odom: ratio=%f, std dev=%fm, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.varianceLin), pose.isNull()?0.0f:std::sqrt(info.varianceAng), (ros::WallTime::now()-time).toSec());
}
}