Open Levaru opened 4 years ago
Levaru
commented
4 years ago I solved my problem for now by using a ray/triangle intersection method. It can be rather slow though when a big mesh is loaded, so I'm still interested if there is a solution using the camera projection method.
rhaschke
commented
4 years ago Could you please provide the source to your new tool to facilitate the reproduction of the issue?
Levaru
commented
4 years ago @rhaschke
Here is the header and source file. Nothing complicated, it tries to get a 3D Point while moving the mouse and if found, shows a sphere at that position. On left mouse button click the sphere is placed down. The sphere itself is a simple STL that I created but I don't know how to attach it.
(For anyone wondering: Yes, this doesn't actually select anything yet, it's just the first step)
mesh_selection_tool.h
#ifndef MESH_SELECTION_TOOL_H
#define MESH_SELECTION_TOOL_H
#include <rviz/tool.h>
namespace Ogre
{
class SceneNode;
class Vector3;
}
namespace rviz
{
class VectorProperty;
class VisualizationManager;
class ViewportMouseEvent;
}
namespace my_rviz_plugins
{
class MeshSelectionTool: public rviz::Tool
{
Q_OBJECT
public:
MeshSelectionTool();
~MeshSelectionTool();
virtual void onInitialize();
virtual void activate();
virtual void deactivate();
virtual int processMouseEvent( rviz::ViewportMouseEvent& event );
private:
void makeSphere( const Ogre::Vector3& position );
std::vector<Ogre::SceneNode*> sphere_nodes_;
Ogre::SceneNode* moving_sphere_node_;
std::string sphere_resource_;
rviz::VectorProperty* current_sphere_property_;
};
}
#endif // MESH_SELECTION_TOOL_Hmesh_selection_tool.cpp
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
#include <OGRE/OgreEntity.h>
#include <ros/console.h>
#include <rviz/viewport_mouse_event.h>
#include <rviz/visualization_manager.h>
#include <rviz/mesh_loader.h>
#include <rviz/geometry.h>
#include <rviz/properties/vector_property.h>
#include <rviz/display_context.h>
#include <rviz/selection/selection_manager.h>
#include <rviz/ogre_helpers/shape.h>
#include <rviz/ogre_helpers/initialization.h>
#include <rviz/ogre_helpers/mesh_shape.h>
#include "mesh_selection_tool/mesh_selection_tool.h"
namespace my_rviz_plugins
{
MeshSelectionTool::MeshSelectionTool() : moving_sphere_node_( NULL ), current_sphere_property_( NULL )
{
shortcut_key_ = 'l';
}
MeshSelectionTool::~MeshSelectionTool()
{
for( unsigned i = 0; i < sphere_nodes_.size(); i++ )
{
scene_manager_->destroySceneNode( sphere_nodes_[ i ]);
}
}
void MeshSelectionTool::onInitialize()
{
sphere_resource_ = "package://mesh_selection_tool/meshes/sphere.stl";
if( rviz::loadMeshFromResource( sphere_resource_ ).isNull() )
{
ROS_ERROR( "MeshSelectionTool: failed to load model resource '%s'.", sphere_resource_.c_str() );
return;
}
moving_sphere_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
Ogre::Entity* entity = scene_manager_->createEntity( sphere_resource_ );
moving_sphere_node_->setScale(Ogre::Vector3(2.0));
moving_sphere_node_->attachObject( entity );
moving_sphere_node_->setVisible( false );
}
void MeshSelectionTool::activate()
{
if( moving_sphere_node_ )
{
moving_sphere_node_->setVisible( true );
current_sphere_property_ = new rviz::VectorProperty( "Sphere " + QString::number( sphere_nodes_.size() ));
current_sphere_property_->setReadOnly( true );
getPropertyContainer()->addChild( current_sphere_property_ );
}
}
void MeshSelectionTool::deactivate()
{
if( moving_sphere_node_ )
{
moving_sphere_node_->setVisible( false );
delete current_sphere_property_;
current_sphere_property_ = NULL;
}
}
int MeshSelectionTool::processMouseEvent( rviz::ViewportMouseEvent& event )
{
if( !moving_sphere_node_ )
return Render;
// Check if mouse moved while not clicked
if( !event.leftDown() )
if ((event.x == event.last_x) && (event.y == event.last_y))
return Render;
moving_sphere_node_->setVisible( false );
Ogre::Vector3 pos;
bool success = context_->getSelectionManager()->get3DPoint( event.viewport, event.x, event.y, pos );
if (success) // A point is found
{
// Coordinates are stored in the `pos` variable and we can do anything we like with them from now on
std::ostringstream s;
s.precision(4);
s << " [" << pos.x << ", " << pos.y << ", " << pos.z << "]";
ROS_INFO("3D Point %s", s.str().c_str());
moving_sphere_node_->setVisible( true );
moving_sphere_node_->setPosition(pos);
current_sphere_property_->setVector( pos );
if( event.leftDown() )
{
makeSphere( pos );
current_sphere_property_ = NULL; // Drop the reference so that deactivate() won't remove it.
return Render | Finished;
}
}
else
{
moving_sphere_node_->setVisible( false );
}
return Render;
}
// This is a helper function to create a new sphere in the Ogre scene and save its scene node in a list.
void MeshSelectionTool::makeSphere( const Ogre::Vector3& position )
{
Ogre::SceneNode* node = scene_manager_->getRootSceneNode()->createChildSceneNode();
Ogre::Entity* entity = scene_manager_->createEntity( sphere_resource_ );
node->attachObject( entity );
node->setVisible( true );
node->setPosition( position );
sphere_nodes_.push_back( node );
}
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(my_rviz_plugins::MeshSelectionTool, rviz::Tool)
The just code computes the 3d coordinates of a 2d image patch (or a single pixel) and looks reasonable. I suspect simple rounding errors. https://github.com/ros-visualization/rviz/blob/51af282f6fe5e6265f8322bfb0f90ad0e9a12eae/src/rviz/selection/selection_manager.cpp#L244-L314
Levaru
commented
4 years ago @rhaschke Yes, that's what I also suspected but I was wondering if there is a way to increase the precision. I'm already using a slightly customized version of Rviz, so it wouldn't be a problem for me to modify the function itself, it's just that I'm not sure how.
And since the error changes depending on camera position and angle, maybe there is a way to reduce the error by changing the FOV? I'm just guessing here, since I have no experience in this area.
I wrote a simple rviz tool for selecting points on a mesh by using this tutorial as an template. Instead of flags, it spawns in a simple sphere by clicking on the surface of a object/mesh. I retrieve the spawn position with the help of the
rviz::SelectionManager::get3DPoint()function.I noticed that the returned position is rather inconsistent. When selecting various points on the top surface of a 1x1x1 cube marker, the Z-position can vary between 0.5013 and 0.4999 (total: 0.0014). With a 0.1x0.1x0.1 cube the Z-position varies between 0.05168 and 0.04893 (total: 0.00275). The effect is stronger on non perfectly flat surfaces like those of a mesh.
I noticed on my meshes, which have a relatively flat top surface, that my selected point would sometimes be above or even below the surface. The expected Z-position should be somewhere around 0.065 but in the following example the actual position is 0.068 which messes up my whole concept for selecting points on meshes. Most of the time I get the correct position but that changes depending on the camera angle and distance.
Sadly, I'm not allowed to upload the mesh, the most I can do is show the wireframe of the top surface:
My current understanding is, that the selection happens by some kind of flattening/projecting of the 3D scene to the 2D view and then corresponding the mouse position to the 3D pixel underneath it. My guess is, that the results of the
rviz::SelectionManager::get3DPoint()function are influenced by the camera settings.Is there a way to solve this problem or at least mitigate it? Should I choose different camera settings for different mesh sizes?
My environment