use std deviation instead of variance

ros-visualization · Mar 8, 2019 · 4a3aa40 · 4a3aa40
1 parent 1ab5dca
commit 4a3aa40
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 12 deletions.
diff --git a/src/rviz/default_plugin/tools/initial_pose_tool.cpp b/src/rviz/default_plugin/tools/initial_pose_tool.cpp
@@ -47,12 +47,12 @@ InitialPoseTool::InitialPoseTool()
   topic_property_ = new StringProperty( "Topic", "initialpose",
                                         "The topic on which to publish initial pose estimates.",
                                         getPropertyContainer(), SLOT( updateTopic() ), this );
-  cov_x_ = new FloatProperty("X covariance", 0.5 * 0.5, "X covariance for initial pose", getPropertyContainer());
-  cov_y_ = new FloatProperty("Y covariance", 0.5 * 0.5, "Y covariance for initial pose", getPropertyContainer());
-  cov_theta_ = new FloatProperty("Theta covariance", M_PI / 12.0 * M_PI / 12.0, "Theta covariance for initial pose", getPropertyContainer());
-  cov_x_->setMin(0);
-  cov_y_->setMin(0);
-  cov_theta_->setMin(0);
+  std_dev_x_ = new FloatProperty("X std deviation", 0.5, "X standard deviation for initial pose [m]", getPropertyContainer());
+  std_dev_y_ = new FloatProperty("Y std deviation", 0.5, "Y standard deviation for initial pose [m]", getPropertyContainer());
+  std_dev_theta_ = new FloatProperty("Theta std deviation", M_PI / 12.0, "Theta standard deviation for initial pose [rad]", getPropertyContainer());
+  std_dev_x_->setMin(0);
+  std_dev_y_->setMin(0);
+  std_dev_theta_->setMin(0);
 }
 
 void InitialPoseTool::onInitialize()
@@ -80,9 +80,9 @@ void InitialPoseTool::onPoseSet(double x, double y, double theta)
   quat.setRPY(0.0, 0.0, theta);
   tf::quaternionTFToMsg(quat,
                         pose.pose.pose.orientation);
-  pose.pose.covariance[6*0+0] = cov_x_->getFloat();
-  pose.pose.covariance[6*1+1] = cov_y_->getFloat();
-  pose.pose.covariance[6*5+5] = cov_theta_->getFloat();
+  pose.pose.covariance[6*0+0] = std::pow(std_dev_x_->getFloat(), 2);
+  pose.pose.covariance[6*1+1] = std::pow(std_dev_y_->getFloat(), 2);
+  pose.pose.covariance[6*5+5] = std::pow(std_dev_theta_->getFloat(), 2);
   ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
   pub_.publish(pose);
 }

diff --git a/src/rviz/default_plugin/tools/initial_pose_tool.h b/src/rviz/default_plugin/tools/initial_pose_tool.h
@@ -43,6 +43,7 @@ namespace rviz
 class Arrow;
 class DisplayContext;
 class StringProperty;
+class FloatProperty;
 
 class InitialPoseTool: public PoseTool
 {
@@ -63,9 +64,9 @@ private Q_SLOTS:
   ros::Publisher pub_;
 
   StringProperty* topic_property_;
-  FloatProperty* cov_x_;
-  FloatProperty* cov_y_;
-  FloatProperty* cov_theta_;
+  FloatProperty* std_dev_x_;
+  FloatProperty* std_dev_y_;
+  FloatProperty* std_dev_theta_;
 };
 
 }