i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-12 04:29:49 +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
CMakeLists.txt
+1 -1
View File
@@ -18,7 +18,7 @@ find_package(rviz)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.12.5 REQUIRED) find_package(RTABMap 0.13.0 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
include/rtabmap_ros/CoreWrapper.h
+2 -4
View File
@@ -127,8 +127,7 @@ private:
const rtabmap::SensorData & data, const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(), const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "", const std::string & odomFrameId = "",
float odomRotationalVariance = 1.0, const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1));
float odomTransitionalVariance = 1.0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -175,8 +174,7 @@ private:
rtabmap::Transform lastPose_; rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_; ros::Time lastPoseStamp_;
bool lastPoseIntermediate_; bool lastPoseIntermediate_;
float rotVariance_; cv::Mat covariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_; rtabmap::Transform currentMetricGoal_;
bool latestNodeWasReached_; bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
msg/Link.msg
+2 -3
View File
@@ -4,12 +4,11 @@
# int to; # int to;
# Type type; # Type type;
# Transform transform; # Transform transform;
# float variance; # cv::Mat(6,6,CV_64FC1) information;
#} #}
int32 fromId int32 fromId
int32 toId int32 toId
int32 type int32 type
geometry_msgs/Transform transform geometry_msgs/Transform transform
float32 rotVariance float64[36] information
float32 transVariance
msg/OdomInfo.msg
+1 -2
View File
@@ -5,8 +5,7 @@ bool lost
int32 matches int32 matches
int32 inliers int32 inliers
float32 icpInliersRatio float32 icpInliersRatio
float32 varianceLin float64[36] covariance
float32 varianceAng
int32 features int32 features
int32 localMapSize int32 localMapSize
int32 localScanMapSize int32 localScanMapSize
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<package> <package>
<name>rtabmap_ros</name> <name>rtabmap_ros</name>
<version>0.12.2</version> <version>0.13.0</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description> <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer> <maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author> <author>Mathieu Labbe</author>
src/CoreWrapper.cpp
+41 -42
View File
@@ -83,8 +83,6 @@ CoreWrapper::CoreWrapper() :
paused_(false), paused_(false),
lastPose_(Transform::getIdentity()), lastPose_(Transform::getIdentity()),
lastPoseIntermediate_(false), lastPoseIntermediate_(false),
rotVariance_(0),
transVariance_(0),
latestNodeWasReached_(false), latestNodeWasReached_(false),
frameId_("base_link"), frameId_("base_link"),
odomFrameId_(""), odomFrameId_(""),
@@ -641,8 +639,7 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
{ {
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0])); UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0]));
rtabmap_.triggerNewMap(); rtabmap_.triggerNewMap();
rotVariance_ = 0; covariance_ = cv::Mat();
transVariance_ = 0;
} }
lastPoseIntermediate_ = false; lastPoseIntermediate_ = false;
@@ -652,28 +649,29 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
// Only update variance if odom is not null // Only update variance if odom is not null
if(!odom.isNull()) if(!odom.isNull())
{ {
// using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist) cv::Mat covariance;
float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE)); float variance = odomMsg->twist.covariance[0];
float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]); if(variance == BAD_COVARIANCE)
if(transVariance == BAD_COVARIANCE)
{ {
//use the one of the pose //use the one of the pose
transVariance = uMax3(odomMsg->pose.covariance[0]/2.0, MIN(odomMsg->pose.covariance[7]/2.0, BAD_COVARIANCE), MIN(odomMsg->pose.covariance[14]/2.0, BAD_COVARIANCE)); covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->pose.covariance.data()).clone();
covariance /= 2.0;
} }
if(rotVariance == BAD_COVARIANCE) else
{ {
//use the one of the pose covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
rotVariance = uMax3(MIN(odomMsg->pose.covariance[21]/2.0,BAD_COVARIANCE), MIN(odomMsg->pose.covariance[28]/2.0, BAD_COVARIANCE), odomMsg->pose.covariance[35]/2.0);
} }
if(uIsFinite(rotVariance) && rotVariance != 1.0f) if(uIsFinite(covariance.at<double>(0,0)) && covariance.at<double>(0,0) != 1.0 && covariance.at<double>(0,0)>0.0)
{ {
rotVariance_ += rotVariance; if(covariance_.empty())
} {
if(uIsFinite(transVariance) && transVariance != 1.0f) covariance_ = covariance;
{ }
transVariance_ += transVariance; else
{
covariance_ += covariance;
}
} }
} }
@@ -724,8 +722,7 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
{ {
UWARN("Odometry is reset (identity pose detected). Increment map id!"); UWARN("Odometry is reset (identity pose detected). Increment map id!");
rtabmap_.triggerNewMap(); rtabmap_.triggerNewMap();
rotVariance_ = 0; covariance_ = cv::Mat();
transVariance_ = 0;
} }
lastPoseIntermediate_ = false; lastPoseIntermediate_ = false;
@@ -957,10 +954,8 @@ void CoreWrapper::commonDepthCallbackImpl(
data, data,
lastPose_, lastPose_,
odomFrameId, odomFrameId,
rotVariance_, covariance_);
transVariance_); covariance_ = cv::Mat();
rotVariance_ = 0;
transVariance_ = 0;
} }
void CoreWrapper::commonStereoCallback( void CoreWrapper::commonStereoCallback(
@@ -1147,11 +1142,9 @@ void CoreWrapper::commonStereoCallback(
data, data,
lastPose_, lastPose_,
odomFrameId, odomFrameId,
rotVariance_, covariance_);
transVariance_);
rotVariance_ = 0; covariance_ = cv::Mat();
transVariance_ = 0;
} }
void CoreWrapper::process( void CoreWrapper::process(
@@ -1159,8 +1152,7 @@ void CoreWrapper::process(
const SensorData & data, const SensorData & data,
const Transform & odom, const Transform & odom,
const std::string & odomFrameId, const std::string & odomFrameId,
float odomRotationalVariance, const cv::Mat & odomCovariance)
float odomTransitionalVariance)
{ {
UTimer timer; UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0) if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1169,16 +1161,25 @@ void CoreWrapper::process(
double timeUpdateMaps = 0.0; double timeUpdateMaps = 0.0;
double timePublishMaps = 0.0; double timePublishMaps = 0.0;
if(!uIsFinite(odomRotationalVariance) || odomRotationalVariance<=0.0f) cv::Mat covariance = odomCovariance;
if(covariance.empty() || !uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{ {
odomRotationalVariance = odomDefaultAngVariance_; covariance = cv::Mat::ones(6,6,CV_64FC1);
} if(odomDefaultLinVariance_ > 0.0f)
if(!uIsFinite(odomTransitionalVariance) || odomTransitionalVariance<=0.0f) {
{ covariance.at<double>(0,0) = odomDefaultLinVariance_;
odomTransitionalVariance = odomDefaultLinVariance_; covariance.at<double>(1,1) = odomDefaultLinVariance_;
covariance.at<double>(2,2) = odomDefaultLinVariance_;
}
if(odomDefaultAngVariance_ > 0.0f)
{
covariance.at<double>(3,3) = odomDefaultAngVariance_;
covariance.at<double>(4,4) = odomDefaultAngVariance_;
covariance.at<double>(5,5) = odomDefaultAngVariance_;
}
} }
if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance))) if(rtabmap_.process(data, odom, covariance))
{ {
timeRtabmap = timer.ticks(); timeRtabmap = timer.ticks();
mapToOdomMutex_.lock(); mapToOdomMutex_.lock();
@@ -1543,8 +1544,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{ {
NODELET_INFO("rtabmap: Reset"); NODELET_INFO("rtabmap: Reset");
rtabmap_.resetMemory(); rtabmap_.resetMemory();
rotVariance_ = 0; covariance_ = cv::Mat();
transVariance_ = 0;
lastPose_.setIdentity(); lastPose_.setIdentity();
lastPoseIntermediate_ = false; lastPoseIntermediate_ = false;
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
@@ -1600,8 +1600,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
rtabmap_.close(); rtabmap_.close();
NODELET_INFO("Backup: Saving memory... done!"); NODELET_INFO("Backup: Saving memory... done!");
rotVariance_ = 0; covariance_ = cv::Mat();
transVariance_ = 0;
lastPose_.setIdentity(); lastPose_.setIdentity();
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
src/DbPlayerNode.cpp
+9 -6
View File
@@ -163,9 +163,11 @@ int main(int argc, char** argv)
tf2_ros::TransformBroadcaster tfBroadcaster; tf2_ros::TransformBroadcaster tfBroadcaster;
UTimer timer; UTimer timer;
rtabmap::CameraInfo info; rtabmap::CameraInfo cameraInfo;
rtabmap::SensorData data = reader.takeImage(&info); rtabmap::SensorData data = reader.takeImage(&cameraInfo);
rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance); rtabmap::OdometryInfo odomInfo;
odomInfo.covariance = cameraInfo.odomCovariance;
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
double acquisitionTime = timer.ticks(); double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id()) while(ros::ok() && odom.data().id())
{ {
@@ -490,9 +492,10 @@ int main(int argc, char** argv)
} }
timer.restart(); timer.restart();
info = rtabmap::CameraInfo(); cameraInfo = rtabmap::CameraInfo();
data = reader.takeImage(&info); data = reader.takeImage(&cameraInfo);
odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance); odomInfo.covariance = cameraInfo.odomCovariance;
odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
acquisitionTime = timer.ticks(); acquisitionTime = timer.ticks();
} }
src/GuiWrapper.cpp
+2 -2
View File
@@ -564,6 +564,7 @@ void GuiWrapper::commonDepthCallback(
return; return;
} }
info.covariance = covariance;
rtabmap::OdometryEvent odomEvent( rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData( rtabmap::SensorData(
scan, scan,
@@ -577,7 +578,6 @@ void GuiWrapper::commonDepthCallback(
odomHeader.seq, odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)), rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info); info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -720,6 +720,7 @@ void GuiWrapper::commonStereoCallback(
return; return;
} }
info.covariance = covariance;
rtabmap::OdometryEvent odomEvent( rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData( rtabmap::SensorData(
scan, scan,
@@ -733,7 +734,6 @@ void GuiWrapper::commonStereoCallback(
odomHeader.seq, odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)), rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT, odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info); info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData)); QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
src/MsgConversion.cpp
+11 -7
View File
@@ -294,7 +294,8 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg) rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
{ {
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.rotVariance, msg.transVariance); cv::Mat information = cv::Mat(6,6,CV_64FC1, (void*)msg.information.data()).clone();
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), information);
} }
void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg) void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
@@ -302,8 +303,10 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
msg.fromId = link.from(); msg.fromId = link.from();
msg.toId = link.to(); msg.toId = link.to();
msg.type = link.type(); msg.type = link.type();
msg.rotVariance = link.rotVariance(); if(link.infMatrix().type() == CV_64FC1 && link.infMatrix().cols == 6 && link.infMatrix().rows == 6)
msg.transVariance = link.transVariance(); {
memcpy(msg.information.data(), link.infMatrix().data, 36*sizeof(double));
}
transformToGeometryMsg(link.transform(), msg.transform); transformToGeometryMsg(link.transform(), msg.transform);
} }
@@ -876,8 +879,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.matches = msg.matches; info.matches = msg.matches;
info.inliers = msg.inliers; info.inliers = msg.inliers;
info.icpInliersRatio = msg.icpInliersRatio; info.icpInliersRatio = msg.icpInliersRatio;
info.varianceLin = msg.varianceLin; info.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
info.varianceAng = msg.varianceAng;
info.features = msg.features; info.features = msg.features;
info.localMapSize = msg.localMapSize; info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize; info.localScanMapSize = msg.localScanMapSize;
@@ -927,8 +929,10 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.matches = info.matches; msg.matches = info.matches;
msg.inliers = info.inliers; msg.inliers = info.inliers;
msg.icpInliersRatio = info.icpInliersRatio; msg.icpInliersRatio = info.icpInliersRatio;
msg.varianceLin = info.varianceLin; if(info.covariance.type() == CV_64FC1 && info.covariance.cols == 6 && info.covariance.rows == 6)
msg.varianceAng = info.varianceAng; {
memcpy(msg.covariance.data(), info.covariance.data, 36*sizeof(double));
}
msg.features = info.features; msg.features = info.features;
msg.localMapSize = info.localMapSize; msg.localMapSize = info.localMapSize;
msg.localScanMapSize = info.localScanMapSize; msg.localScanMapSize = info.localScanMapSize;
src/OdometryROS.cpp
+15 -15
View File
@@ -431,12 +431,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
//set covariance //set covariance
// libviso2 uses approximately vel variance * 2 // libviso2 uses approximately vel variance * 2
odom.pose.covariance.at(0) = info.varianceLin*2; // xx odom.pose.covariance.at(0) = info.covariance.at<double>(0,0)*2; // xx
odom.pose.covariance.at(7) = info.varianceLin*2; // yy odom.pose.covariance.at(7) = info.covariance.at<double>(1,1)*2; // yy
odom.pose.covariance.at(14) = info.varianceLin*2; // zz odom.pose.covariance.at(14) = info.covariance.at<double>(2,2)*2; // zz
odom.pose.covariance.at(21) = info.varianceAng*2; // rr odom.pose.covariance.at(21) = info.covariance.at<double>(3,3)*2; // rr
odom.pose.covariance.at(28) = info.varianceAng*2; // pp odom.pose.covariance.at(28) = info.covariance.at<double>(4,4)*2; // pp
odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw odom.pose.covariance.at(35) = info.covariance.at<double>(5,5)*2; // yawyaw
//set velocity //set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull(); 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.twist.angular.z = yaw;
} }
odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE; // xx odom.twist.covariance.at(0) = setTwist?info.covariance.at<double>(0,0):BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE; // yy odom.twist.covariance.at(7) = setTwist?info.covariance.at<double>(1,1):BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz odom.twist.covariance.at(14) = setTwist?info.covariance.at<double>(2,2):BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr odom.twist.covariance.at(21) = setTwist?info.covariance.at<double>(3,3):BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp odom.twist.covariance.at(28) = setTwist?info.covariance.at<double>(4,4):BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw odom.twist.covariance.at(35) = setTwist?info.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
//publish the message //publish the message
odomPub_.publish(odom); odomPub_.publish(odom);
@@ -608,16 +608,16 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{ {
if(icpParams_) 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 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 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());
} }
} }