35 #include <OgreSceneManager.h> 36 #include <OgreSceneNode.h> 37 #include <OgreQuaternion.h> 49 double deg2rad (
double degrees)
51 return degrees * 4.0 * atan (1.0) / 180.0;
55 void makeRightHanded( Eigen::Matrix3d& eigenvectors, Eigen::Vector3d& eigenvalues)
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);
64 eigenvectors << c1, c0, c2;
65 double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e;
67 eigenvectors << c0, c1, c2;
72 void makeRightHanded( Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues)
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);
80 eigenvectors << c1.head<2>(), c0.head<2>();
81 double e = eigenvalues[0]; eigenvalues[0] = eigenvalues[1]; eigenvalues[1] = e;
83 eigenvectors << c0.head<2>(), c1.head<2>();
87 void computeShapeScaleAndOrientation3D(
const Eigen::Matrix3d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation)
89 Eigen::Vector3d eigenvalues(Eigen::Vector3d::Identity());
90 Eigen::Matrix3d eigenvectors(Eigen::Matrix3d::Zero());
94 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(covariance);
96 if (eigensolver.info () == Eigen::Success)
98 eigenvalues = eigensolver.eigenvalues();
99 eigenvectors = eigensolver.eigenvectors();
103 ROS_WARN_THROTTLE(1,
"failed to compute eigen vectors/values for position. Is the covariance matrix correct?");
104 eigenvalues = Eigen::Vector3d::Zero();
105 eigenvectors = Eigen::Matrix3d::Identity();
109 makeRightHanded(eigenvectors, eigenvalues);
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)));
117 scale.x = 2*std::sqrt (eigenvalues[0]);
118 scale.y = 2*std::sqrt (eigenvalues[1]);
119 scale.z = 2*std::sqrt (eigenvalues[2]);
128 void computeShapeScaleAndOrientation2D(
const Eigen::Matrix2d& covariance, Ogre::Vector3& scale, Ogre::Quaternion& orientation,
Plane plane = XY_PLANE)
130 Eigen::Vector2d eigenvalues(Eigen::Vector2d::Identity());
131 Eigen::Matrix2d eigenvectors(Eigen::Matrix2d::Zero());
135 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(covariance);
137 if (eigensolver.info () == Eigen::Success)
139 eigenvalues = eigensolver.eigenvalues();
140 eigenvectors = eigensolver.eigenvectors();
144 ROS_WARN_THROTTLE(1,
"failed to compute eigen vectors/values for position. Is the covariance matrix correct?");
145 eigenvalues = Eigen::Vector2d::Zero();
146 eigenvectors = Eigen::Matrix2d::Identity();
150 makeRightHanded(eigenvectors, eigenvalues);
155 if(plane == YZ_PLANE)
157 orientation.FromRotationMatrix(Ogre::Matrix3(1, 0, 0,
158 0, eigenvectors(0,0), eigenvectors(0,1),
159 0, eigenvectors(1,0), eigenvectors(1,1)));
162 scale.y = 2*std::sqrt (eigenvalues[0]);
163 scale.z = 2*std::sqrt (eigenvalues[1]);
166 else if(plane == XZ_PLANE)
168 orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), 0, eigenvectors(0,1),
170 eigenvectors(1,0), 0, eigenvectors(1,1)));
172 scale.x = 2*std::sqrt (eigenvalues[0]);
174 scale.z = 2*std::sqrt (eigenvalues[1]);
178 orientation.FromRotationMatrix(Ogre::Matrix3(eigenvectors(0,0), eigenvectors(0,1), 0,
179 eigenvectors(1,0), eigenvectors(1,1), 0,
182 scale.x = 2*std::sqrt (eigenvalues[0]);
183 scale.y = 2*std::sqrt (eigenvalues[1]);
188 void radianScaleToMetricScaleBounded(Ogre::Real & radian_scale,
float max_degrees)
191 if(radian_scale > deg2rad(max_degrees)) radian_scale = deg2rad(max_degrees);
192 radian_scale = 2.0 * tan(radian_scale);
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)
204 root_node_ = parent_node->createChildSceneNode();
239 orientation_offset_node_[
kRoll]->setOrientation( Ogre::Quaternion(Ogre::Degree( 90 ), Ogre::Vector3::UNIT_X ) * Ogre::Quaternion( Ogre::Degree( 90 ), Ogre::Vector3::UNIT_Z ) );
255 static const double cone_origin_to_top = 0.49115;
286 for (
unsigned i = 0; i < 3; ++i)
288 if(std::isnan(pose.covariance[i]))
295 if(pose.covariance[14] <= 0 && pose.covariance[21] <= 0 && pose.covariance[28] <= 0 )
303 Ogre::Quaternion ori;
310 Eigen::Map<const Eigen::Matrix<double,6,6> > covariance(pose.covariance.data());
329 Ogre::Vector3 shape_scale;
330 Ogre::Quaternion shape_orientation;
333 computeShapeScaleAndOrientation2D(covariance.topLeftCorner<2,2>(), shape_scale, shape_orientation, XY_PLANE);
335 shape_scale.z = 0.001;
339 computeShapeScaleAndOrientation3D(covariance.topLeftCorner<3,3>(), shape_scale, shape_orientation);
343 if(!shape_scale.isNaN())
351 Ogre::Vector3 shape_scale;
352 Ogre::Quaternion shape_orientation;
359 shape_scale.x = 2.0*sqrt(covariance(5,5));
363 shape_scale.z = 0.001;
371 radianScaleToMetricScaleBounded(shape_scale.x,
max_degrees);
378 Eigen::Matrix2d covarianceAxis;
381 covarianceAxis = covariance.bottomRightCorner<2,2>();
385 covarianceAxis << covariance(3,3), covariance(3,5), covariance(5,3), covariance(5,5);
387 else if(index ==
kYaw)
389 covarianceAxis = covariance.block<2,2>(3,3);
393 computeShapeScaleAndOrientation2D(covarianceAxis, shape_scale, shape_orientation, XZ_PLANE);
395 shape_scale.y = 0.001;
404 radianScaleToMetricScaleBounded(shape_scale.x,
max_degrees);
405 radianScaleToMetricScaleBounded(shape_scale.z,
max_degrees);
410 if(!shape_scale.isNaN())
413 ROS_WARN_STREAM(
"orientation shape_scale contains NaN: " << shape_scale);
469 radianScaleToMetricScaleBounded(shape_scale.x,
max_degrees);
478 radianScaleToMetricScaleBounded(shape_scale.x,
max_degrees);
479 radianScaleToMetricScaleBounded(shape_scale.z,
max_degrees);
virtual void setOrientation(const Ogre::Quaternion &orientation)
Set the orientation of the object.
#define ROS_WARN_THROTTLE(rate,...)
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.
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)
void updateOrientationVisibility()
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].
Base class for visible objects, providing a minimal generic interface.
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)
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
virtual ~CovarianceVisual()
Ogre::SceneNode * fixed_orientation_node_
#define ROS_WARN_STREAM(args)
float normalizeQuaternion(float &w, float &x, float &y, float &z)
float current_ori_scale_factor_
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.
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.
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_