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

Fixed build for rtabmap 0.13.0

Browse Source
This commit is contained in:
matlabbe
2017-05-23 16:31:06 -04:00
parent 77f0172942
commit f81edcf790
10 changed files with 85 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+15 -15
View File
@@ -431,12 +431,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.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
odom.pose.covariance.at(0) = info.covariance.at<double>(0,0)*2; // xx
odom.pose.covariance.at(7) = info.covariance.at<double>(1,1)*2; // yy
odom.pose.covariance.at(14) = info.covariance.at<double>(2,2)*2; // zz
odom.pose.covariance.at(21) = info.covariance.at<double>(3,3)*2; // rr
odom.pose.covariance.at(28) = info.covariance.at<double>(4,4)*2; // pp
odom.pose.covariance.at(35) = info.covariance.at<double>(5,5)*2; // yawyaw
//set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -452,12 +452,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.twist.twist.angular.z = yaw;
}
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
odom.twist.covariance.at(0) = setTwist?info.covariance.at<double>(0,0):BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.covariance.at<double>(1,1):BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.covariance.at<double>(2,2):BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.covariance.at<double>(3,3):BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.covariance.at<double>(4,4):BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
//publish the message
odomPub_.publish(odom);
@@ -608,16 +608,16 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(icpParams_)
{
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());
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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
else
{
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());
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}
else
{
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());
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}