ros-pkg: removed guessed covariance in visual_odometry msgs

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1661 f169173b-cf89-36c8-b27e-44dbe73f0c83

src/GridMapAssemblerNode.cpp

@@ -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 <ros/ros.h>
-#include <QtGui/QApplication>
+#include "rtabmap/MapData.h"
-#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 "rtabmap/MsgConversion.h"
-
+#include <rtabmap/core/util3d.h>
-#include "PreferencesDialogROS.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

@@ -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 <ros/ros.h>
-#include <QtGui/QApplication>
+#include "rtabmap/MapData.h"
-#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 "rtabmap/MsgConversion.h"
-
+#include <rtabmap/core/util3d.h>
-#include "PreferencesDialogROS.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

@@ -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_;
 };