camera.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * conversions between ROS and ViSP structures representing camera parameters
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  * 
00042  *
00043  *****************************************************************************/
00044 
00050 #include <sensor_msgs/distortion_models.h>
00051 
00052 #include "visp_bridge/camera.h"
00053 
00054 namespace visp_bridge{
00055 
00056 vpCameraParameters toVispCameraParameters(const sensor_msgs::CameraInfo& cam_info){
00057   vpCameraParameters cam;
00058   // Check that the camera is calibrated, as specified in the
00059   // sensor_msgs/CameraInfo message documentation.
00060   if (cam_info.K.size() != 3 * 3 || cam_info.K[0] == 0.)
00061     throw std::runtime_error ("uncalibrated camera");
00062 
00063   // Check matrix size.
00064   if (cam_info.P.size() != 3 * 4)
00065     throw std::runtime_error
00066       ("camera calibration P matrix has an incorrect size");
00067 
00068   if (cam_info.distortion_model.empty ())
00069     {
00070       const double& px = cam_info.K[0 * 3 + 0];
00071       const double& py = cam_info.K[1 * 3 + 1];
00072       const double& u0 = cam_info.K[0 * 3 + 2];
00073       const double& v0 = cam_info.K[1 * 3 + 2];
00074       cam.initPersProjWithoutDistortion(px, py, u0, v0);
00075       return cam;
00076     }
00077 
00078   if (cam_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB)
00079     {
00080       const double& px = cam_info.P[0 * 4 + 0];
00081       const double& py = cam_info.P[1 * 4 + 1];
00082       const double& u0 = cam_info.P[0 * 4 + 2];
00083       const double& v0 = cam_info.P[1 * 4 + 2];
00084       cam.initPersProjWithoutDistortion(px, py, u0, v0);
00085       //cam.initPersProjWithDistortion(px, py, u0, v0, -cam_info.D[0], cam_info.D[0]);
00086       return cam;
00087     }
00088 
00089   throw std::runtime_error ("unsupported distortion model");
00090 
00091  // return vpCameraParameters(cam_info.P[0 * 4 + 0],cam_info.P[1 * 4 + 1],cam_info.P[0 * 4 + 2],cam_info.P[1 * 4 + 2],-cam_info.D[0],cam_info.D[0]);
00092 }
00093 
00094   sensor_msgs::CameraInfo toSensorMsgsCameraInfo(vpCameraParameters& cam_info, unsigned int cam_image_width, unsigned int cam_image_height ){
00095       sensor_msgs::CameraInfo ret;
00096 
00097       std::vector<double> D(5);
00098       D[0]=cam_info.get_kdu();
00099       D[1] = D[2] = D[3] = D[4] = 0.;
00100       ret.D = D;
00101       ret.P.assign(0.);
00102       ret.K.assign(0.);
00103       ret.R.assign(0.);
00104 
00105       ret.R[0] = 1.;
00106       ret.R[1 * 3 + 1] = 1.;
00107       ret.R[2 * 3 + 2] = 1.;
00108 
00109       ret.P[0 * 4 + 0] = cam_info.get_px();
00110       ret.P[1 * 4 + 1] = cam_info.get_py();
00111       ret.P[0 * 4 + 2] = cam_info.get_u0();
00112       ret.P[1 * 4 + 2] = cam_info.get_v0();
00113       ret.P[2 * 4 + 2] = 1;
00114 
00115 
00116       ret.K[0 * 3 + 0] = cam_info.get_px();
00117       ret.K[1 * 3 + 1] = cam_info.get_py();
00118       ret.K[0 * 3 + 2] = cam_info.get_u0();
00119       ret.K[1 * 3 + 2] = cam_info.get_v0();
00120       ret.K[2 * 3 + 2] = 1;
00121 
00122       ret.distortion_model = "plumb_bob";
00123       ret.binning_x = 0;
00124       ret.binning_y = 0;
00125       ret.width = cam_image_width;
00126       ret.height = cam_image_height;
00127 
00128       return ret;
00129   }
00130 }
00131 


visp_bridge
Author(s): Filip Novotny
autogenerated on Thu Jul 4 2019 19:30:54