1 #include <stdlib.h>
2 #include <stdio.h>
3 #include <string.h>
4 #include <errno.h>
5 #include <time.h>
6
7 #if OPENCV_SUPPORT
8 #include <Eigen/Core>
9 #include <opencv2/opencv.hpp>
10 #define MIN_VALUE 2.2204e-16 //������Ҫ�������ֵ
11 #define IS_DOUBLE_ZERO(d) ((d < MIN_VALUE)&&((-d)<MIN_VALUE))
12 /* ���۲��� */
13 struct CameraCoeff
14 {
15 double cx, cy; // �����ͷ�Ĺ���
16 double a0, a2, a3, a4; // ���۾�ͷ�Ļ���ϵ��
17 double sf; // sf�����ӽǣ�sfԽ���ӽ�Խ��
18 double invpol[8];
19 double big_rho[2002]; // Ԥ�����ɵ�tan(theta)��rho�Ķ�Ӧ��
20 double small_rho[2001]; // Ԥ�����ɵ�cot(theta)��rho�Ķ�Ӧ��
21 double Z[5000];
22 };
23
SmallMeshCorrect(int imgWidth,int imgHeight,int meshSizeW,int meshSizeH,CameraCoeff camCoeff,double * mapx,double * mapy,unsigned short * pMeshXY)24 void SmallMeshCorrect(int imgWidth, int imgHeight, int meshSizeW, int meshSizeH, CameraCoeff camCoeff, double *mapx, double *mapy, unsigned short *pMeshXY)
25 {
26 double cx = camCoeff.cx;
27 double cy = camCoeff.cy;
28 double a0;
29 a0 = camCoeff.a0;
30
31 double *invpol = camCoeff.invpol;
32 double sf = camCoeff.sf;// sf�����ӽǣ�sfԽ���ӽ�Խ��
33 double Nz = a0 / sf;
34 double x1, y1, rho_1;
35 double theta_1, r, t_i;
36 double x2, y2;
37 int index = 0;
38 double temp;
39 int x_cnt = 0;
40 int y_cnt = 2 * meshSizeH * meshSizeW;
41 cv::Mat theta_rho = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
42 cv::Mat mapX = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
43 cv::Mat mapY = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
44 for (int j = 0; j < meshSizeH; ++j)
45 {
46 for (int i = 0; i < meshSizeW; ++i)
47 {
48 /* ��У�����귴�Ƶ��������� */
49 x1 = mapx[index] - cx;
50 y1 = mapy[index] - cy;
51 rho_1 = sqrt(x1 * x1 + y1 * y1);
52 theta_1 = atan(Nz / rho_1);
53 theta_rho.at<double>(j, i) = theta_1;
54
55 if (IS_DOUBLE_ZERO(rho_1))
56 {
57 x2 = cx;
58 y2 = cy;
59 }
60 else
61 {
62 r = invpol[0];
63 t_i = 1;
64 for (int k = 1; k < 8; ++k)
65 {
66 t_i = t_i * theta_1;
67 r = r + invpol[k] * t_i;
68 }
69 x2 = (x1 / rho_1) * r + cx;
70 y2 = (y1 / rho_1) * r + cy;
71 }
72 mapX.at<double>(j, i) = x2;
73 mapY.at<double>(j, i) = y2;
74 // ���Ʊ߽�
75 if (x2 < 0) {
76 x2 = 0;
77 }
78 if (x2 > (imgWidth - 1)) {
79 x2 = (imgWidth - 1); // 1919
80 }
81 if (y2 < 0) {
82 y2 = 0;
83 }
84 if (y2 > (imgHeight - 1)) {
85 y2 = (imgHeight - 1); // 1079
86 }
87
88 // X���궨�㻯
89 pMeshXY[x_cnt] = (unsigned short)x2;
90 temp = x2 - pMeshXY[x_cnt];
91 pMeshXY[x_cnt + 1] = unsigned short(temp * 128);
92 x_cnt = x_cnt + 2;
93
94 // Y���궨�㻯
95 pMeshXY[y_cnt] = (unsigned short)y2;
96 temp = y2 - pMeshXY[y_cnt];
97 pMeshXY[y_cnt + 1] = unsigned short(temp * 128);
98 y_cnt = y_cnt + 2;
99 ++index;
100 }
101 }
102 cv::Mat meshXY = cv::Mat(meshSizeH, meshSizeW, CV_8UC1, pMeshXY);
103 }
GenMeshTable(int imgWidth,int imgHeight,int meshStepW,int meshStepH,int meshSizeW,int meshSizeH,CameraCoeff camCoeff,unsigned short * pMeshXY,unsigned short * pMeshXI,unsigned short * pMeshYI,unsigned char * pMeshXF,unsigned char * pMeshYF)104 void GenMeshTable(int imgWidth, int imgHeight, int meshStepW, int meshStepH, int meshSizeW, int meshSizeH, CameraCoeff camCoeff,
105 unsigned short *pMeshXY, unsigned short *pMeshXI, unsigned short *pMeshYI, unsigned char *pMeshXF, unsigned char *pMeshYF)
106 {
107 double stride_w = meshStepW;
108 double stride_h = meshStepH;
109
110
111 /* �����mesh���� */
112 double *mapx = new double[meshSizeW * meshSizeH];
113 double *mapy = new double[meshSizeW * meshSizeH];
114 double *mapz = new double[meshSizeW * meshSizeH];
115 // ��ʼ��
116 double start_w = 0;
117 double start_h = 0;
118
119 double a, b;
120 int index = 0;
121 b = start_h;
122 for (int j = 0; j < meshSizeH; ++j, b = b + stride_h)
123 {
124 a = start_w;
125 for (int i = 0; i < meshSizeW; ++i, a = a + stride_w)
126 {
127 mapx[index] = a;
128 mapy[index] = b;
129 mapz[index] = (double)1;
130 ++index;
131 }
132 }
133
134 /* ���������meshXY */
135 SmallMeshCorrect(imgWidth, imgHeight, meshSizeW, meshSizeH, camCoeff, mapx, mapy, pMeshXY);
136
137 /* ����pMeshXI��pMeshXF��pMeshYI��pMeshYF */
138 unsigned short *pTmpXi = (unsigned short *)&pMeshXY[0];
139 unsigned short *pTmpXf = (unsigned short *)&pMeshXY[1];
140 unsigned short *pTmpYi = (unsigned short *)&pMeshXY[meshSizeW * meshSizeH * 2];
141 unsigned short *pTmpYf = (unsigned short *)&pMeshXY[meshSizeW * meshSizeH * 2 + 1];
142
143 for (int i = 0; i < meshSizeW * meshSizeH; i++)
144 {
145 pMeshXI[i] = *pTmpXi;
146 pTmpXi += 2;
147 pMeshXF[i] = (unsigned char) * pTmpXf;
148 pTmpXf += 2;
149 pMeshYI[i] = *pTmpYi;
150 pTmpYi += 2;
151 pMeshYF[i] = (unsigned char) * pTmpYf;
152 pTmpYf += 2;
153 }
154
155 /* ����Ϊbin�ļ� */
156 /* MeshXY.bin */
157 FILE *fpMeshXY = fopen("MeshXY.bin", "wb");
158 if (fpMeshXY == NULL) {
159 printf("MeshXY.bin open error!!!");
160 goto err;
161 }
162 fwrite(&imgWidth, sizeof(unsigned short), 1, fpMeshXY);
163 fwrite(&imgHeight, sizeof(unsigned short), 1, fpMeshXY);
164 fwrite(&meshSizeW, sizeof(unsigned short), 1, fpMeshXY);
165 fwrite(&meshSizeH, sizeof(unsigned short), 1, fpMeshXY);
166 fwrite(&meshStepW, sizeof(unsigned short), 1, fpMeshXY);
167 fwrite(&meshStepH, sizeof(unsigned short), 1, fpMeshXY);
168 fwrite(pMeshXY, sizeof(unsigned short), meshSizeW * meshSizeH * 2 * 2, fpMeshXY);
169 if (fclose(fpMeshXY) != 0) {
170 printf("MeshXY.bin close error!!!");
171 goto err;
172 }
173 /* MeshXI.bin */
174 FILE *fpMeshXI = fopen("MeshXI.bin", "wb");
175 if (fpMeshXI == NULL) {
176 printf("MeshXI.bin open error!!!");
177 goto err;
178 }
179 fwrite(pMeshXI, sizeof(unsigned short), meshSizeW * meshSizeH, fpMeshXI);
180 if (fclose(fpMeshXI) != 0) {
181 printf("MeshXI.bin close error!!!");
182 goto err;
183 }
184 /* MeshXF.bin */
185 FILE *fpMeshXF = fopen("MeshXF.bin", "wb");
186 if (fpMeshXF == NULL) {
187 printf("MeshXF.bin open error!!!");
188 goto err;
189 }
190 fwrite(pMeshXF, sizeof(unsigned char), meshSizeW * meshSizeH, fpMeshXF);
191 if (fclose(fpMeshXF) != 0) {
192 printf("MeshXF.bin close error!!!");
193 goto err;
194 }
195 /* MeshYI.bin */
196 FILE *fpMeshYI = fopen("MeshYI.bin", "wb");
197 if (fpMeshYI == NULL) {
198 printf("MeshYI.bin open error!!!");
199 goto err;
200 }
201 fwrite(pMeshYI, sizeof(unsigned short), meshSizeW * meshSizeH, fpMeshYI);
202 if (fclose(fpMeshYI) != 0) {
203 printf("MeshYI.bin close error!!!");
204 goto err;
205 }
206 /* MeshYF.bin */
207 FILE *fpMeshYF = fopen("MeshYF.bin", "wb");
208 if (fpMeshYF == NULL) {
209 printf("MeshYF.bin open error!!!");
210 goto err;
211 }
212 fwrite(pMeshYF, sizeof(unsigned char), meshSizeW * meshSizeH, fpMeshYF);
213 if (fclose(fpMeshYF) != 0) {
214 printf("MeshYF.bin close error!!!");
215 goto err;
216 }
217
218 err:
219 delete[] mapx;
220 delete[] mapy;
221 delete[] mapz;
222 }
223
gen_default_mesh_table(int imgWidth,int imgHeight,int mesh_density,unsigned short * pMeshXI,unsigned short * pMeshYI,unsigned char * pMeshXF,unsigned char * pMeshYF)224 int gen_default_mesh_table(int imgWidth, int imgHeight, int mesh_density,
225 unsigned short* pMeshXI, unsigned short* pMeshYI, unsigned char* pMeshXF, unsigned char* pMeshYF)
226 {
227 /* ���ݻ����������mesh�� */
228 CameraCoeff camCoeff;
229 camCoeff.cx = 951.813257;
230 camCoeff.cy = 700.761832;
231 camCoeff.a0 = -1.547500405530367e+03;
232 camCoeff.a2 = 3.399953844687639e-04;
233 camCoeff.a3 = -9.201736312511578e-08;
234 camCoeff.a4 = 6.793998839476364e-11;
235 camCoeff.sf = 1;
236
237 camCoeff.invpol[0] = 1350.42993320407;
238 camCoeff.invpol[1] = 414.478976267835;
239 camCoeff.invpol[2] = -871.475096726774;
240 camCoeff.invpol[3] = -1775.87581281456;
241 camCoeff.invpol[4] = -2556.70096229938;
242 camCoeff.invpol[5] = -1941.21355417283;
243 camCoeff.invpol[6] = -735.900048505591;
244 camCoeff.invpol[7] = -111.196712124121;
245 #if 0
246 int meshStepW = 16;
247 int meshStepH = 8;
248 if (imgWidth > 1920)
249 {
250 int mapStepW = 16;
251 int mapStepH = 8;
252 }
253
254 int imgWidth = 1920;
255 int imgHeight = 1080;
256 int meshSizeW = imgWidth / meshStepW + 1; // ��1920->121
257 int meshSizeH = imgHeight / meshStepH + 1; // ��1080->136
258 unsigned short *pMeshXY; // remap����
259 unsigned short *pMeshXI;
260 unsigned short *pMeshYI;
261 unsigned char *pMeshXF;
262 unsigned char *pMeshYF;
263 pMeshXY = new unsigned short[meshSizeW * meshSizeH * 2 * 2];
264 pMeshXI = new unsigned short[meshSizeW * meshSizeH];
265 pMeshXF = new unsigned char[meshSizeW * meshSizeH];
266 pMeshYI = new unsigned short[meshSizeW * meshSizeH];
267 pMeshYF = new unsigned char[meshSizeW * meshSizeH];
268 #else
269 int meshStepW = 32;
270 int meshStepH = 16;
271 if (mesh_density == 0)
272 {
273 int mapStepW = 16;
274 int mapStepH = 8;
275 }
276 int meshSizeW = imgWidth / meshStepW + 1; // ��1920->121
277 int meshSizeH = imgHeight / meshStepH + 1; // ��1080->136
278 #endif
279
280 GenMeshTable(imgWidth, imgHeight, meshStepW, meshStepH, meshSizeW, meshSizeH, camCoeff, pMeshXY, pMeshXI, pMeshYI, pMeshXF, pMeshYF);
281 return 0;
282 }
283 #endif
284