MATLAB code summary

MATLAB code summary

Recently, I have written a lot of code with MATLAB, so I will summarize it here:

1: The macro definition is really convenient, especially if all the parameters are put together, you can easily modify the parameters to see the expected result.

2: A particle is known, and the particle code is scattered in the nine-square grid around it

pix_exp = 5;
angle_exp = 5;  
for count_x = -(pix_exp-1)/2 : 1 : (pix_exp-1)/2
    for count_y = -(pix_exp-1)/2 : 1 : (pix_exp-1)/2
        for count_angle = -(angle_exp-1)/2 : 1 : (angle_exp-1)/2
            particle_point = particle_potential + [count_x;count_y;count_angle*angle_resol];
            %如果该点在地图范围内，则撒粒子，否则不撒
            if particle_point(1) < map_x && particle_point(1) > 0 && particle_point(2) < map_y && particle_point(2) > 0 
                P(:,(P_num)) = particle_point;
                P_num = P_num + 1;
            end
        end
    end
end

3: Know a point, in the image, draw a circle in the image with this point as the center

% 计算圆上点的坐标并取整。其中feature_point_x，y为圆心，Dis_corner_feature为半径，count_angle为自定义的角度数组，0到2pi
circle_x = round(feature_point_x(i) + Dis_corner_feature*cos(count_angle(k)));
circle_y = round(feature_point_y(i) + Dis_corner_feature*sin(count_angle(k)));
if circle_x < map_x && circle_x > 0 && circle_y < map_y && circle_y > 0
    count_zero_point(circle_y,circle_x) = count_zero_point(circle_y,circle_x) + 1;
end

4: Same as 3, many problems can be solved by discretization. For example, when solving the intersection of trigonometric functions, such as the Hough transform algorithm, it is realized by constructing a matrix and discretizing the angle.

5: Regarding the Monte Carlo algorithm, although this algorithm is very mindless, it is definitely the most useful for single-frame data of lidar and known global maps. At the same time, this algorithm can also be used for map construction, dynamic positioning, etc. It can be said that one The algorithm solves several hot issues in the field of robotics. The disadvantage is that the amount of calculation is huge.

6: Knowing the coordinates and angles of the points, draw the points with arrows in the figure: where mypose(1) is the x coordinate, mypose(2) is the y coordinate, and mypose(3) is the angle information. In fact, it is a circle to represent the point, and three lines to draw an arrow.

px = myPose(1)+20*cos(myPose(3));
py = myPose(2)+20*sin(myPose(3));
plot(myPose(1),myPose(2),'og');
plot([myPose(1) px],[myPose(2) py],'-r');
plot([px px+10*cos(myPose(3) - 2*pi/3)],[py py+10*sin(myPose(3) - 2*pi/3)],'-r');
plot([px px+10*cos(myPose(3) + 2*pi/3)],[py py+10*sin(myPose(3) + 2*pi/3)],'-r');

7: Least squares method, input the x, y coordinate array, output the straight line y=a*x+b fitted by the least squares, and output the maximum and minimum x values, and indicate its position in the array.

%least squares 
function least_squares_result = least_squares_point(x,y)
point_num = length(x);

x_min = x(1);x_max = x(1);
x2 = 0;x1 = 0;x1y1 = 0;y1 = 0;point_min = 0;point_max = 0;
for j = 1 : 1 : point_num
    if x(j) < x_min
        x_min = x(j);
        point_min = j;
    end
    if x(j) > x_max
        x_max = x(j);
        point_max = j;
    end       
    x2 = x2 + x(j)^2;   % 求Σ(xi^2)
    x1 = x1 + x(j);
    x1y1 = x1y1 + x(j)*y(j);
    y1 = y1 + y(j);
end
%解出直线y=ax+b斜率a=(n*x1y1-x1*y1)/(n*x2-x1*x1)
a=(point_num*x1y1-x1*y1)/(point_num*x2-x1*x1); 
%解出直线截距b=(y1-a*x1)/n
b=(y1-a*x1)/point_num;                    
%     py=a*px+b;
%     plot(px,py,'-g');
result = [a,b,x_min,x_max,point_min,point_max];
least_squares_result = result;