get3DPoint() of SelectionManager returns inconsistent results

[Levaru]

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

• OS Version: e.g. Ubuntu 18.04
• ROS Distro: Melodic
• RViz, Qt, OGRE, OpenGl version as printed by rviz:

  [ INFO]: rviz version 1.13.7
  [ INFO]: compiled against Qt version 5.9.5
  [ INFO]: compiled against OGRE version 1.9.0 (Ghadamon)
  [ INFO]: Forcing OpenGl version 0.
  [ INFO]: Stereo is NOT SUPPORTED
  [ INFO]: OpenGl version: 3 (GLSL 1.3).
  

[Levaru]

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]

Could you please provide the source to your new tool to facilitate the reproduction of the issue?

[Levaru]

@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_H

mesh_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)

[rhaschke]

The just code computes the 3d coordinates of a 2d image patch (or a single pixel) and looks reasonable. I suspect simple rounding errors.

rviz/src/rviz/selection/selection_manager.cpp

Lines 244 to 314 in 51af282

	bool SelectionManager::get3DPatch( Ogre::Viewport* viewport, int x, int y, unsigned width,
	unsigned height, bool skip_missing, std::vector<Ogre::Vector3> &result_points )
	{
	boost::recursive_mutex::scoped_lock lock(global_mutex_);
	ROS_DEBUG("SelectionManager.get3DPatch()");
	std::vector<float> depth_vector;
	if ( !getPatchDepthImage( viewport, x, y, width, height, depth_vector ) )
	return false;
	unsigned int pixel_counter = 0;
	Ogre::Matrix4 projection = camera_->getProjectionMatrix();
	float depth;
	for(unsigned y_iter = 0; y_iter < height; ++y_iter)
	for(unsigned x_iter = 0 ; x_iter < width; ++x_iter)
	{
	depth = depth_vector[pixel_counter];
	//Deal with missing or invalid points
	if( ( depth > camera_->getFarClipDistance() ) || ( depth == 0 ) )
	{
	++pixel_counter;
	if (!skip_missing)
	{
	result_points.push_back(Ogre::Vector3(NAN,NAN,NAN));
	}
	continue;
	}
	Ogre::Vector3 result_point;
	// We want to shoot rays through the center of pixels, not the corners,
	// so add .5 pixels to the x and y coordinate to get to the center
	// instead of the top left of the pixel.
	Ogre::Real screenx = float(x_iter + .5)/float(width);
	Ogre::Real screeny = float(y_iter + .5)/float(height);
	if( projection[3][3] == 0.0 ) // If this is a perspective projection
	{
	// get world-space ray from camera & mouse coord
	Ogre::Ray vp_ray = camera_->getCameraToViewportRay(screenx, screeny );
	// transform ray direction back into camera coords
	Ogre::Vector3 dir_cam = camera_->getDerivedOrientation().Inverse() * vp_ray.getDirection();
	// normalize, so dir_cam.z == -depth
	dir_cam = dir_cam / dir_cam.z * depth * -1;
	// compute 3d point from camera origin and direction*/
	result_point = camera_->getDerivedPosition() + camera_->getDerivedOrientation() * dir_cam;
	}
	else // else this must be an orthographic projection.
	{
	// For orthographic projection, getCameraToViewportRay() does
	// the right thing for us, and the above math does not work.
	Ogre::Ray ray;
	camera_->getCameraToViewportRay(screenx, screeny, &ray);
	result_point = ray.getPoint(depth);
	}
	result_points.push_back(result_point);
	++pixel_counter;
	}
	return result_points.size() > 0;
	}

[Levaru]

@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.