void SceneNodeProvider::setOffsets(const Ogre::Vector3& translate, const Ogre::Quaternion& rotate) {
if (translate.isNaN() || rotate.isNaN()
|| (translate == Ogre::Vector3::ZERO && (rotate == Ogre::Quaternion::IDENTITY || rotate == Ogre::Quaternion::ZERO))) {
if (mOffsetNode) {
while (mOffsetNode->numAttachedObjects()) {
auto movable = mOffsetNode->detachObject((unsigned short) 0);
mNode->attachObject(movable);
}
}
} else {
if (mOffsetNode) {
mOffsetNode->setPosition(translate);
mOffsetNode->setOrientation(rotate);
} else {
mOffsetNode = mNode->createChildSceneNode(translate, rotate);
mOffsetNode->setInheritScale(true);
while (mNode->numAttachedObjects()) {
auto movable = mNode->detachObject((unsigned short) 0);
mOffsetNode->attachObject(movable);
}
}
}
}
//-----------------------------------------------------------------------
Real ScaledPixelCountLodStrategy::getValueImpl(const MovableObject *movableObject, const Ogre::Camera *camera) const
{
// Get viewport
const Viewport *viewport = camera->getViewport();
// Get viewport area
Real viewportArea = static_cast<Real>(viewport->getActualWidth() * viewport->getActualHeight());
Ogre::Real boundingRadius = movableObject->getBoundingRadius();
// Get area of unprojected circle with object bounding radius
const Ogre::Vector3 nodeScale = movableObject->getParentNode()->getScale();
if (!nodeScale.isNaN()) {
//Get the largest scale of the parent node.
//This is a bit inexact, since if the node is scaled with different values for the different axes we won't be using the most optimal one.
//However, that would be too expensive (we would need to check with how the bounding box is rotated against the camera and so on.)
//And further the vast majority of meshes we use are uniformly scaled.
Ogre::Real scale = std::max(nodeScale.x, std::max(nodeScale.y, nodeScale.z));
boundingRadius *= scale;
}
//Increase the bounding area by the scale
Real boundingArea = Math::PI * Math::Sqr(boundingRadius);
// Base computation on projection type
switch (camera->getProjectionType())
{
case PT_PERSPECTIVE:
{
// Get camera distance
Real distanceSquared = movableObject->getParentNode()->getSquaredViewDepth(camera);
// Check for 0 distance
if (distanceSquared <= std::numeric_limits<Real>::epsilon())
return getBaseValue();
// Get projection matrix (this is done to avoid computation of tan(fov / 2))
const Matrix4& projectionMatrix = camera->getProjectionMatrix();
// Estimate pixel count
return (boundingArea * viewportArea * projectionMatrix[0][0] * projectionMatrix[1][1]) / distanceSquared;
}
case PT_ORTHOGRAPHIC:
{
// Compute orthographic area
Real orthoArea = camera->getOrthoWindowHeight() * camera->getOrthoWindowWidth();
// Check for 0 orthographic area
if (orthoArea <= std::numeric_limits<Real>::epsilon())
return getBaseValue();
// Estimate pixel count
return (boundingArea * viewportArea) / orthoArea;
}
default:
{
// This case is not covered for obvious reasons
assert(0);
return 0;
}
}
}
void CovarianceVisual::setMessage(const geometry_msgs::PoseWithCovariance& msg)
{
// Construct pose position and orientation.
const geometry_msgs::Point& p = msg.pose.position;
const geometry_msgs::Quaternion& q = msg.pose.orientation;
Ogre::Vector3 position(p.x, p.y, p.z);
Ogre::Quaternion orientation(q.w, q.x, q.y, q.z);
// Set position and orientation for axes scene node.
if(!position.isNaN())
axes_->setPosition(position);
else
ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position);
if(!orientation.isNaN())
axes_->setOrientation (orientation);
else
ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation);
// check for NaN in covariance
for (unsigned i = 0; i < 3; ++i)
{
if(isnan(msg.covariance[i]))
{
ROS_WARN_THROTTLE(1, "covariance contains NaN");
return;
}
}
// Compute eigen values and vectors for both shapes.
std::pair<Eigen::Matrix3d, Eigen::Vector3d> positionEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 0));
std::pair<Eigen::Matrix3d, Eigen::Vector3d> orientationEigenVectorsAndValues(computeEigenValuesAndVectors(msg, 3));
Ogre::Quaternion positionQuaternion(computeRotation(msg, positionEigenVectorsAndValues));
Ogre::Quaternion orientationQuaternion(computeRotation(msg, orientationEigenVectorsAndValues));
positionNode_->setOrientation(positionQuaternion);
orientationNode_->setOrientation(orientationQuaternion);
// Compute scaling.
//Ogre::Vector3 axesScaling(1, 1, 1);
//axesScaling *= scaleFactor_;
Ogre::Vector3 positionScaling
(std::sqrt (positionEigenVectorsAndValues.second[0]),
std::sqrt (positionEigenVectorsAndValues.second[1]),
std::sqrt (positionEigenVectorsAndValues.second[2]));
positionScaling *= scaleFactor_;
Ogre::Vector3 orientationScaling
(std::sqrt (orientationEigenVectorsAndValues.second[0]),
std::sqrt (orientationEigenVectorsAndValues.second[1]),
std::sqrt (orientationEigenVectorsAndValues.second[2]));
orientationScaling *= scaleFactor_;
// Set the scaling.
/*if(!axesScaling.isNaN())
axes_->setScale(axesScaling);
else
ROS_WARN_STREAM("axesScaling contains NaN: " << axesScaling);*/
if(!positionScaling.isNaN())
positionNode_->setScale(positionScaling);
else
ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling);
if(!orientationScaling.isNaN())
orientationNode_->setScale(orientationScaling);
else
ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling);
// Debugging.
ROS_INFO_STREAM_THROTTLE
(1.,
"Position:\n"
<< position << "\n"
<< "Positional part 3x3 eigen values:\n"
<< positionEigenVectorsAndValues.second << "\n"
<< "Positional part 3x3 eigen vectors:\n"
<< positionEigenVectorsAndValues.first << "\n"
<< "Sphere orientation:\n"
<< positionQuaternion << "\n"
<< positionQuaternion.getRoll () << " "
<< positionQuaternion.getPitch () << " "
<< positionQuaternion.getYaw () << "\n"
<< "Sphere scaling:\n"
<< positionScaling << "\n"
<< "Rotational part 3x3 eigen values:\n"
<< orientationEigenVectorsAndValues.second << "\n"
<< "Rotational part 3x3 eigen vectors:\n"
<< orientationEigenVectorsAndValues.first << "\n"
<< "Cone orientation:\n"
<< orientationQuaternion << "\n"
<< orientationQuaternion.getRoll () << " "
<< orientationQuaternion.getPitch () << " "
<< orientationQuaternion.getYaw () << "\n"
<< "Cone scaling:\n"
<< orientationScaling
);
}
