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

Odom: Added twist covariance (pose covariance/2)

Browse Source
This commit is contained in:
matlabbe
2016-04-26 10:19:29 -04:00
parent e18365e28f
commit 3f6811b156
3 changed files with 25 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+2 -1
View File
@@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <octomap_msgs/conversions.h> #include <octomap_msgs/conversions.h>
#endif #endif
#define BAD_COVARIANCE 9999
//msgs //msgs
#include "rtabmap_ros/Info.h" #include "rtabmap_ros/Info.h"
@@ -609,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
if(!paused_) if(!paused_)
{ {
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= 9999)) if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE))
{ {
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]);
rtabmap_.triggerNewMap(); rtabmap_.triggerNewMap();
src/OdometryROS.cpp
+23 -4
View File
@@ -49,6 +49,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UFile.h" #include "rtabmap/utilite/UFile.h"
#define BAD_COVARIANCE 9999
using namespace rtabmap; using namespace rtabmap;
namespace rtabmap_ros { namespace rtabmap_ros {
@@ -385,7 +387,8 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.pose.covariance.at(35) = info.variance; // yawyaw odom.pose.covariance.at(35) = info.variance; // yawyaw
//set velocity //set velocity
if(previousStamp_.isValid()) bool setTwist = !odometry_->previousVelocityTransform().isNull();
if(setTwist)
{ {
float x,y,z,roll,pitch,yaw; float x,y,z,roll,pitch,yaw;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
@@ -396,7 +399,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.twist.twist.angular.y = pitch; odom.twist.twist.angular.y = pitch;
odom.twist.twist.angular.z = yaw; odom.twist.twist.angular.z = yaw;
} }
previousStamp_ = stamp; // libviso2 uses approximately pose variance/2
odom.twist.covariance.at(0) = setTwist?odom.pose.covariance.at(0)/2.0:BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = setTwist?odom.pose.covariance.at(7)/2.0:BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = setTwist?odom.pose.covariance.at(14)/2.0:BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = setTwist?odom.pose.covariance.at(21)/2.0:BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = setTwist?odom.pose.covariance.at(28)/2.0:BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = setTwist?odom.pose.covariance.at(35)/2.0:BAD_COVARIANCE; // yawyaw
//publish the message //publish the message
odomPub_.publish(odom); odomPub_.publish(odom);
@@ -471,6 +480,18 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odom.header.stamp = stamp; // use corresponding time stamp to image odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_; odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_; odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
//publish the message //publish the message
odomPub_.publish(odom); odomPub_.publish(odom);
@@ -521,7 +542,6 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{ {
ROS_INFO("visual_odometry: reset odom!"); ROS_INFO("visual_odometry: reset odom!");
odometry_->reset(); odometry_->reset();
previousStamp_ = ros::Time();
return true; return true;
} }
@@ -530,7 +550,6 @@ bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros:
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw); Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str()); ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose); odometry_->reset(pose);
previousStamp_ = ros::Time();
return true; return true;
} }
src/OdometryROS.h
-1
View File
@@ -104,7 +104,6 @@ private:
bool paused_; bool paused_;
int resetCountdown_; int resetCountdown_;
int resetCurrentCount_; int resetCurrentCount_;
ros::Time previousStamp_;
}; };
} }