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 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
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.11.12 REQUIRED) find_package(RTABMap 0.11.13 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
msg/OdomInfo.msg
+4 -2
View File
@@ -6,7 +6,8 @@ Header header
# bool lost; # bool lost;
# int matches; # int matches;
# int inliers; # int inliers;
# float variance; # float varianceLin;
# float varianceAng;
# int features; # int features;
# int localMapSize; # int localMapSize;
# float time; # float time;
@@ -27,7 +28,8 @@ Header header
bool lost bool lost
int32 matches int32 matches
int32 inliers int32 inliers
float32 variance float32 varianceLin
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.11.10</version> <version>0.11.13</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/MsgConversion.cpp
+4 -2
View File
@@ -879,7 +879,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.localMapSize = msg.localMapSize; info.localMapSize = msg.localMapSize;
info.localScanMapSize = msg.localScanMapSize; info.localScanMapSize = msg.localScanMapSize;
info.timeEstimation = msg.timeEstimation; info.timeEstimation = msg.timeEstimation;
info.variance = msg.variance; info.varianceLin = msg.varianceLin;
info.varianceAng = msg.varianceAng;
info.timeParticleFiltering = msg.timeParticleFiltering; info.timeParticleFiltering = msg.timeParticleFiltering;
info.stamp = msg.stamp; info.stamp = msg.stamp;
info.interval = msg.interval; info.interval = msg.interval;
@@ -923,7 +924,8 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.localMapSize = info.localMapSize; msg.localMapSize = info.localMapSize;
msg.localScanMapSize = info.localScanMapSize; msg.localScanMapSize = info.localScanMapSize;
msg.timeEstimation = info.timeEstimation; msg.timeEstimation = info.timeEstimation;
msg.variance = info.variance; msg.varianceLin = info.varianceLin;
msg.varianceAng = info.varianceAng;
msg.timeParticleFiltering = info.timeParticleFiltering; msg.timeParticleFiltering = info.timeParticleFiltering;
msg.stamp = info.stamp; msg.stamp = info.stamp;
msg.interval = info.interval; 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 //set covariance
// libviso2 uses approximately vel variance * 2 // libviso2 uses approximately vel variance * 2
odom.pose.covariance.at(0) = info.variance*2; // xx odom.pose.covariance.at(0) = info.varianceLin*2; // xx
odom.pose.covariance.at(7) = info.variance*2; // yy odom.pose.covariance.at(7) = info.varianceLin*2; // yy
odom.pose.covariance.at(14) = info.variance*2; // zz odom.pose.covariance.at(14) = info.varianceLin*2; // zz
odom.pose.covariance.at(21) = info.variance*2; // rr odom.pose.covariance.at(21) = info.varianceAng*2; // rr
odom.pose.covariance.at(28) = info.variance*2; // pp odom.pose.covariance.at(28) = info.varianceAng*2; // pp
odom.pose.covariance.at(35) = info.variance*2; // yawyaw odom.pose.covariance.at(35) = info.varianceAng*2; // yawyaw
//set velocity //set velocity
bool setTwist = !odometry_->previousVelocityTransform().isNull(); 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.twist.angular.z = yaw;
} }
odom.twist.covariance.at(0) = setTwist?info.variance:BAD_COVARIANCE; // xx odom.twist.covariance.at(0) = setTwist?info.varianceLin:BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?info.variance:BAD_COVARIANCE; // yy odom.twist.covariance.at(7) = setTwist?info.varianceLin:BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?info.variance:BAD_COVARIANCE; // zz odom.twist.covariance.at(14) = setTwist?info.varianceLin:BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?info.variance:BAD_COVARIANCE; // rr odom.twist.covariance.at(21) = setTwist?info.varianceAng:BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?info.variance:BAD_COVARIANCE; // pp odom.twist.covariance.at(28) = setTwist?info.varianceAng:BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?info.variance:BAD_COVARIANCE; // yawyaw odom.twist.covariance.at(35) = setTwist?info.varianceAng:BAD_COVARIANCE; // yawyaw
//publish the message //publish the message
odomPub_.publish(odom); odomPub_.publish(odom);
@@ -620,16 +620,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, 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 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 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());
} }
} }