iCub-main
spherical_projection.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2007 Alex Bernardino, ISR-IST
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 
19 #include <iCub/spherical_projection.h>
20 
21 /*
22 Computes spherical projection parameters, from pinhole projection parameters.
23 Considers possible radial distortion in the pinhole model.
24 WARNING: The function do not validate input parameters. These should be checked previously (see check_sp_params).
25 */
26 void compute_sp_params(int input_lines, int input_cols,
27  int output_lines, int output_cols,
28  double fx, double fy,
29  double cx, double cy,
30  double k1, double k2,
31  double p1, double p2,
32  double *fa, double *fe,
33  double *ca, double *ce)
34 {
35  double max_azimuth = atan((input_cols-1-cx)/fx);
36  double min_azimuth = atan(-cx/fx);
37  double max_elevation = atan((input_lines-1-cy)/fy);
38  double min_elevation = atan(-cy/fy);
39  *fa = output_cols/(max_azimuth-min_azimuth);
40  *fe = output_lines/(max_elevation-min_elevation);
41  *ca = -min_azimuth*(*fa);
42  *ce = -min_elevation*(*fe);
43 }
44 
45 /*
46 Checks validity of spherical projection input parameters.
47 */
48 bool check_sp_params(int input_lines, int input_cols,
49  int output_lines, int output_cols,
50  double fx, double fy,
51  double cx, double cy,
52  double k1, double k2,
53  double p1, double p2,
54  float *mapx, float *mapy)
55 {
56  if(input_lines <= 1) return false;
57  if(input_cols <= 1) return false;
58  if(output_lines <= 1) return false;
59  if(output_cols <= 1) return false;
60  if(fx <= 0) return false;
61  if(fy <= 0) return false;
62  if(mapx == 0) return false;
63  if(mapy == 0) return false;
64  return true;
65 }
66 
67 /* Computes the warping map for spherical projection.
68  To actually perform the image spherical projection, the computed map
69  (mapx, mapy) must be used in conjuction with opencv funtion cvRemap.
70 */
71 bool compute_sp_map(int input_lines, int input_cols,
72  int output_lines, int output_cols,
73  double fx, double fy,
74  double cx, double cy,
75  double k1, double k2,
76  double p1, double p2,
77  float *mapx, float *mapy)
78 {
79  double fa, fe, ca, ce, a, e, x, y, r2, xd, yd;
80 
81  if(!check_sp_params(input_lines, input_cols, output_lines, output_cols,
82  fx, fy, cx, cy, k1, k2, p1, p2, mapx, mapy))
83  return false;
84 
85  compute_sp_params(input_lines, input_cols, output_lines, output_cols,
86  fx, fy, cx, cy, k1, k2, p1, p2,
87  &fa, &fe, &ca, &ce);
88  int i, j;
89  for(i=0;i<output_lines;i++)
90  {
91  for(j=0;j<output_cols;j++)
92  {
93  a=(j-ca)/fa;
94  e=(i-ce)/fe;
95  x=tan(a);
96  y=tan(e);
97  r2 = x*x+y*y;
98  xd = x*(1+k1*r2+k2*r2*r2)+2*p1*x*y+p2*(r2+2*x*x);
99  yd = y*(1+k1*r2+k2*r2*r2)+p1*(r2+2*y*y)+2*p2*x*y;
100  *mapx++ = (float)(fx*xd+cx);
101  *mapy++ = (float)(fy*yd+cy);
102  }
103  }
104  return true;
105 }
106 
107 /* Computes the warping map for egosphere projection.
108  To actually perform the image projection, the computed map
109  (mapx, mapy) must be used in conjuction with opencv funtion cvRemap.
110 
111  Assumes iCub's specific reference frames:
112  - World Reference: X axis vertical pointing up
113  Y axis horizontal pointing backward
114  Z axis horizontal pointing right
115 
116  - Camera Reference: X axis aligned with optical axis, pointing forward
117  Y axis aligned with image plane pointing right(increasing column index)
118  Z axis aligned with image plane pointing down (increasing line index)
119 
120 */
121 bool compute_icub_egosp_map( int input_lines, int input_cols,
122  int output_lines, int output_cols,
123  double fx, double fy,
124  double cx, double cy,
125  double *R, //camera to world rotation matrix
126  float *mapx, float *mapy)
127 {
128  // converts to intuitive reference frames:
129  /*
130 
131  - World Reference: X axis horizontal pointing front
132  Y axis horizontal pointing left
133  Z axis vertical pointing up
134 
135  - Camera Reference: X axis aligned with image plane pointing right(increasing column index)
136  Y axis aligned with image plane pointing down (increasing line index)
137  Z axis aligned with optical axis, pointing forward
138  */
139 
140  double Rt[9];
141  Rt[0] = -R[4];
142  Rt[1] = -R[7];
143  Rt[2] = R[1];
144  Rt[3] = -R[5];
145  Rt[4] = -R[8];
146  Rt[5] = R[2];
147  Rt[6] = -R[3];
148  Rt[7] = -R[6];
149  Rt[8] = R[0];
150  compute_egosp_map(input_lines, input_cols, output_lines, output_cols, fx, fy, cx, cy, Rt, mapx, mapy);
151  return true;
152 }
153 
154 
155 /* Computes the warping map for egosphere projection.
156  To actually perform the image projection, the computed map
157  (mapx, mapy) must be used in conjuction with opencv funtion cvRemap.
158 
159  Assumes the following reference frames:
160 
161  - World Reference: X axis horizontal pointing front
162  Y axis horizontal pointing left
163  Z axis vertical pointing up
164 
165  - Camera Reference: X axis aligned with image plane pointing right(increasing column index)
166  Y axis aligned with image plane pointing down (increasing line index)
167  Z axis aligned with optical axis, pointing forward
168 
169 */
170 bool compute_egosp_map( int input_lines, int input_cols,
171  int output_lines, int output_cols,
172  double fx, double fy,
173  double cx, double cy,
174  double *R, //world to camera rotation matrix
175  float *mapx, float *mapy)
176 {
177  double flongitude, flatitude, clongitude, clatitude, longitude, latitude, x, y;
178  double Xworld,Yworld,Zworld,Xcamera,Ycamera,Zcamera;
179 
180  clongitude = output_cols/2;
181  clatitude = output_lines/2;
182  flongitude = output_cols/2/3.141529;
183  flatitude = output_lines/3.141529;
184  int i, j;
185  for(i=0;i<output_lines;i++)
186  {
187  for(j=0;j<output_cols;j++)
188  {
189  latitude=-(i-clatitude)/flatitude;
190  longitude=-(j-clongitude)/flongitude;
191  //world coordinates x,y,z
192  Xworld = cos(longitude)*cos(latitude);
193  Yworld = sin(longitude)*cos(latitude);
194  Zworld = sin(latitude);
195  // convert to camera frame
196  Xcamera= R[0]*Xworld+R[1]*Yworld+R[2]*Zworld;
197  Ycamera= R[3]*Xworld+R[4]*Yworld+R[5]*Zworld;
198  Zcamera= R[6]*Xworld+R[7]*Yworld+R[8]*Zworld;
199  //compute the image projection
200  if(Zcamera > 0)
201  {
202  x = Xcamera/Zcamera;
203  y = Ycamera/Zcamera;
204  *mapx++ = (float)(x*fx+cx);
205  *mapy++ = (float)(y*fy+cy);
206  }
207  else
208  {
209  *mapx++ = -1;
210  *mapy++ = -1;
211  }
212  }
213  }
214  return true;
215 }
e
e
Definition: compute_ekf_fast.m:13
compute_egosp_map
bool compute_egosp_map(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double *R, float *mapx, float *mapy)
Definition: spherical_projection.cpp:170
compute_sp_params
void compute_sp_params(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double k1, double k2, double p1, double p2, double *fa, double *fe, double *ca, double *ce)
Definition: spherical_projection.cpp:26
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
compute_icub_egosp_map
bool compute_icub_egosp_map(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double *R, float *mapx, float *mapy)
Definition: spherical_projection.cpp:121
check_sp_params
bool check_sp_params(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double k1, double k2, double p1, double p2, float *mapx, float *mapy)
Definition: spherical_projection.cpp:48
compute_sp_map
bool compute_sp_map(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double k1, double k2, double p1, double p2, float *mapx, float *mapy)
Definition: spherical_projection.cpp:71