void TFTLCD::drawFastLine(uint16_t x, uint16_t y, uint16_t length,
uint16_t color, uint8_t rotflag)
{
uint16_t newentrymod;
uint16_t prevEntryMod = readRegister(TFTLCD_ENTRY_MOD); // JW ADD
switch (rotation) {
case 0:
x = X(x);
y = Y(y);
if (rotflag)
#ifdef INVERT_Y
newentrymod = 0x1008; // we want a 'vertical line' decrementing
#else
newentrymod = 0x1028; // we want a 'vertical line' incrementing
#endif
else
#ifdef INVERT_X
newentrymod = 0x1020; // we want a 'horizontal line' decrementing
#else
newentrymod = 0x1030; // we want a 'horizontal line' incrementing
#endif
break;
case 1:
swap(x, y);
// first up fix the X
x = I_X(x);
y = Y(y);
if (rotflag)
#ifdef INVERT_X
newentrymod = 0x1010; // we want a 'horizontal line' incrementing
#else
newentrymod = 0x1000; // we want a 'horizontal line' decrementing
#endif
else
bool CMOOSNavBeacon::GetFullState(Matrix &Result,Matrix * m_pXToUse,bool bUseEstimate)
{
if(Result.Nrows()!=FULL_STATES || Result.Ncols()!=1)
{
Result.ReSize(FULL_STATES,1);
}
//set all to zero..
Result =0;
I_X(Result,1)=m_State.m_dfX;
I_Y(Result,1)=m_State.m_dfY;
I_Z(Result,1)=m_State.m_dfZ;
return true;
}
cv::Mat SIFT::sp_find_sift_grid( Mat I,Mat gridX,Mat gridY,int patchSize, float sigma_edge )
{
int num_angles=8;
float num_bins=4;
int num_samples=num_bins*num_bins;
int alpha = 9;
//此处需要判断总共传入多少个变量，如果变量数小于5，就把sigma_edge设置为1
float angle_step=2*pi/num_angles;
//初始化angles 为一个一维矩阵从0到2*pi；间隔为angle_step
Mat angles=create(0,2*pi,angle_step);
angles=deleteO(angles); //删除最后一个元素
CvSize size=I.size();
//int hgt=size.height;
//int wid=size.width;
int num_patches=gridX.total();//计算gridX总共有多少个元素
Mat sift_arr=Mat::zeros(num_patches,num_samples*num_angles,CV_32F);
//计算滤波算子
int f_wid = 4 * ceil(sigma_edge) + 1;
Mat G=gaussian(f_wid,sigma_edge);
Mat GX=gradientX(G);
Mat GY=gradientY(G);
GX=GX*2/totalSumO(GX);
GY=GY*2/totalSumO(GY);
Mat I_X(I.rows,I.cols,CV_32F);
I_X=filter2(GX,I); //因为I，图片读入不同，所以I_X不同，与I有关的均布相同，但是，都正确
Mat I_Y(I.rows,I.cols,CV_32F);
I_Y=filter2(GY,I);
Mat T(I_X.rows,I_X.cols,CV_32F);
add(I_X.mul(I_X),I_Y.mul(I_Y),T);
Mat I_mag(I_X.rows,I_X.cols,CV_32F);
sqrt(T,I_mag);
Mat I_theta=matan2(I_Y,I_X);
Mat interval=create(2/num_bins,2,2/num_bins);
interval-=(1/num_bins+1);
Mat sample_x=meshgrid_X(interval,interval);
Mat sample_y=meshgrid_Y(interval,interval);
sample_x=reshapeX(sample_x);//变为一个1维矩阵
sample_y=reshapeX(sample_y);
Mat I_orientation[8] = {Mat::zeros(size,CV_32F)};
for(int i=0;i<8;i++)
{
I_orientation[i] = Mat::zeros(size,CV_32F);
}
float *pt=angles.ptr<float>(0);
for(int a=0;a<num_angles;a++)
{
Mat tep1=mcos(I_theta-pt[a]);//cos
//cout<<tep1.at<float>(0,1)<<endl;
Mat tep(tep1.rows,tep1.cols,CV_32F);
pow(tep1,alpha,tep);
tep=compareB(tep,0);
I_orientation[a]=tep.mul(I_mag);
}
for(int i=0;i<num_patches;i++)
{
double r=patchSize/2;
float l=(float)(i/gridX.rows);
float m=i%gridX.rows;
float cx=gridX.at<float>(m,l)+r-0.5;
float cy=gridY.at<float>(m,l)+r-0.5;
Mat sample_x_t=Add(sample_x*r,cx);
Mat sample_y_t=Add(sample_y*r,cy);
float *pt1=sample_y_t.ptr<float>(0);
float sample_res=pt1[1]-pt1[0];
// int c=(int)i/gridX.rows;
// float *ptc1=gridX.ptr<float>(c);
// int x_lo=ptc1[i%gridX.rows];
int x_lo = gridX.at<float>(i % gridX.rows, i / gridX.rows);
int x_hi=patchSize+x_lo-1;
/* float *ptc2=gridY.ptr<float>(c);*/
int y_lo=gridY.at<float>(i % gridY.rows, i / gridY.rows);
int y_hi=y_lo+patchSize-1;
Mat A=create(x_lo,x_hi,1);
Mat B=create(y_lo,y_hi,1);
Mat sample_px=meshgrid_X(A,B);
Mat sample_py=meshgrid_Y(A,B);
int num_pix = sample_px.total();//计算sample_px元素总数
sample_px=reshapeY(sample_px);
sample_py=reshapeY(sample_py);
Mat dist_px=abs(repmat(sample_px,1,num_samples)-repmat(sample_x_t,num_pix,1));
Mat dist_py=abs(repmat(sample_py,1,num_samples)-repmat(sample_y_t,num_pix,1));
Mat weights_x=dist_px/sample_res;
Mat weights_x_l=Less(weights_x,1);
weights_x=(1-weights_x).mul(weights_x_l);
Mat weights_y=dist_py/sample_res;
Mat weights_y_l=Less(weights_y,1);
weights_y=(1-weights_y).mul(weights_y_l);
Mat weights=weights_x.mul(weights_y);
Mat curr_sift=Mat::zeros(num_angles,num_samples,CV_32F);
for(int a=0;a<num_angles;a++)
{
//Mat I=getNum(I_orientation[a],y_lo,y_hi,x_lo,x_hi);
Mat I = I_orientation[a](Range(y_lo, y_hi), Range(x_lo, x_hi));
Mat tep=reshapeY(I);
// Fill tep with zeros to fit size of weight
if (tep.cols < weights.cols)
{
for (int i = tep.rows; i < weights.rows; i++)
tep.push_back(0.0f);
}
tep=repmat(tep,1,num_samples);
Mat t=tep.mul(weights);
Mat ta=sum_every_col(t);
float *p=ta.ptr<float>(0);
for(int i=0;i<curr_sift.cols;i++)
{
curr_sift.at<float>(a,i)=p[i];
}
}
Mat tp=reshapeX(curr_sift);
float *p=tp.ptr<float>(0);
for(int j=0;j<sift_arr.cols;j++)
{
sift_arr.at<float>(i,j)=p[j];
}
}
return sift_arr;
}