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 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/CoreWrapper.cpp
+41 -42
View File
@@ -83,8 +83,6 @@ CoreWrapper::CoreWrapper() :
paused_(false),
lastPose_(Transform::getIdentity()),
lastPoseIntermediate_(false),
rotVariance_(0),
transVariance_(0),
latestNodeWasReached_(false),
frameId_("base_link"),
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]));
rtabmap_.triggerNewMap();
rotVariance_ = 0;
transVariance_ = 0;
covariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
@@ -652,28 +649,29 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
// Only update variance if odom is not null
if(!odom.isNull())
{
// using MIN in case of 3DoF mapping (maybe no parameters are set, except x and yaw for the twist)
float transVariance = uMax3(odomMsg->twist.covariance[0], MIN(odomMsg->twist.covariance[7], BAD_COVARIANCE), MIN(odomMsg->twist.covariance[14], BAD_COVARIANCE));
float rotVariance = uMax3(MIN(odomMsg->twist.covariance[21],BAD_COVARIANCE), MIN(odomMsg->twist.covariance[28], BAD_COVARIANCE), odomMsg->twist.covariance[35]);
if(transVariance == BAD_COVARIANCE)
cv::Mat covariance;
float variance = odomMsg->twist.covariance[0];
if(variance == BAD_COVARIANCE)
{
//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
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);
covariance = cv::Mat(6,6,CV_64FC1, (void*)odomMsg->twist.covariance.data()).clone();
}
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(uIsFinite(transVariance) && transVariance != 1.0f)
{
transVariance_ += transVariance;
if(covariance_.empty())
{
covariance_ = covariance;
}
else
{
covariance_ += covariance;
}
}
}
@@ -724,8 +722,7 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp)
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
rtabmap_.triggerNewMap();
rotVariance_ = 0;
transVariance_ = 0;
covariance_ = cv::Mat();
}
lastPoseIntermediate_ = false;
@@ -957,10 +954,8 @@ void CoreWrapper::commonDepthCallbackImpl(
data,
lastPose_,
odomFrameId,
rotVariance_,
transVariance_);
rotVariance_ = 0;
transVariance_ = 0;
covariance_);
covariance_ = cv::Mat();
}
void CoreWrapper::commonStereoCallback(
@@ -1147,11 +1142,9 @@ void CoreWrapper::commonStereoCallback(
data,
lastPose_,
odomFrameId,
rotVariance_,
transVariance_);
covariance_);
rotVariance_ = 0;
transVariance_ = 0;
covariance_ = cv::Mat();
}
void CoreWrapper::process(
@@ -1159,8 +1152,7 @@ void CoreWrapper::process(
const SensorData & data,
const Transform & odom,
const std::string & odomFrameId,
float odomRotationalVariance,
float odomTransitionalVariance)
const cv::Mat & odomCovariance)
{
UTimer timer;
if(rtabmap_.isIDsGenerated() || data.id() > 0)
@@ -1169,16 +1161,25 @@ void CoreWrapper::process(
double timeUpdateMaps = 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_;
}
if(!uIsFinite(odomTransitionalVariance) || odomTransitionalVariance<=0.0f)
{
odomTransitionalVariance = odomDefaultLinVariance_;
covariance = cv::Mat::ones(6,6,CV_64FC1);
if(odomDefaultLinVariance_ > 0.0f)
{
covariance.at<double>(0,0) = 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();
mapToOdomMutex_.lock();
@@ -1543,8 +1544,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{
NODELET_INFO("rtabmap: Reset");
rtabmap_.resetMemory();
rotVariance_ = 0;
transVariance_ = 0;
covariance_ = cv::Mat();
lastPose_.setIdentity();
lastPoseIntermediate_ = false;
currentMetricGoal_.setNull();
@@ -1600,8 +1600,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
rtabmap_.close();
NODELET_INFO("Backup: Saving memory... done!");
rotVariance_ = 0;
transVariance_ = 0;
covariance_ = cv::Mat();
lastPose_.setIdentity();
currentMetricGoal_.setNull();
latestNodeWasReached_ = false;
src/DbPlayerNode.cpp
+9 -6
View File
@@ -163,9 +163,11 @@ int main(int argc, char** argv)
tf2_ros::TransformBroadcaster tfBroadcaster;
UTimer timer;
rtabmap::CameraInfo info;
rtabmap::SensorData data = reader.takeImage(&info);
rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance);
rtabmap::CameraInfo cameraInfo;
rtabmap::SensorData data = reader.takeImage(&cameraInfo);
rtabmap::OdometryInfo odomInfo;
odomInfo.covariance = cameraInfo.odomCovariance;
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id())
{
@@ -490,9 +492,10 @@ int main(int argc, char** argv)
}
timer.restart();
info = rtabmap::CameraInfo();
data = reader.takeImage(&info);
odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
cameraInfo = rtabmap::CameraInfo();
data = reader.takeImage(&cameraInfo);
odomInfo.covariance = cameraInfo.odomCovariance;
odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
acquisitionTime = timer.ticks();
}
src/GuiWrapper.cpp
+2 -2
View File
@@ -564,6 +564,7 @@ void GuiWrapper::commonDepthCallback(
return;
}
info.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
@@ -577,7 +578,6 @@ void GuiWrapper::commonDepthCallback(
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
@@ -720,6 +720,7 @@ void GuiWrapper::commonStereoCallback(
return;
}
info.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
@@ -733,7 +734,6 @@ void GuiWrapper::commonStereoCallback(
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
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)
{
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)
@@ -302,8 +303,10 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
msg.fromId = link.from();
msg.toId = link.to();
msg.type = link.type();
msg.rotVariance = link.rotVariance();
msg.transVariance = link.transVariance();
if(link.infMatrix().type() == CV_64FC1 && link.infMatrix().cols == 6 && link.infMatrix().rows == 6)
{
memcpy(msg.information.data(), link.infMatrix().data, 36*sizeof(double));
}
transformToGeometryMsg(link.transform(), msg.transform);
}
@@ -876,8 +879,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.matches = msg.matches;
info.inliers = msg.inliers;
info.icpInliersRatio = msg.icpInliersRatio;
info.varianceLin = msg.varianceLin;
info.varianceAng = msg.varianceAng;
info.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
info.features = msg.features;
info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize;
@@ -927,8 +929,10 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.matches = info.matches;
msg.inliers = info.inliers;
msg.icpInliersRatio = info.icpInliersRatio;
msg.varianceLin = info.varianceLin;
msg.varianceAng = info.varianceAng;
if(info.covariance.type() == CV_64FC1 && info.covariance.cols == 6 && info.covariance.rows == 6)
{
memcpy(msg.covariance.data(), info.covariance.data, 36*sizeof(double));
}
msg.features = info.features;
msg.localMapSize = info.localMapSize;
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
// 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());
}
}