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

Updated API for OdomInfo object/conversion.

Browse Source
This commit is contained in:
matlabbe
2017-09-11 13:26:34 -04:00
parent fe968e5d40
commit aee8e0c645
5 changed files with 41 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/DbPlayerNode.cpp
+2 -2
View File
@@ -166,7 +166,7 @@ int main(int argc, char** argv)
rtabmap::CameraInfo cameraInfo;
rtabmap::SensorData data = reader.takeImage(&cameraInfo);
rtabmap::OdometryInfo odomInfo;
odomInfo.covariance = cameraInfo.odomCovariance;
odomInfo.reg.covariance = cameraInfo.odomCovariance;
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id())
@@ -494,7 +494,7 @@ int main(int argc, char** argv)
timer.restart();
cameraInfo = rtabmap::CameraInfo();
data = reader.takeImage(&cameraInfo);
odomInfo.covariance = cameraInfo.odomCovariance;
odomInfo.reg.covariance = cameraInfo.odomCovariance;
odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
acquisitionTime = timer.ticks();
}
src/GuiWrapper.cpp
+2 -2
View File
@@ -569,7 +569,7 @@ void GuiWrapper::commonDepthCallback(
return;
}
info.covariance = covariance;
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
@@ -724,7 +724,7 @@ void GuiWrapper::commonStereoCallback(
return;
}
info.covariance = covariance;
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
src/MsgConversion.cpp
+19 -13
View File
@@ -884,10 +884,13 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{
rtabmap::OdometryInfo info;
info.lost = msg.lost;
info.matches = msg.matches;
info.inliers = msg.inliers;
info.icpInliersRatio = msg.icpInliersRatio;
info.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
info.reg.matches = msg.matches;
info.reg.inliers = msg.inliers;
info.reg.icpInliersRatio = msg.icpInliersRatio;
info.reg.icpRotation = msg.icpRotation;
info.reg.icpTranslation = msg.icpTranslation;
info.reg.icpStructuralComplexity = msg.icpStructuralComplexity;
info.reg.covariance = cv::Mat(6,6,CV_64FC1, (void*)msg.covariance.data()).clone();
info.features = msg.features;
info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize;
@@ -910,8 +913,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
}
info.wordMatches = msg.wordMatches;
info.wordInliers = msg.wordInliers;
info.reg.matchesIDs = msg.wordMatches;
info.reg.inliersIDs = msg.wordInliers;
info.refCorners = points2fFromROS(msg.refCorners);
info.newCorners = points2fFromROS(msg.newCorners);
@@ -934,12 +937,15 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
{
msg.lost = info.lost;
msg.matches = info.matches;
msg.inliers = info.inliers;
msg.icpInliersRatio = info.icpInliersRatio;
if(info.covariance.type() == CV_64FC1 && info.covariance.cols == 6 && info.covariance.rows == 6)
msg.matches = info.reg.matches;
msg.inliers = info.reg.inliers;
msg.icpInliersRatio = info.reg.icpInliersRatio;
msg.icpRotation = info.reg.icpRotation;
msg.icpTranslation = info.reg.icpTranslation;
msg.icpStructuralComplexity = info.reg.icpStructuralComplexity;
if(info.reg.covariance.type() == CV_64FC1 && info.reg.covariance.cols == 6 && info.reg.covariance.rows == 6)
{
memcpy(msg.covariance.data(), info.covariance.data, 36*sizeof(double));
memcpy(msg.covariance.data(), info.reg.covariance.data, 36*sizeof(double));
}
msg.features = info.features;
msg.localMapSize = info.localMapSize;
@@ -960,8 +966,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.wordsKeys = uKeys(info.words);
keypointsToROS(uValues(info.words), msg.wordsValues);
msg.wordMatches = info.wordMatches;
msg.wordInliers = info.wordInliers;
msg.wordMatches = info.reg.matchesIDs;
msg.wordInliers = info.reg.inliersIDs;
points2fToROS(info.refCorners, msg.refCorners);
points2fToROS(info.newCorners, msg.newCorners);
src/OdometryROS.cpp
+15 -17
View File
@@ -449,12 +449,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.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
odom.pose.covariance.at(0) = info.reg.covariance.at<double>(0,0)*2; // xx
odom.pose.covariance.at(7) = info.reg.covariance.at<double>(1,1)*2; // yy
odom.pose.covariance.at(14) = info.reg.covariance.at<double>(2,2)*2; // zz
odom.pose.covariance.at(21) = info.reg.covariance.at<double>(3,3)*2; // rr
odom.pose.covariance.at(28) = info.reg.covariance.at<double>(4,4)*2; // pp
odom.pose.covariance.at(35) = info.reg.covariance.at<double>(5,5)*2; // yawyaw
//set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull();
@@ -470,12 +470,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.twist.twist.angular.z = yaw;
}
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
odom.twist.covariance.at(0) = setTwist?info.reg.covariance.at<double>(0,0):BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.reg.covariance.at<double>(1,1):BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.reg.covariance.at<double>(2,2):BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.reg.covariance.at<double>(3,3):BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.reg.covariance.at<double>(4,4):BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
//publish the message
odomPub_.publish(odom);
@@ -539,8 +539,6 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
}
}else{
NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
}
}
@@ -626,16 +624,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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.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.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}