代码拉取完成，页面将自动刷新
#include "common.h"
bool m_isThreadWorkBusy = false;
bool currentIsAorB = true; //current timr,capture thread should capture image A(imgData) or image B(imgDataB)
bool openOdometryByVelocity = false;//true means open
bool openOdometryByDisplacement = true;//true means open
bool openLocaterByDMbar = false;//true means open
uchar imgData[imgSize];
uchar imgDataB[imgSize];
uchar imgRgb[imgRgbSize];
uchar imagePtr[sampledImgSize];
uchar imgMirriorSeam[sampledImgSize];
uchar imgMirriorSeam_copy[sampledImgSize];
uint imgMirriorSeam_copy_copy[sampledImgSize];
uint preComput[imgHeight];
uint preComput_sampled[sampledHeight];
uint preSampleLocat[sampledImgSize];
signed char coor[8][2] = {{0,-1},{-1,-1},{-1,0},{-1,1},{0,1},{1,1},{1,0},{1,-1}}; //1:row 2:col
//signed char coor5X5[16][2] = {{0,2},{-1,2},{-2,2},{-2,1},{-2,0},{-2,-1},{-2,-2},{-1,-2},{0,-2},{1,-2},{2,-2},{2,-1},{2,0},{2,1},{2,2},{1,2}};
signed char coor5X5[16][2] = {{0,-2},{-1,-2},{-2,-2},{-2,-1},{-2,0},{-2,1},{-2,2},{-1,2},{0,2},{1,2},{2,2},{2,1},{2,0},{2,-1},{2,-2},{1,-2}};
line_template line_seg[segNumLimit];
line_template line_seg_mat[60][segNumLimit/2]; //先暂定这么大，可考虑链表
int line_seg_cnt[60];
line_template line_cell[1];
line_template line_sub_cell[1];
int seg_cnt;
float L_lth=97.08;
uchar grayThres =128;//148
pose agvPoseFromDM;
pose agvPoseFromOdometry;
int samImgCnt=0;
int decodeCnt=0;
struct timeval tvNow,tvNowNow;
position samPS[100];
/*定位边准确性检验点*/
const position locP[19]={{19,1},{19,3},{19,5},{19,7},{19,9},{19,11},{19,13},{19,15},{19,17},{19,19},{17,19},{15,19},{13,19},{11,19},{9,19},{7,19},{5,19},{3,19},{1,19}};
/*数据区数据点*/
const position DatP[64]={{13,15},{13,17},{11,15},{11,17},{11,3},{9,15},{9,17},{9,3},
{17,3} ,{17,5} ,{15,3} ,{15,5} ,{15,7},{13,3},{13,5},{13,7},
{5,7},{5,9},{3,7},{3,9},{3,11},{17,7},{17,9},{17,11},
{13,17},{15,17},{13,3},{15,3},{17,3},{13,5},{15,5},{17,5},
{9,5},{11,5},{9,7},{11,7},{13,7},{9,9},{11,9},{13,9},
{5,9},{7,9},{5,11},{7,11},{9,11},{5,13},{7,13},{9,13},
{17,13},{3,13},{17,15},{3,15},{5,15},{17,17},{3,17},{5,17},
{11,11},{13,11},{11,13},{13,13},{15,13},{11,15},{13,15},{15,15}
};//4-7 位置错误
void initVar()
{
agvPoseFromDM.x = -1.0;
agvPoseFromDM.y = -1.0;
agvPoseFromDM.theta = 0.0;
m_isThreadWorkBusy = false;
currentIsAorB = true;
openOdometryByVelocity = false;
openOdometryByDisplacement = true;
}
void initLineSeg()
{
position pos={0,0};
for(int i=0; i<60; i++)
{
line_seg_cnt[i] = 0;
for(int j=0; j<segNumLimit/2; j++){
line_seg_mat[i][j].s = pos;
line_seg_mat[i][j].e = pos;
line_seg_mat[i][j].pixNum = 0;
}
}
}
void calcPreComput()
{
for(ushort i=0; i<imgHeight; i++)
preComput[i] = i*imgWidth;
}
void calcPreComput_sampled()
{
for(ushort i=0; i<sampledHeight; i++)
preComput_sampled[i] = i*sampledWidth;
}
void calcPreSampleLocat()
{
for(ushort x=0; x < sampledWidth; x++)
for(ushort y=0; y < sampledHeight; y++)
{
preSampleLocat[y*sampledWidth +x]=y*sampleInterval*imgWidth +x*sampleInterval;
}
}
/*@brief 叉乘
@param A 浮点齐次坐标Ａ
@param B 浮点齐次坐标Ｂ
@return C 浮点齐次坐标C
*/
Hposition_f cross_fhp_fhp(Hposition_f A, Hposition_f B)
{
Hposition_f C;
C.x = A.y*B.scale - A.scale*B.y;
C.y = A.scale*B.x - A.x*B.scale;
C.scale = A.x*B.y - A.y*B.x;
return C;
}
/*@brief 叉乘
@param A 浮点齐次坐标Ａ
@param B 浮点齐次坐标Ｂ
@return C 浮点非齐次坐标C
*/
position_f cross_fhp_fp(Hposition_f A, Hposition_f B)
{
double temp;
position_f C;
temp= A.x*B.y - A.y*B.x;
C.x = (A.y*B.scale - A.scale*B.y)/temp;
C.y = (A.scale*B.x - A.x*B.scale)/temp;
return C;
}
/*@brief 叉乘
@param A 浮点齐次坐标Ａ
@param B 浮点齐次坐标Ｂ
@return C 整型非齐次坐标C
*/
position cross_fhp_ip(Hposition_f A, Hposition_f B)
{
double temp;
position C;
temp= A.x*B.y - A.y*B.x;
C.col = (int)round((A.y*B.scale - A.scale*B.y)/temp);
C.row = (int)round((A.scale*B.x - A.x*B.scale)/temp);
return C;
}
position cross_hp_ip(Hposition A, Hposition B)
{
float temp;
position C;
temp= A.x*B.y - A.y*B.x;
C.col = (int)round((A.y*B.scale - A.scale*B.y)/temp);
C.row = (int)round((A.scale*B.x - A.x*B.scale)/temp);
return C;
}
position_f cross_hp_fp(Hposition A, Hposition B)
{
float temp;
position_f C;
temp= A.x*B.y - A.y*B.x;
C.x = float((A.y*B.scale - A.scale*B.y))/temp;
C.y = float((A.scale*B.x - A.x*B.scale))/temp;
return C;
}
Hposition cross_ip_hp(position A, position B) //默认了尺度为1
{
Hposition C;
C.x = A.row - B.row;
C.y = B.col - A.col;
C.scale = A.col*B.row - A.row*B.col;
return C;
}
/*@brief 叉乘
@param A 非齐次坐标Ａ
@param B 非齐次坐标Ｂ
@return C 齐次坐标C
*/
Hposition_f cross_ip_fhp(position A, position B) //默认了尺度为1
{
Hposition_f C;
C.x = double(A.row - B.row);
C.y = double(B.col - A.col);
C.scale = double(A.col*B.row - A.row*B.col);
return C;
}
int manhattan(position A, position B)
{
return (abs(A.col-B.col)+abs(A.row-B.row));
}
/*@brief保存一帧bmp格式图像
@param szFile 图像文件名
@param pBmp 图像原始数据
@param width 图像宽度
@param height 图像高度
@param bitCount 像素点大小
*/
int save_to_bmp(char *szFile, uchar* pBmp, int width, int height, int bitCount)
{
//char header[14]={0x42, 0x4d, 0x36, 0x82, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00};
char header[14]={0x42, 0x4d, 0x36, 0xB4, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x04, 0x00, 0x00};
//char header[14]={0x42, 0x4d, 0x36, 0x10, 0x0e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x00, 0x00, 0x00};
FILE *file=NULL;
long bmp_size=0;
ASSERT(szFile!=NULL);
ASSERT(pBmp!=NULL);
long *size=(long *)&header[2];
bmp_size = width*height*(bitCount/8);
if((file=fopen(szFile, "wb"))==NULL)
{
perror("open file error\n");
return 0;
}
// write bmp file header
BITMAPFILEHEADER bmpHeader;
BITMAPINFOHEADER bmiHeader;
RGBQUAD rgbquad[256];
unsigned long dwFileSize = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + sizeof(RGBQUAD)*256 + width*height;
// bmpHeader.bfSize = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + bmp_size; // BMP file size
bmpHeader.bfSize = dwFileSize; // BMP file size
bmpHeader.bfType = 0x4D42; // BMP file type
// bmpHeader.bfOffBits = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER); // offset of image data
bmpHeader.bfOffBits = sizeof(BITMAPFILEHEADER) + sizeof(BITMAPINFOHEADER) + sizeof(RGBQUAD)*256; // offset of image data
printf("%ld\n",bmpHeader.bfOffBits );
fflush(stdout);
bmpHeader.bfReserved1 = 0; // reservered1
bmpHeader.bfReserved2 = 0; // reservered2
*size = bmpHeader.bfSize;
bmiHeader.biSize = sizeof(bmiHeader); // sizeof of this struct
bmiHeader.biWidth = width; // image width
bmiHeader.biHeight = height; // image height
bmiHeader.biPlanes = 1; // set it to 1
bmiHeader.biBitCount = bitCount; // bit count
bmiHeader.biCompression = 0; // compression type [0= no compression, 1=BI_RLE8, 2=BI_RLE4
bmiHeader.biSizeImage = 0; // image data size
bmiHeader.biXPelsPerMeter = 0; // X per meter
bmiHeader.biYPelsPerMeter = 0; // Y per meter
bmiHeader.biClrUsed = 256; // index use or no
bmiHeader.biClrImportant = 0; // 0
for(int i=0;i<256;i++) //构造灰度图的调色版
{
rgbquad[i].rgbBlue = (uchar)i;
rgbquad[i].rgbGreen = (uchar)i;
rgbquad[i].rgbRed = (uchar)i;
rgbquad[i].rgbReserved=0;
}
uchar *buff;
buff = (uchar*)malloc(bmp_size);
for(int i=height-1; i>=0; i--)
{
for(int j=0; j<width; j++)
{
buff[i*width+j] = pBmp[(height-1-i)*width+j];
}
}
DEBUGMSG(("bmpHeader=%d, bmiHeader=%d\n", sizeof(BITMAPFILEHEADER), sizeof(BITMAPINFOHEADER)));
fwrite(header, 1, 14, file);
// len = 14
//fwrite(&bmpHeader, 1, sizeof(bmpHeader), file);
// len = 40
fwrite(&bmiHeader, 1, sizeof(bmiHeader), file);
fwrite(&rgbquad, 1, sizeof(RGBQUAD)*256, file);
fwrite(buff, 1, bmp_size, file);
free(buff);
fclose(file);
return true;
}
int read_from_bmp(char *szFile, uchar* pBmp, int width, int height)
{
FILE *file=NULL;
if((file=fopen(szFile, "rb"))==NULL)
{
perror("open file error\n");
return 0;
}
char header[14];
//BITMAPFILEHEADER bmpHeader;
BITMAPINFOHEADER bmiHeader;
RGBQUAD rgbquad[256];
fread(&header, 1, 14, file);
fread(&bmiHeader, 1, sizeof(bmiHeader), file);
fread(&rgbquad, 1, sizeof(RGBQUAD)*256, file);
for(int i=height-1; i>=0; i--)
{
fread(pBmp+i*width, 1, width, file);
}
return true;
}
void bayer2rgb24_ccjt(unsigned char *dst, unsigned char *src, long width, long height)
{
unsigned char *bayer,
*image;
unsigned char red = 0,
green = 0,
blue = 0;
bayer = src; //raw
image = dst; //rgb24
unsigned char hg = 0 ,
vg = 0 ,
db1 = 0 ,
db2 = 0 ;
int y = 0;
int x = 0;
int yy =0;
int xx =0;
char h_flg = false;
char v_flg = false;
for(yy = 0; yy < height; yy++ )
{
if(h_flg == false)//TRUE) // the software to get the opposite
{
y = height - yy - 1;
}
else
{
y = yy;
}
for(xx = 0; xx < width; xx++ )
{
if(v_flg == true)
{
x = width - xx -1;
}
else
{
x = xx;
}
if( x == 0 )
{
if( y == 0 ) // (0,0) point
{
red = bayer[x + (y+1)*width];
green = (bayer[x + (y+1)*width] + bayer[x+1 + y*width])/2;
blue = bayer[x + y*width];
}
else if( y == height - 1 ) // (width,0) point
{
red = bayer[x+1 + y*width];
green = bayer[x + y*width];
blue = bayer[x + (y-1)*width];
}
else if( (y % 2) == 1 ) // row 1 even
{
red = bayer[x+1+ y*width];
green = bayer[x + y*width];
blue = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
}
else if( (y % 2) == 0 ) // row 1 odd
{
red = (bayer[x+1+ (y-1)*width] + bayer[x+1 + (y+1)*width])/2;
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
+ bayer[x+1+ y*width]+ bayer[x+1 + y*width])/4;
blue = bayer[x + y *width];
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
else if( x == width )
{
if( y == 0 ) // (0,0) point
{
red = bayer[x + (y+1)*width];
green = (bayer[x + (y)*width]) ;
blue = bayer[x-1 + y*width];
}
else if( y == height - 1) // (height,0) point
{
red = bayer[x + y*width];
green =(bayer[x + (y-1)*width] + bayer[x-1 + y*width])/2;
blue = bayer[x-1 + (y-1)*width];
}
else if( (y % 2) == 1 ) // last row even
{
red = bayer[x + y*width];
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
+ bayer[x-1 + y*width] + bayer[x-1 + y*width])/2;
blue = (bayer[x-1 + (y-1)*width] + bayer[x-1 +(y+1)*width])/2;
}
else if( (y % 2) == 0 ) // last row odd
{
red = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
green = bayer[x + y*width] ;
blue = bayer[x-1 + y *width];
}
}
////////////////////////////////////////////////////////////////////////////////////////////
else if( x % 2 == 1) // even row
{
if( y == 0 ) // left 1 even column
{
red = bayer[x + (y+1)*width];
green = bayer[x + y*width];
blue = (bayer[x-1 + y*width] + bayer[x+1 +y*width])/2;
}
else if( y == height - 1) // (height,0) point
{
red = bayer[x + y*width];
green = (bayer[x + (y-1)*width] + bayer[x + (y-1)*width]
+ bayer[x-1 + y*width] + bayer[x+1 + y*width])/4;
blue = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y-1)*width])/2;
}
else if( (y % 2) == 1 ) // even column
{
hg = abs(bayer[x + (y-1)*width] - bayer[x + (y+1)*width]);
vg = abs(bayer[x-1 + y*width] - bayer[x+1 + y*width]);
db1 = abs(bayer[x-1 + (y-1)*width] - bayer[x+1 + (y+1)*width]);
db2 = abs(bayer[x-1 + (y+1)*width] - bayer[x+1 + (y-1)*width]);
red = bayer[x + y*width];
if(hg < vg)
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
else if(hg > vg)
green = (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
else
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
+bayer[x-1 + y*width] + bayer[x+1 + y*width]) /4 ;
if(db1 < db2)
blue = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width])/2;
else if (db1 > db2)
blue = (bayer[x-1 + (y+1)*width] + bayer[x+1 + (y-1)*width]) /2;
else
blue = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width]
+ bayer[x+1 + (y-1)*width] + bayer[x-1 + (y+1)*width])/4;
}
else if( (y % 2) == 0 ) // odd column
{
red = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
green = bayer[x + y*width];
blue = (bayer[x-1 + y *width] + bayer[x+1 + y*width])/2;
}
}
////////////////////////////////////////////////////////////////////////////////////////////
else if ((x % 2) == 0) // odd row
{
if( y == 0 ) // left 1 even column
{
red = (bayer[x-1+ (y+1)*width] + bayer[x+1 + (y+1)*width])/2;
green = (bayer[x + (y+1)*width] + bayer[x + (y+1)*width]
+ bayer[x-1 + (y)*width] + bayer[x+1 + y*width])/4;
blue = bayer[x + y*width];
}
else if( y == height -1 ) // (width,0) point
{
red = (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
green = bayer[x + y*width];
blue = bayer[x + (y-1)*width];
}
else if( (y % 2) == 0 ) // odd column
{
hg = abs(bayer[x + (y-1)*width] - bayer[x + (y+1)*width]);
vg = abs(bayer[x-1 + y*width] - bayer[x+1 + y*width]);
db1 = abs(bayer[x-1 + (y-1)*width] - bayer[x+1 + (y+1)*width]);
db2 = abs(bayer[x-1 + (y+1)*width] - bayer[x+1 + (y-1)*width]);
blue = bayer[x + y*width];
if(hg < vg)
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width])/2;
else if(hg > vg)
green = (bayer[x-1 + y*width] + bayer[x+1 + y*width])/2;
else
green = (bayer[x + (y-1)*width] + bayer[x + (y+1)*width]
+ bayer[x-1 + y*width]+ bayer[x+1 + y*width] ) /4;
if(db1 < db2)
red = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width])/2;
else if (db1 > db2)
red = (bayer[x-1 + (y+1)*width] + bayer[x+1 + (y-1)*width]) /2;
else
red = (bayer[x-1 + (y-1)*width] + bayer[x+1 + (y+1)*width]
+ bayer[x+1 + (y-1)*width] + bayer[x-1 + (y+1)*width])/4;
}
else if( (y % 2) ==1 ) // even column
{
red = (bayer[x-1 + y*width] + bayer[x+1 + (y)*width])/2;
green = bayer[x + y*width];
blue = (bayer[x + (y-1) *width] + bayer[x + (y+1)*width])/2;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////
image[0] = min(max(blue,0),255);
image[1] = min(max(green,0),255);
image[2] = min(max(red,0),255);
image += 3;
}
}
}
此处可能存在不合适展示的内容，页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容，可点击提交进行申诉，我们将尽快为您处理。
马建仓 AI 助手