#include <stdlib.h>
|
#include <stdio.h>
|
#include <string.h>
|
#include <errno.h>
|
#include <time.h>
|
|
#if OPENCV_SUPPORT
|
#include <Eigen/Core>
|
#include <opencv2/opencv.hpp>
|
#define MIN_VALUE 2.2204e-16 //¸ù¾ÝÐèÒªµ÷ÕûÕâ¸öÖµ
|
#define IS_DOUBLE_ZERO(d) ((d < MIN_VALUE)&&((-d)<MIN_VALUE))
|
/* ÓãÑÛ²ÎÊý */
|
struct CameraCoeff
|
{
|
double cx, cy; // Ïà»ú¾µÍ·µÄ¹âÐÄ
|
double a0, a2, a3, a4; // ÓãÑÛ¾µÍ·µÄ»û±äϵÊý
|
double sf; // sf¿ØÖÆÊӽǣ¬sfÔ½´óÊÓ½ÇÔ½´ó
|
double invpol[8];
|
double big_rho[2002]; // Ô¤ÏÈÉú³ÉµÄtan(theta)ÓërhoµÄ¶ÔÓ¦±í
|
double small_rho[2001]; // Ô¤ÏÈÉú³ÉµÄcot(theta)ÓërhoµÄ¶ÔÓ¦±í
|
double Z[5000];
|
};
|
|
void SmallMeshCorrect(int imgWidth, int imgHeight, int meshSizeW, int meshSizeH, CameraCoeff camCoeff, double *mapx, double *mapy, unsigned short *pMeshXY)
|
{
|
double cx = camCoeff.cx;
|
double cy = camCoeff.cy;
|
double a0;
|
a0 = camCoeff.a0;
|
|
double *invpol = camCoeff.invpol;
|
double sf = camCoeff.sf;// sf¿ØÖÆÊӽǣ¬sfÔ½´óÊÓ½ÇÔ½´ó
|
double Nz = a0 / sf;
|
double x1, y1, rho_1;
|
double theta_1, r, t_i;
|
double x2, y2;
|
int index = 0;
|
double temp;
|
int x_cnt = 0;
|
int y_cnt = 2 * meshSizeH * meshSizeW;
|
cv::Mat theta_rho = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
|
cv::Mat mapX = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
|
cv::Mat mapY = cv::Mat(meshSizeH, meshSizeW, CV_64FC1);
|
for (int j = 0; j < meshSizeH; ++j)
|
{
|
for (int i = 0; i < meshSizeW; ++i)
|
{
|
/* ´ÓУÕý×ø±ê·´ÍƵ½»û±ä×ø±ê */
|
x1 = mapx[index] - cx;
|
y1 = mapy[index] - cy;
|
rho_1 = sqrt(x1 * x1 + y1 * y1);
|
theta_1 = atan(Nz / rho_1);
|
theta_rho.at<double>(j, i) = theta_1;
|
|
if (IS_DOUBLE_ZERO(rho_1))
|
{
|
x2 = cx;
|
y2 = cy;
|
}
|
else
|
{
|
r = invpol[0];
|
t_i = 1;
|
for (int k = 1; k < 8; ++k)
|
{
|
t_i = t_i * theta_1;
|
r = r + invpol[k] * t_i;
|
}
|
x2 = (x1 / rho_1) * r + cx;
|
y2 = (y1 / rho_1) * r + cy;
|
}
|
mapX.at<double>(j, i) = x2;
|
mapY.at<double>(j, i) = y2;
|
// ÏÞÖƱ߽ç
|
if (x2 < 0) {
|
x2 = 0;
|
}
|
if (x2 > (imgWidth - 1)) {
|
x2 = (imgWidth - 1); // 1919
|
}
|
if (y2 < 0) {
|
y2 = 0;
|
}
|
if (y2 > (imgHeight - 1)) {
|
y2 = (imgHeight - 1); // 1079
|
}
|
|
// X×ø±ê¶¨µã»¯
|
pMeshXY[x_cnt] = (unsigned short)x2;
|
temp = x2 - pMeshXY[x_cnt];
|
pMeshXY[x_cnt + 1] = unsigned short(temp * 128);
|
x_cnt = x_cnt + 2;
|
|
// Y×ø±ê¶¨µã»¯
|
pMeshXY[y_cnt] = (unsigned short)y2;
|
temp = y2 - pMeshXY[y_cnt];
|
pMeshXY[y_cnt + 1] = unsigned short(temp * 128);
|
y_cnt = y_cnt + 2;
|
++index;
|
}
|
}
|
cv::Mat meshXY = cv::Mat(meshSizeH, meshSizeW, CV_8UC1, pMeshXY);
|
}
|
void 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)
|
{
|
double stride_w = meshStepW;
|
double stride_h = meshStepH;
|
|
|
/* ¸¡µãµÄmeshÍø¸ñ */
|
double *mapx = new double[meshSizeW * meshSizeH];
|
double *mapy = new double[meshSizeW * meshSizeH];
|
double *mapz = new double[meshSizeW * meshSizeH];
|
// Æðʼµã
|
double start_w = 0;
|
double start_h = 0;
|
|
double a, b;
|
int index = 0;
|
b = start_h;
|
for (int j = 0; j < meshSizeH; ++j, b = b + stride_h)
|
{
|
a = start_w;
|
for (int i = 0; i < meshSizeW; ++i, a = a + stride_w)
|
{
|
mapx[index] = a;
|
mapy[index] = b;
|
mapz[index] = (double)1;
|
++index;
|
}
|
}
|
|
/* Éú³ÉУÕýºóµÄmeshXY */
|
SmallMeshCorrect(imgWidth, imgHeight, meshSizeW, meshSizeH, camCoeff, mapx, mapy, pMeshXY);
|
|
/* Éú³ÉpMeshXI£¬pMeshXF£¬pMeshYI£¬pMeshYF */
|
unsigned short *pTmpXi = (unsigned short *)&pMeshXY[0];
|
unsigned short *pTmpXf = (unsigned short *)&pMeshXY[1];
|
unsigned short *pTmpYi = (unsigned short *)&pMeshXY[meshSizeW * meshSizeH * 2];
|
unsigned short *pTmpYf = (unsigned short *)&pMeshXY[meshSizeW * meshSizeH * 2 + 1];
|
|
for (int i = 0; i < meshSizeW * meshSizeH; i++)
|
{
|
pMeshXI[i] = *pTmpXi;
|
pTmpXi += 2;
|
pMeshXF[i] = (unsigned char) * pTmpXf;
|
pTmpXf += 2;
|
pMeshYI[i] = *pTmpYi;
|
pTmpYi += 2;
|
pMeshYF[i] = (unsigned char) * pTmpYf;
|
pTmpYf += 2;
|
}
|
|
/* ±£´æΪbinÎļþ */
|
/* MeshXY.bin */
|
FILE *fpMeshXY = fopen("MeshXY.bin", "wb");
|
if (fpMeshXY == NULL) {
|
printf("MeshXY.bin open error!!!");
|
goto err;
|
}
|
fwrite(&imgWidth, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(&imgHeight, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(&meshSizeW, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(&meshSizeH, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(&meshStepW, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(&meshStepH, sizeof(unsigned short), 1, fpMeshXY);
|
fwrite(pMeshXY, sizeof(unsigned short), meshSizeW * meshSizeH * 2 * 2, fpMeshXY);
|
if (fclose(fpMeshXY) != 0) {
|
printf("MeshXY.bin close error!!!");
|
goto err;
|
}
|
/* MeshXI.bin */
|
FILE *fpMeshXI = fopen("MeshXI.bin", "wb");
|
if (fpMeshXI == NULL) {
|
printf("MeshXI.bin open error!!!");
|
goto err;
|
}
|
fwrite(pMeshXI, sizeof(unsigned short), meshSizeW * meshSizeH, fpMeshXI);
|
if (fclose(fpMeshXI) != 0) {
|
printf("MeshXI.bin close error!!!");
|
goto err;
|
}
|
/* MeshXF.bin */
|
FILE *fpMeshXF = fopen("MeshXF.bin", "wb");
|
if (fpMeshXF == NULL) {
|
printf("MeshXF.bin open error!!!");
|
goto err;
|
}
|
fwrite(pMeshXF, sizeof(unsigned char), meshSizeW * meshSizeH, fpMeshXF);
|
if (fclose(fpMeshXF) != 0) {
|
printf("MeshXF.bin close error!!!");
|
goto err;
|
}
|
/* MeshYI.bin */
|
FILE *fpMeshYI = fopen("MeshYI.bin", "wb");
|
if (fpMeshYI == NULL) {
|
printf("MeshYI.bin open error!!!");
|
goto err;
|
}
|
fwrite(pMeshYI, sizeof(unsigned short), meshSizeW * meshSizeH, fpMeshYI);
|
if (fclose(fpMeshYI) != 0) {
|
printf("MeshYI.bin close error!!!");
|
goto err;
|
}
|
/* MeshYF.bin */
|
FILE *fpMeshYF = fopen("MeshYF.bin", "wb");
|
if (fpMeshYF == NULL) {
|
printf("MeshYF.bin open error!!!");
|
goto err;
|
}
|
fwrite(pMeshYF, sizeof(unsigned char), meshSizeW * meshSizeH, fpMeshYF);
|
if (fclose(fpMeshYF) != 0) {
|
printf("MeshYF.bin close error!!!");
|
goto err;
|
}
|
|
err:
|
delete[] mapx;
|
delete[] mapy;
|
delete[] mapz;
|
}
|
|
int gen_default_mesh_table(int imgWidth, int imgHeight, int mesh_density,
|
unsigned short* pMeshXI, unsigned short* pMeshYI, unsigned char* pMeshXF, unsigned char* pMeshYF)
|
{
|
/* ¸ù¾Ý»û±ä²ÎÊýÉú³Émesh±í */
|
CameraCoeff camCoeff;
|
camCoeff.cx = 951.813257;
|
camCoeff.cy = 700.761832;
|
camCoeff.a0 = -1.547500405530367e+03;
|
camCoeff.a2 = 3.399953844687639e-04;
|
camCoeff.a3 = -9.201736312511578e-08;
|
camCoeff.a4 = 6.793998839476364e-11;
|
camCoeff.sf = 1;
|
|
camCoeff.invpol[0] = 1350.42993320407;
|
camCoeff.invpol[1] = 414.478976267835;
|
camCoeff.invpol[2] = -871.475096726774;
|
camCoeff.invpol[3] = -1775.87581281456;
|
camCoeff.invpol[4] = -2556.70096229938;
|
camCoeff.invpol[5] = -1941.21355417283;
|
camCoeff.invpol[6] = -735.900048505591;
|
camCoeff.invpol[7] = -111.196712124121;
|
#if 0
|
int meshStepW = 16;
|
int meshStepH = 8;
|
if (imgWidth > 1920)
|
{
|
int mapStepW = 16;
|
int mapStepH = 8;
|
}
|
|
int imgWidth = 1920;
|
int imgHeight = 1080;
|
int meshSizeW = imgWidth / meshStepW + 1; // Èç1920->121
|
int meshSizeH = imgHeight / meshStepH + 1; // Èç1080->136
|
unsigned short *pMeshXY; // remapÊý¾Ý
|
unsigned short *pMeshXI;
|
unsigned short *pMeshYI;
|
unsigned char *pMeshXF;
|
unsigned char *pMeshYF;
|
pMeshXY = new unsigned short[meshSizeW * meshSizeH * 2 * 2];
|
pMeshXI = new unsigned short[meshSizeW * meshSizeH];
|
pMeshXF = new unsigned char[meshSizeW * meshSizeH];
|
pMeshYI = new unsigned short[meshSizeW * meshSizeH];
|
pMeshYF = new unsigned char[meshSizeW * meshSizeH];
|
#else
|
int meshStepW = 32;
|
int meshStepH = 16;
|
if (mesh_density == 0)
|
{
|
int mapStepW = 16;
|
int mapStepH = 8;
|
}
|
int meshSizeW = imgWidth / meshStepW + 1; // Èç1920->121
|
int meshSizeH = imgHeight / meshStepH + 1; // Èç1080->136
|
#endif
|
|
GenMeshTable(imgWidth, imgHeight, meshStepW, meshStepH, meshSizeW, meshSizeH, camCoeff, pMeshXY, pMeshXI, pMeshYI, pMeshXF, pMeshYF);
|
return 0;
|
}
|
#endif
|
