covariance_visual.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, Ellon Paiva Mendes @ LAAS-CNRS
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include "covariance_visual.h"
31 
34 
35 #include <OgreSceneManager.h>
36 #include <OgreSceneNode.h>
37 #include <OgreQuaternion.h>
38 
39 #include <ros/console.h>
40 
41 #include <sstream>
42 
43 namespace rviz
44 {
45 
46 namespace
47 {
48 
49 double deg2rad (double degrees)
50 {
51  return degrees * 4.0 * atan (1.0) / 180.0;
52 }
53 
54 // Local function to force the axis to be right handed for 3D. Taken from ecl_statistics
55 void makeRightHanded( Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues)
56 {
57  // Note that sorting of eigenvalues may end up with left-hand coordinate system.
58  // So here we correctly sort it so that it does end up being righ-handed and normalised.
59  Eigen::Vector3d c0 = eigenvectors.block<3,1>(0,0); c0.normalize();
60  Eigen::Vector3d c1 = eigenvectors.block<3,1>(0,1); c1.normalize();
61  Eigen::Vector3d c2 = eigenvectors.block<3,1>(0,2); c2.normalize();
62  Eigen::Vector3d cc = c0.cross(c1);
63  if (cc.dot(c2) < 0) {
64  eigenvectors << c1, c0, c2;
65  double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e;
66  } else {
67  eigenvectors << c0, c1, c2;
68  }
69 }
70 
71 // Local function to force the axis to be right handed for 2D. Based on the one from ecl_statistics
72 void makeRightHanded( Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues)
73 {
74  // Note that sorting of eigenvalues may end up with left-hand coordinate system.
75  // So here we correctly sort it so that it does end up being righ-handed and normalised.
76  Eigen::Vector3d c0; c0.setZero(); c0.head<2>() = eigenvectors.col(0); c0.normalize();
77  Eigen::Vector3d c1; c1.setZero(); c1.head<2>() = eigenvectors.col(1); c1.normalize();
78  Eigen::Vector3d cc = c0.cross(c1);
79  if (cc[2] < 0) {
80  eigenvectors << c1.head<2>(), c0.head<2>();
81  double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e;
82  } else {
83  eigenvectors << c0.head<2>(), c1.head<2>();
84  }
85 }
86 
87 void computeShapeScaleAndOrientation3D(const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation)
88 {
89  Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity());
90  Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero());
91 
92  // NOTE: The SelfAdjointEigenSolver only references the lower triangular part of the covariance matrix
93  // FIXME: Should we use Eigen's pseudoEigenvectors() ?
94  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance);
95  // Compute eigenvectors and eigenvalues
96  if (eigensolver.info () == Eigen::Success)
97  {
98  eigenvalues = eigensolver.eigenvalues();
99  eigenvectors = eigensolver.eigenvectors();
100  }
101  else
102  {
103  ROS_WARN_THROTTLE(1, "failed to compute eigen vectors/values for position. Is the covariance matrix correct?");
104  eigenvalues = Eigen::Vector3d::Zero(); // Setting the scale to zero will hide it on the screen
105  eigenvectors = Eigen::Matrix3d::Identity();
106  }
107 
108  // Be sure we have a right-handed orientation system
109  makeRightHanded(eigenvectors, eigenvalues);
110 
111  // Define the rotation
112  orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), eigenvectors(0,1), eigenvectors(0,2),
113  eigenvectors(1,0), eigenvectors(1,1), eigenvectors(1,2),
114  eigenvectors(2,0), eigenvectors(2,1), eigenvectors(2,2)));
115 
116  // Define the scale. eigenvalues are the variances, so we take the sqrt to draw the standard deviation
117  scale.x = 2*std::sqrt (eigenvalues[0]);
118  scale.y = 2*std::sqrt (eigenvalues[1]);
119  scale.z = 2*std::sqrt (eigenvalues[2]);
120 }
121 
122 enum Plane {
123  YZ_PLANE, // normal is x-axis
124  XZ_PLANE, // normal is y-axis
125  XY_PLANE // normal is z-axis
126 };
127 
128 void computeShapeScaleAndOrientation2D(const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation, Plane plane = XY_PLANE)
129 {
130  Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity());
131  Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero());
132 
133  // NOTE: The SelfAdjointEigenSolver only references the lower triangular part of the covariance matrix
134  // FIXME: Should we use Eigen's pseudoEigenvectors() ?
135  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(covariance);
136  // Compute eigenvectors and eigenvalues
137  if (eigensolver.info () == Eigen::Success)
138  {
139  eigenvalues = eigensolver.eigenvalues();
140  eigenvectors = eigensolver.eigenvectors();
141  }
142  else
143  {
144  ROS_WARN_THROTTLE(1, "failed to compute eigen vectors/values for position. Is the covariance matrix correct?");
145  eigenvalues = Eigen::Vector2d::Zero(); // Setting the scale to zero will hide it on the screen
146  eigenvectors = Eigen::Matrix2d::Identity();
147  }
148 
149  // Be sure we have a right-handed orientation system
150  makeRightHanded(eigenvectors, eigenvalues);
151 
152  // Define the rotation and scale of the plane
153  // The Eigenvalues are the variances. The scales are two times the standard
154  // deviation. The scale of the missing dimension is set to zero.
155  if(plane == YZ_PLANE)
156  {
157  orientation.FromRotationMatrix(Ogre::Matrix3(1, 0, 0,
158  0, eigenvectors(0,0), eigenvectors(0,1),
159  0, eigenvectors(1,0), eigenvectors(1,1)));
160 
161  scale.x = 0;
162  scale.y = 2*std::sqrt (eigenvalues[0]);
163  scale.z = 2*std::sqrt (eigenvalues[1]);
164 
165  }
166  else if(plane == XZ_PLANE)
167  {
168  orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), 0, eigenvectors(0,1),
169  0, 1, 0,
170  eigenvectors(1,0), 0, eigenvectors(1,1)));
171 
172  scale.x = 2*std::sqrt (eigenvalues[0]);
173  scale.y = 0;
174  scale.z = 2*std::sqrt (eigenvalues[1]);
175  }
176  else // plane == XY_PLANE
177  {
178  orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), eigenvectors(0,1), 0,
179  eigenvectors(1,0), eigenvectors(1,1), 0,
180  0, 0, 1));
181 
182  scale.x = 2*std::sqrt (eigenvalues[0]);
183  scale.y = 2*std::sqrt (eigenvalues[1]);
184  scale.z = 0;
185  }
186 }
187 
188 void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale, float max_degrees)
189 {
190  radian_scale /= 2.0;
191  if(radian_scale > deg2rad(max_degrees)) radian_scale = deg2rad(max_degrees);
192  radian_scale = 2.0 * tan(radian_scale);
193 }
194 
195 
196 }
197 
198 const float CovarianceVisual::max_degrees = 89.0;
199 
200 CovarianceVisual::CovarianceVisual( Ogre::SceneManager* scene_manager, Ogre::SceneNode* parent_node, bool is_local_rotation, bool is_visible, float pos_scale, float ori_scale, float ori_offset)
201 : Object( scene_manager ), local_rotation_(is_local_rotation), pose_2d_(false), orientation_visible_(is_visible)
202 {
203  // Main node of the visual
204  root_node_ = parent_node->createChildSceneNode();
205  // Node that will have the same orientation as the fixed frame. Updated from the message on setCovariance()
206  fixed_orientation_node_ = root_node_->createChildSceneNode();
207  // Node to scale the position part of the covariance from the property value
208  position_scale_node_ = fixed_orientation_node_->createChildSceneNode();
209  // Node to be oriented and scaled from the message's covariance
210  position_node_ = position_scale_node_->createChildSceneNode();
212 
213  // Node to scale the orientation part of the covariance. May be attached to both the local (root) node or the fixed frame node.
214  // May be re-attached later by setRotatingFrame()
215  if(local_rotation_)
216  orientation_root_node_ = root_node_->createChildSceneNode();
217  else
218  orientation_root_node_ = fixed_orientation_node_->createChildSceneNode();
219 
220  for(int i = 0; i < kNumOriShapes; i++)
221  {
222  // Node to position and orient the shape along the axis. One for each axis.
223  orientation_offset_node_[i] = orientation_root_node_->createChildSceneNode();
224  // Does not inherit scale from the parent. This is needed to keep the cylinders with the same height. The scale is set by setOrientationScale()
225  orientation_offset_node_[i]->setInheritScale( false );
226 
227  if(i != kYaw2D)
229  else
231 
232  // Initialize all current scales to 0
233  current_ori_scale_[i] = Ogre::Vector3(0,0,0);
234  }
235 
236  // Position the cylindes at position 1.0 in the respective axis, and perpendicular to the axis.
237  // x-axis (roll)
238  orientation_offset_node_[kRoll]->setPosition( Ogre::Vector3::UNIT_X );
239  orientation_offset_node_[kRoll]->setOrientation( Ogre::Quaternion(Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) * Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z ) );
240  // y-axis (pitch)
241  orientation_offset_node_[kPitch]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Y ) );
242  orientation_offset_node_[kPitch]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Y ) );
243  // z-axis (yaw)
244  orientation_offset_node_[kYaw]->setPosition( Ogre::Vector3( Ogre::Vector3::UNIT_Z ) );
245  orientation_offset_node_[kYaw]->setOrientation( Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) );
246  // z-axis (yaw 2D)
247  // NOTE: rviz use a cone defined by the file rviz/ogre_media/models/rviz_cone.mesh, and it's
248  // origin is not at the top of the cone. Since we want the top to be at the origin of
249  // the pose we need to use an offset here.
250  // WARNING: This number was found by trial-and-error on rviz and it's not the correct
251  // one, so changes on scale are expected to cause the top of the cone to move
252  // from the pose origin, although it's only noticeable with big scales.
253  // FIXME: Find the right value from the cone.mesh file, or implement a class that draws
254  // something like a 2D "pie slice" and use it instead of the cone.
255  static const double cone_origin_to_top = 0.49115;
256  orientation_offset_node_[kYaw2D]->setPosition(cone_origin_to_top * Ogre::Vector3::UNIT_X);
257  orientation_offset_node_[kYaw2D]->setOrientation(Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z ));
258 
259  // set initial visibility and scale
260  // root node is always visible. The visibility will be updated on its childs.
261  root_node_->setVisible( true );
262  setVisible( is_visible );
263  setScales( pos_scale, ori_scale );
264  setOrientationOffset( ori_offset );
265 }
266 
268 {
269  delete position_shape_;
270  scene_manager_->destroySceneNode( position_node_->getName() );
271 
272  for(int i = 0; i < kNumOriShapes; i++)
273  {
274  delete orientation_shape_[i];
275  scene_manager_->destroySceneNode( orientation_offset_node_[i]->getName() );
276  }
277 
278  scene_manager_->destroySceneNode( position_scale_node_->getName() );
279  scene_manager_->destroySceneNode( fixed_orientation_node_->getName() );
280  scene_manager_->destroySceneNode( root_node_->getName() );
281 }
282 
283 void CovarianceVisual::setCovariance( const geometry_msgs::PoseWithCovariance& pose )
284 {
285  // check for NaN in covariance
286  for (unsigned i = 0; i < 3; ++i)
287  {
288  if(std::isnan(pose.covariance[i]))
289  {
290  ROS_WARN_THROTTLE(1, "covariance contains NaN");
291  return;
292  }
293  }
294 
295  if(pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0 )
296  pose_2d_ = true;
297  else
298  pose_2d_ = false;
299 
301 
302  // store orientation in Ogre structure
303  Ogre::Quaternion ori;
304  normalizeQuaternion(pose.pose.orientation, ori);
305 
306  // Set the orientation of the fixed node. Since this node is attached to the root node, it's orientation will be the
307  // inverse of pose's orientation.
308  fixed_orientation_node_->setOrientation(ori.Inverse());
309  // Map covariance to a Eigen::Matrix
310  Eigen::Map<const Eigen::Matrix<double,6,6> > covariance(pose.covariance.data());
311 
312  updatePosition(covariance);
313  if(!pose_2d_)
314  {
315  updateOrientation(covariance, kRoll);
316  updateOrientation(covariance, kPitch);
317  updateOrientation(covariance, kYaw);
318  }
319  else
320  {
321  updateOrientation(covariance, kYaw2D);
322  }
323 
324 }
325 
327 {
328  // Compute shape and orientation for the position part of covariance
329  Ogre::Vector3 shape_scale;
330  Ogre::Quaternion shape_orientation;
331  if(pose_2d_)
332  {
333  computeShapeScaleAndOrientation2D(covariance.topLeftCorner<2,2>(), shape_scale, shape_orientation, XY_PLANE);
334  // Make the scale in z minimal for better visualization
335  shape_scale.z = 0.001;
336  }
337  else
338  {
339  computeShapeScaleAndOrientation3D(covariance.topLeftCorner<3,3>(), shape_scale, shape_orientation);
340  }
341  // Rotate and scale the position scene node
342  position_node_->setOrientation(shape_orientation);
343  if(!shape_scale.isNaN())
344  position_node_->setScale(shape_scale);
345  else
346  ROS_WARN_STREAM("position shape_scale contains NaN: " << shape_scale);
347 }
348 
350 {
351  Ogre::Vector3 shape_scale;
352  Ogre::Quaternion shape_orientation;
353  // Compute shape and orientation for the orientation shape
354  if(pose_2d_)
355  {
356  // We should only enter on this scope if the index is kYaw2D
357  assert(index == kYaw2D);
358  // 2D poses only depend on yaw.
359  shape_scale.x = 2.0*sqrt(covariance(5,5));
360  // To display the cone shape properly the scale along y-axis has to be one.
361  shape_scale.y = 1.0;
362  // Give a minimal height for the cone for better visualization
363  shape_scale.z = 0.001;
364  // Store the computed scale to be used if the user change the scale
365  current_ori_scale_[index] = shape_scale;
366  // Apply the current scale factor
367  shape_scale.x *= current_ori_scale_factor_;
368  // The scale on x means twice the standard deviation, but _in radians_.
369  // So we need to convert it to the linear scale of the shape using tan().
370  // Also, we bound the maximum std
371  radianScaleToMetricScaleBounded(shape_scale.x, max_degrees);
372  }
373  else
374  {
375  assert(index != kYaw2D);
376 
377  // Get the correct sub-matrix based on the index
378  Eigen::Matrix2d covarianceAxis;
379  if(index == kRoll)
380  {
381  covarianceAxis = covariance.bottomRightCorner<2,2>();
382  }
383  else if(index == kPitch)
384  {
385  covarianceAxis << covariance(3,3), covariance(3,5), covariance(5,3), covariance(5,5);
386  }
387  else if(index == kYaw)
388  {
389  covarianceAxis = covariance.block<2,2>(3,3);
390  }
391 
392  // NOTE: The cylinder mesh is oriented along its y axis, thus we want to flat it out into the XZ plane
393  computeShapeScaleAndOrientation2D(covarianceAxis, shape_scale, shape_orientation, XZ_PLANE);
394  // Give a minimal height for the cylinder for better visualization
395  shape_scale.y = 0.001;
396  // Store the computed scale to be used if the user change the scale
397  current_ori_scale_[index] = shape_scale;
398  // Apply the current scale factor
399  shape_scale.x *= current_ori_scale_factor_;
400  shape_scale.z *= current_ori_scale_factor_;
401  // The computed scale is equivalent to twice the standard deviation _in radians_.
402  // So we need to convert it to the linear scale of the shape using tan().
403  // Also, we bound the maximum std.
404  radianScaleToMetricScaleBounded(shape_scale.x, max_degrees);
405  radianScaleToMetricScaleBounded(shape_scale.z, max_degrees);
406  }
407 
408  // Rotate and scale the scene node of the orientation part
409  orientation_shape_[index]->setOrientation(shape_orientation);
410  if(!shape_scale.isNaN())
411  orientation_shape_[index]->setScale(shape_scale);
412  else
413  ROS_WARN_STREAM("orientation shape_scale contains NaN: " << shape_scale);
414 }
415 
416 void CovarianceVisual::setScales( float pos_scale, float ori_scale)
417 {
418  setPositionScale(pos_scale);
419  setOrientationScale(ori_scale);
420 }
421 
422 void CovarianceVisual::setPositionScale( float pos_scale )
423 {
424  if(pose_2d_)
425  position_scale_node_->setScale( pos_scale, pos_scale, 1.0 );
426  else
427  position_scale_node_->setScale( pos_scale, pos_scale, pos_scale );
428 }
429 
431 {
432  // Scale the orientation root node to position the shapes along the axis
433  orientation_root_node_->setScale( ori_offset, ori_offset, ori_offset );
434  // The scale the offset_nodes as well so the displayed shape represents a 1-sigma
435  // standard deviation when displayed with an scale of 1.0
436  // NOTE: We only want to change the scales of the dimentions that represent the
437  // orientation covariance. The other dimensions are set to 1.0.
438  for(int i = 0; i < kNumOriShapes; i++)
439  {
440  if(i == kYaw2D)
441  {
442  // For 2D, the angle is only encoded on x, but we also scale on y to put the top of the cone at the pose origin
443  orientation_offset_node_[i]->setScale( ori_offset, ori_offset, 1.0 );
444  }
445  else
446  {
447  // For 3D, the angle covariance is encoded on x and z dimensions
448  orientation_offset_node_[i]->setScale( ori_offset, 1.0, ori_offset );
449  }
450  }
451 }
452 
454 {
455  // Here we update the current scale factor, apply it to the current scale _in radians_,
456  // convert it to meters and apply to the shape scale. Note we have different invariant
457  // scales in the 3D and in 2D.
458  current_ori_scale_factor_ = ori_scale;
459  for(int i = 0; i < kNumOriShapes; i++)
460  {
461  // Recover the last computed scale
462  Ogre::Vector3 shape_scale = current_ori_scale_[i];
463  if(i == kYaw2D)
464  {
465  // Changes in scale in 2D only affects the x dimension
466  // Apply the current scale factor
467  shape_scale.x *= current_ori_scale_factor_;
468  // Convert from radians to meters
469  radianScaleToMetricScaleBounded(shape_scale.x, max_degrees);
470  }
471  else
472  {
473  // Changes in scale in 3D only affects the x and z dimensions
474  // Apply the current scale factor
475  shape_scale.x *= current_ori_scale_factor_;
476  shape_scale.z *= current_ori_scale_factor_;
477  // Convert from radians to meters
478  radianScaleToMetricScaleBounded(shape_scale.x, max_degrees);
479  radianScaleToMetricScaleBounded(shape_scale.z, max_degrees);
480  }
481  // Apply the new scale
482  orientation_shape_[i]->setScale(shape_scale);
483  }
484 }
485 
486 void CovarianceVisual::setPositionColor(const Ogre::ColourValue& c)
487 {
489 }
490 
491 void CovarianceVisual::setOrientationColor(const Ogre::ColourValue& c)
492 {
493  for(int i = 0; i < kNumOriShapes; i++)
494  {
496  }
497 }
498 
500 {
501  orientation_shape_[kRoll]->setColor(Ogre::ColourValue(1.0, 0.0, 0.0, a ));
502  orientation_shape_[kPitch]->setColor(Ogre::ColourValue(0.0, 1.0, 0.0, a ));
503  orientation_shape_[kYaw]->setColor(Ogre::ColourValue(0.0, 0.0, 1.0, a ));
504  orientation_shape_[kYaw2D]->setColor(Ogre::ColourValue(0.0, 0.0, 1.0, a ));
505 }
506 
507 void CovarianceVisual::setPositionColor( float r, float g, float b, float a )
508 {
509  setPositionColor( Ogre::ColourValue(r, g, b, a ));
510 }
511 
512 void CovarianceVisual::setOrientationColor( float r, float g, float b, float a )
513 {
514  setOrientationColor( Ogre::ColourValue(r, g, b, a ));
515 }
516 
518 {
519  return position_node_->getScale();
520 }
521 
523 {
524  return position_node_->getOrientation();
525 }
526 
527 void CovarianceVisual::setUserData( const Ogre::Any& data )
528 {
529  position_shape_->setUserData( data );
530  for(int i = 0; i < kNumOriShapes; i++)
531  {
532  orientation_shape_[i]->setUserData( data );
533  }
534 }
535 
536 void CovarianceVisual::setVisible( bool visible )
537 {
538  setPositionVisible( visible );
539  setOrientationVisible( visible );
540 }
541 
543 {
544  position_node_->setVisible( visible );
545 }
546 
548 {
549  orientation_visible_ = visible;
551 }
552 
554 {
559 }
560 
561 
562 const Ogre::Vector3& CovarianceVisual::getPosition()
563 {
564  return position_node_->getPosition();
565 }
566 
567 const Ogre::Quaternion& CovarianceVisual::getOrientation()
568 {
569  return position_node_->getOrientation();
570 }
571 
572 void CovarianceVisual::setPosition( const Ogre::Vector3& position )
573 {
574  root_node_->setPosition( position );
575 }
576 
577 void CovarianceVisual::setOrientation( const Ogre::Quaternion& orientation )
578 {
579  root_node_->setOrientation( orientation );
580 }
581 
582 void CovarianceVisual::setRotatingFrame( bool is_local_rotation )
583 {
584  if(local_rotation_ == is_local_rotation)
585  return;
586 
587  local_rotation_ = is_local_rotation;
588 
589  if(local_rotation_)
590  root_node_->addChild( fixed_orientation_node_->removeChild( orientation_root_node_->getName() ) );
591  else
592  fixed_orientation_node_->addChild( root_node_->removeChild( orientation_root_node_->getName() ) );
593 
594 }
595 
597 {
598  return orientation_shape_[index];
599 }
600 
601 } // namespace rviz
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
Definition: shape.cpp:157
virtual void setCovariance(const geometry_msgs::PoseWithCovariance &pose)
Set the covariance.
virtual void setOrientation(const Ogre::Quaternion &orientation)
Sets orientation of the frame this covariance is attached.
void setPositionScale(float pos_scale)
void setUserData(const Ogre::Any &data)
Sets user data on all ogre objects we own.
Definition: shape.cpp:177
void setScales(float pos_scale, float ori_scale)
Set the position and orientation scales for this covariance.
virtual const Ogre::Vector3 & getPosition()
Get the local position of this object.
Ogre::SceneNode * position_node_
virtual void setPositionVisible(bool visible)
Sets visibility of the position part of this covariance.
std::string getName(void *handle)
virtual void setOrientationColor(float r, float g, float b, float a)
Set the color of the orientation covariance. Values are in the range [0, 1].
rviz::Shape * getOrientationShape(ShapeIndex index)
Get the shape used to display orientation covariance in an especific axis.
void setOrientationColorToRGB(float a)
void updatePosition(const Eigen::Matrix6d &covariance)
virtual const Ogre::Quaternion & getOrientation()
Get the local orientation of this object.
virtual void setColor(float r, float g, float b, float a)
Set the color of the object. Values are in the range [0, 1].
Definition: shape.cpp:142
Base class for visible objects, providing a minimal generic interface.
Definition: object.h:49
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
virtual const Ogre::Vector3 & getPositionCovarianceScale()
virtual void setOrientationVisible(bool visible)
Sets visibility of the orientation part of this covariance.
void setOrientationOffset(float ori_offset)
#define ROS_WARN_THROTTLE(period,...)
Ogre::SceneNode * orientation_root_node_
virtual void setRotatingFrame(bool use_rotating_frame)
Sets which frame to attach the covariance of the orientation.
Matrix< double, 6, 6 > Matrix6d
Ogre::SceneNode * fixed_orientation_node_
#define ROS_WARN_STREAM(args)
float normalizeQuaternion(float &w, float &x, float &y, float &z)
virtual void setPosition(const Ogre::Vector3 &position)
Sets position of the frame this covariance is attached.
bool orientation_visible_
If the orientation component is visible.
Ogre::SceneNode * position_scale_node_
virtual void setPositionColor(float r, float g, float b, float a)
Set the color of the position covariance. Values are in the range [0, 1].
Ogre::Vector3 current_ori_scale_[kNumOriShapes]
Ogre::SceneManager * scene_manager_
Ogre scene manager this object is part of.
Definition: object.h:103
rviz::Shape * orientation_shape_[kNumOriShapes]
Cylinders used for the orientation covariance.
virtual const Ogre::Quaternion & getPositionCovarianceOrientation()
virtual void setVisible(bool visible)
Sets visibility of this covariance.
CovarianceVisual(Ogre::SceneManager *scene_manager, Ogre::SceneNode *parent_node, bool is_local_rotation, bool is_visible=true, float pos_scale=1.0f, float ori_scale=0.1f, float ori_offset=0.1f)
Private Constructor.
virtual void setUserData(const Ogre::Any &data)
Sets user data on all ogre objects we own.
void setOrientationScale(float ori_scale)
void updateOrientation(const Eigen::Matrix6d &covariance, ShapeIndex index)
virtual void setScale(const Ogre::Vector3 &scale)
Set the scale of the object. Always relative to the identity orientation of the object.
Definition: shape.cpp:162
rviz::Shape * position_shape_
Ellipse used for the position covariance.
Ogre::SceneNode * orientation_offset_node_[kNumOriShapes]
static const float max_degrees
Ogre::SceneNode * root_node_


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Sat Apr 27 2019 02:33:41