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

ros-pkg: removed guessed covariance in visual_odometry msgs

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1661 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-20 16:57:49 +00:00
parent b114ae62db
commit 396f0d6648
3 changed files with 12 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GridMapAssemblerNode.cpp
+7 -27
View File
@@ -25,36 +25,16 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "GuiWrapper.h"
#include <QtGui/QApplication>
#include <QtCore/QDir>
#include <cv_bridge/cv_bridge.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Empty.h>
#include <nav_msgs/GetMap.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/util3d.h>
#include <ros/ros.h>
#include "rtabmap/MapData.h"
#include "rtabmap/MsgConversion.h"
#include "PreferencesDialogROS.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/GetMap.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <laser_geometry/laser_geometry.h>
using namespace rtabmap;
src/MapAssemblerNode.cpp
+5 -25
View File
@@ -25,34 +25,14 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "GuiWrapper.h"
#include <QtGui/QApplication>
#include <QtCore/QDir>
#include <cv_bridge/cv_bridge.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Empty.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/util3d.h>
#include <ros/ros.h>
#include "rtabmap/MapData.h"
#include "rtabmap/MsgConversion.h"
#include "PreferencesDialogROS.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
using namespace rtabmap;
src/VisualOdometryNode.cpp
-15
View File
@@ -64,8 +64,6 @@ public:
odomFrameId_("odom"),
publishTf_(true),
sync_(0),
xyzCov_(0.2),
rpyCov_(pow(0.01745,2)), // 1 degre
paused_(false)
{
ros::NodeHandle nh;
@@ -134,9 +132,6 @@ public:
}
}
//set covariance depending on the max feature correspondence distance
xyzCov_ = std::atof(parametersOdom.at(Parameters::kOdomInlierDistance()).c_str());
odometry_ = new rtabmap::OdometryBOW(parametersOdom);
ros::NodeHandle rgb_nh(nh, "rgb");
@@ -260,14 +255,6 @@ public:
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
odom.pose.covariance.fill(0);
odom.pose.covariance[0] = xyzCov_; //x
odom.pose.covariance[7] = xyzCov_; //x
odom.pose.covariance[14] = xyzCov_; //x
odom.pose.covariance[21] = rpyCov_; //roll
odom.pose.covariance[28] = rpyCov_; //pitch
odom.pose.covariance[35] = rpyCov_; //yaw
//publish the message
odomPub_.publish(odom);
}
@@ -347,8 +334,6 @@ private:
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
double xyzCov_;
double rpyCov_;
bool paused_;
};