screen_to_world.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <rve_common/eigen_conversions.h>
#include <rve_render_client/camera.h>
#include <rve_msgs/make_vector3.h>
#include "rve_geometry/screen_to_world.h"

namespace rve_geometry
{

class Ray
{
public:
  rve_msgs::Vector3 getPoint( float distance );

  std::pair<bool, float> intersect( const Plane& plane );

  static Ray createFromCameraPixel( const rve_render_client::CameraPtr& camera,
                                    float screen_x, float screen_y );

private:
  Eigen::Vector3f origin_;
  Eigen::Vector3f direction_;
};

rve_msgs::Vector3 Ray::getPoint( float distance )
{
  return rve_common::eigenToMsg( origin_ + distance * direction_ );
}

std::pair<bool, float> Ray::intersect( const Plane& plane )
{
  float denominator = rve_common::msgToEigen(plane.normal_).dot( direction_ );
  if( fabsf( denominator ) < std::numeric_limits<float>::epsilon() )
  {
    return std::pair<bool, float>(false, 0);
  }
  else
  {
    float numerator = rve_common::msgToEigen(plane.normal_).dot( origin_ ) + plane.distance_from_origin_;
    float distance = -(numerator / denominator);
    return std::pair<bool,float>(distance >= 0, distance);
  }
}

// This duplicates functionality in rve_render_server/src/convert.cpp.
// The code there uses Ogre classes, and we don't want to pollute
// client applications with those.
static Eigen::Matrix3f compute_view_to_robot()
{
  Eigen::Matrix3f view_to_robot;
  view_to_robot =
    Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitY()) *
    Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitZ());
  return view_to_robot;
}

// "robot coordinates" are +X forward, +Z up right-handed coordinates.
// "view coordinates" are -Z forward, +Y up right-handed coordinates.
static Eigen::Matrix3f view_to_robot = compute_view_to_robot();
static Eigen::Matrix3f robot_to_view = view_to_robot.inverse();

// Return the transform from world coordinates to view coordinates,
// where the view is from the pose of the camera.
static Eigen::Matrix4f getViewMatrix( const rve_render_client::CameraPtr& camera )
{
  Eigen::Matrix3f rot_t = Eigen::Matrix3f( rve_common::msgToEigen( camera->getOrientation() )).transpose();
  Eigen::Vector3f trans = -rot_t * rve_common::msgToEigen( camera->getPosition() );

  Eigen::Matrix4f cam_to_view;
  cam_to_view <<
    robot_to_view, Eigen::MatrixXf::Zero(3,1),
    0, 0, 0, 1;
  Eigen::Matrix4f world_to_cam;
  world_to_cam <<
    rot_t , trans,
    0, 0, 0, 1;

  return cam_to_view * world_to_cam;
}

Ray Ray::createFromCameraPixel( const rve_render_client::CameraPtr& camera,
                                float screen_x, float screen_y )
{
  Eigen::Matrix4f inverse_projection = (rve_common::msgToEigen(camera->getProjectionMatrix()) * getViewMatrix(camera)).inverse();

  float nx = (2.0f * screen_x) - 1.0f;
  float ny = 1.0f - (2.0f * screen_y);
  // Use midPoint rather than far point to avoid issues with infinite projection
  Eigen::Vector4f near_point(nx, ny, -1.f, 1.f);
  Eigen::Vector4f mid_point (nx, ny,  0.0f, 1.f);

  // Get ray origin and ray target on near plane in world space
  Eigen::Vector4f ray_origin, ray_target;
                
  ray_origin = inverse_projection * near_point;
  ray_target = inverse_projection * mid_point;

  ray_origin /= ray_origin(3);
  ray_target /= ray_target(3);

  Eigen::Vector3f ray_origin3 = ray_origin.start<3>();
  Eigen::Vector3f ray_target3 = ray_target.start<3>();

  Eigen::Vector3f ray_direction = ray_target3 - ray_origin3;

  ray_direction.normalize();

  Ray out_ray;
  out_ray.origin_= ray_origin3;
  out_ray.direction_ = ray_direction;

  return out_ray;
}

bool screenToWorld( const Plane& world_plane,
                    const rve_render_client::CameraPtr &camera,
                    int screen_x, int screen_y, 
                    int screen_width, int screen_height,
                    rve_msgs::Vector3* position_output )
{
  Ray pixel_ray = Ray::createFromCameraPixel( camera,
                                              float(screen_x)/float(screen_width),
                                              float(screen_y)/float(screen_height) );
  std::pair<bool, float> intersection = pixel_ray.intersect( world_plane );
  if( intersection.first )
  {
    if( position_output )
    {
      *position_output = pixel_ray.getPoint( intersection.second );
    }
    return true;
  }
  else
  {
    return false;
  }
}

}