image=imread('map.jpg');%读入地图文件

image_2=im2bw(image,0.56);%进行阈值化处理 参数可改

image_d=double(image_2);

obj_col=54;%设定原始列位置 可改  X

obj_row=77;%设定原始行位置 可改  Y

input_angle=atan(obj_row/obj_col);%设定原始速度方向 可改

%-----------------------------------------

A=8;%A为分块大小 分为A*A块大小

[m,n]=size(image_d);

row=fix(m/A);%row为分块后的行数

col=fix(n/A);%col为分块后的列数

image_cell=mat2cell(image_d,ones(row,1)*A,ones(col,1)*A);

image_array=ones(A,A,row*col);

for k=1:row*col

image_array(:,:,k)=image_cell{k}(:,:);

end

%-----------------------------------------

temp_row=ceil((obj_row/A));

temp_col=ceil((obj_col/A));

number=(temp_col-1)*row+temp_row;%number为目标点所在的块的编号

%-----------------------------------------

near(1)=number-1-row;%获得邻近块编号

near(2)=number-1;

near(3)=number-1+row;

near(4)=number-row;

near(5)=number;

near(6)=number+row;

near(7)=number+1-row;

near(8)=number+1;

near(9)=number+1+row;

%-----------------------------------------

small=1000;

final_i=0;

fianl_j=0;

final_k=0;

flag=0;%标志是否找到匹配点

for k=1:9

for i=2:A-1

for j=2:A-1

if image_array(i,j,near(k))==0

temp_col_2=1+floor(near(k)/row);

temp_row_2=near(k)-(temp_col_2-1)*row;

temp_row_3=(temp_row_2-1)*A+i;

temp_col_3=(temp_col_2-1)*A+j;

dis=distance(temp_row_3,temp_col_3,obj_row,obj_col);

temp=image_array(:,:,near(k));

angle=find_near_node(temp,i,j,k,input_angle);

if deside(temp_col_3,temp_row_3,obj_col,obj_row)

all=dis*0.6+angle*0.4;

if all<=small

small=dis;

final_i=i;%最终匹配点

fianl_j=j;%最终匹配点

final_k=near(k);%最终匹配点

flag=1;

end

else

flag=0;

end

end

end

end

end

%-----------------------------------------

switch final_k

case 1

final_k=number-1-row;

case 2

final_k=number-1;

case 3

final_k=number-1+row;

case 4

final_k=number-row;

case 5

final_k=number;

case 6

final_k=number+row;

case 7

final_k=number+1-row;

case 8

final_k=number+1;

case 9

final_k=number+1+row;

end

final_col=floor(final_k/row)+1;

final_row=final_k-(final_col-1)*row;

final_x=(final_row-1)*A+final_i;

final_y=(final_col-1)*A+fianl_j;

%-----------------------------------------

imshow(image_d);

hold on

plot(obj_col,obj_row,'o')

place_x_o=num2str(obj_row);

place_y_o=num2str(obj_col);

clc;

temp=['原始点位置为',place_x_o,'行',place_y_o,'列'];

disp(temp);

if flag==1

plot(final_y,final_x,'*')

place_x=num2str(final_x);

place_y=num2str(final_y);

temp=['找到匹配点位置为',place_x,'行',place_y,'列'];

disp(temp);

else

disp('附近没有可匹配的点，请尝试修改网格大小');

end

axis([0 128 0 128]);

xlabel('.表示原始位置 *表示校准后位置');