rk3588-game
/
android13
3
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
android13/external/camera_engine_rkaiq/algos/afec/fec_algo.cpp

284 lines
9.4 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
#define IS_DOUBLE_ZERO(d) ((d < MIN_VALUE)&&((-d)<MIN_VALUE))
/* <20><><EFBFBD>۲<EFBFBD><DBB2><EFBFBD> */
struct CameraCoeff
{
double cx, cy; // <20><><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD>Ĺ<EFBFBD><C4B9><EFBFBD>
double a0, a2, a3, a4; // <20><><EFBFBD>۾<EFBFBD>ͷ<EFBFBD>Ļ<EFBFBD><C4BB><EFBFBD>ϵ<EFBFBD><CFB5>
double sf; // sf<73><66><EFBFBD><EFBFBD><EFBFBD>ӽǣ<D3BD>sfԽ<66><D4BD><EFBFBD>ӽ<EFBFBD>Խ<EFBFBD><D4BD>
double invpol[8];
double big_rho[2002]; // Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD>tan(theta)<29><>rho<68>Ķ<EFBFBD>Ӧ<EFBFBD><D3A6>
double small_rho[2001]; // Ԥ<><D4A4><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD>cot(theta)<29><>rho<68>Ķ<EFBFBD>Ӧ<EFBFBD><D3A6>
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<73><66><EFBFBD><EFBFBD><EFBFBD>ӽǣ<D3BD>sfԽ<66><D4BD><EFBFBD>ӽ<EFBFBD>Խ<EFBFBD><D4BD>
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)
{
/* <20><>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD>귴<EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
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;
// <20><><EFBFBD>Ʊ߽<C6B1>
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<><58><EFBFBD>궨<EFBFBD>㻯
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<><59><EFBFBD>궨<EFBFBD>㻯
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;
/* <20><><EFBFBD><EFBFBD><EFBFBD>mesh<73><68><EFBFBD><EFBFBD> */
double *mapx = new double[meshSizeW * meshSizeH];
double *mapy = new double[meshSizeW * meshSizeH];
double *mapz = new double[meshSizeW * meshSizeH];
// <20><>ʼ<EFBFBD><CABC>
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;
}
}
/* <20><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD>meshXY */
SmallMeshCorrect(imgWidth, imgHeight, meshSizeW, meshSizeH, camCoeff, mapx, mapy, pMeshXY);
/* <20><><EFBFBD><EFBFBD>pMeshXI<58><49>pMeshXF<58><46>pMeshYI<59><49>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;
}
/* <20><><EFBFBD><EFBFBD>Ϊbin<69>ļ<EFBFBD> */
/* 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)
{
/* <20><><EFBFBD>ݻ<EFBFBD><DDBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>mesh<73><68> */
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; // <20><>1920->121
int meshSizeH = imgHeight / meshStepH + 1; // <20><>1080->136
unsigned short *pMeshXY; // remap<61><70><EFBFBD><EFBFBD>
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; // <20><>1920->121
int meshSizeH = imgHeight / meshStepH + 1; // <20><>1080->136
#endif
GenMeshTable(imgWidth, imgHeight, meshStepW, meshStepH, meshSizeW, meshSizeH, camCoeff, pMeshXY, pMeshXI, pMeshYI, pMeshXF, pMeshYF);
return 0;
}
#endif
Reference in New Issue View Git Blame Copy Permalink