rviz_interaction_tools: rviz_interaction_tools: camera

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include "rviz_interaction_tools/camera_tools.h"
00032 
00033 namespace rviz_interaction_tools {
00034 
00035 void updateCamera( Ogre::Camera* camera, const sensor_msgs::CameraInfo &camera_info )
00036 {
00037   //align camera with robot camera coordinate frame (z forward and x right)
00038   camera->setPosition(0.0, 0.0, 0.0);
00039   camera->lookAt(Ogre::Vector3(0, 0, 1));
00040   camera->roll(Ogre::Radian(3.141592653));
00041   camera->setNearClipDistance( 0.01f );
00042 
00043   double width = camera_info.width;
00044   double height = camera_info.height;
00045 
00046   double fx = camera_info.P[0];
00047   double fy = camera_info.P[5];
00048 
00049   // Add the camera's translation relative to the left camera (untested);
00050 
00051   double tx = -1 * (camera_info.P[3] / fx);
00052   double ty = -1 * (camera_info.P[7] / fy);
00053 
00054   camera->setPosition( tx, ty, 0 );
00055 
00056   // calculate the projection matrix
00057   double cx = camera_info.P[2];
00058   double cy = camera_info.P[6]+1;
00059 
00060   double far_plane = 100;
00061   double near_plane = 0.01;
00062 
00063   Ogre::Matrix4 proj_matrix;
00064   proj_matrix = Ogre::Matrix4::ZERO;
00065 
00066   proj_matrix[0][0]= 2.0*fx/width;
00067   proj_matrix[1][1]= 2.0*fy/height;
00068 
00069   proj_matrix[0][2]= 2*(0.5 - cx/width);
00070   proj_matrix[1][2]= 2*(cy/height - 0.5);
00071 
00072   proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
00073   proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);
00074 
00075   proj_matrix[3][2]= -1;
00076 
00077   camera->setCustomProjectionMatrix( true, proj_matrix );
00078 }
00079 
00080 }
00081
camera_tools.cpp
