camera.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * $Id: file.cpp 3496 2011-11-22 15:14:32Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Contact visp@irisa.fr if any conditions of this licensing are
34  * not clear to you.
35  *
36  * Description:
37  * conversions between ROS and ViSP structures representing camera parameters
38  *
39  * Authors:
40  * Filip Novotny
41  *
42  *
43  *****************************************************************************/
44 
51 
52 #include "visp_bridge/camera.h"
53 
54 namespace visp_bridge{
55 
56 vpCameraParameters toVispCameraParameters(const sensor_msgs::CameraInfo& cam_info){
57  vpCameraParameters cam;
58  // Check that the camera is calibrated, as specified in the
59  // sensor_msgs/CameraInfo message documentation.
60  if (cam_info.K.size() != 3 * 3 || cam_info.K[0] == 0.)
61  throw std::runtime_error ("uncalibrated camera");
62 
63  // Check matrix size.
64  if (cam_info.P.size() != 3 * 4)
65  throw std::runtime_error
66  ("camera calibration P matrix has an incorrect size");
67 
68  if (cam_info.distortion_model.empty ())
69  {
70  const double& px = cam_info.K[0 * 3 + 0];
71  const double& py = cam_info.K[1 * 3 + 1];
72  const double& u0 = cam_info.K[0 * 3 + 2];
73  const double& v0 = cam_info.K[1 * 3 + 2];
74  cam.initPersProjWithoutDistortion(px, py, u0, v0);
75  return cam;
76  }
77 
78  if (cam_info.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB)
79  {
80  const double& px = cam_info.P[0 * 4 + 0];
81  const double& py = cam_info.P[1 * 4 + 1];
82  const double& u0 = cam_info.P[0 * 4 + 2];
83  const double& v0 = cam_info.P[1 * 4 + 2];
84  cam.initPersProjWithoutDistortion(px, py, u0, v0);
85  //cam.initPersProjWithDistortion(px, py, u0, v0, -cam_info.D[0], cam_info.D[0]);
86  return cam;
87  }
88 
89  throw std::runtime_error ("unsupported distortion model");
90 
91  // 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]);
92 }
93 
94  sensor_msgs::CameraInfo toSensorMsgsCameraInfo(vpCameraParameters& cam_info, unsigned int cam_image_width, unsigned int cam_image_height ){
95  sensor_msgs::CameraInfo ret;
96 
97  std::vector<double> D(5);
98  D[0]=cam_info.get_kdu();
99  D[1] = D[2] = D[3] = D[4] = 0.;
100  ret.D = D;
101  ret.P.assign(0.);
102  ret.K.assign(0.);
103  ret.R.assign(0.);
104 
105  ret.R[0] = 1.;
106  ret.R[1 * 3 + 1] = 1.;
107  ret.R[2 * 3 + 2] = 1.;
108 
109  ret.P[0 * 4 + 0] = cam_info.get_px();
110  ret.P[1 * 4 + 1] = cam_info.get_py();
111  ret.P[0 * 4 + 2] = cam_info.get_u0();
112  ret.P[1 * 4 + 2] = cam_info.get_v0();
113  ret.P[2 * 4 + 2] = 1;
114 
115 
116  ret.K[0 * 3 + 0] = cam_info.get_px();
117  ret.K[1 * 3 + 1] = cam_info.get_py();
118  ret.K[0 * 3 + 2] = cam_info.get_u0();
119  ret.K[1 * 3 + 2] = cam_info.get_v0();
120  ret.K[2 * 3 + 2] = 1;
121 
122  ret.distortion_model = "plumb_bob";
123  ret.binning_x = 0;
124  ret.binning_y = 0;
125  ret.width = cam_image_width;
126  ret.height = cam_image_height;
127 
128  return ret;
129  }
130 }
131 
sensor_msgs::CameraInfo toSensorMsgsCameraInfo(vpCameraParameters &cam_info, unsigned int cam_image_width, unsigned int cam_image_height)
Converts ViSP camera parameters (vpCameraParameters) to sensor_msgs::CameraInfo.
Definition: camera.cpp:94
vpCameraParameters toVispCameraParameters(const sensor_msgs::CameraInfo &cam_info)
Converts a sensor_msgs::CameraInfo to ViSP camera parameters (vpCameraParameters).
Definition: camera.cpp:56
conversions between ROS and ViSP structures representing camera parameters


visp_bridge
Author(s): Filip Novotny
autogenerated on Sat Mar 13 2021 03:20:01