% Improved Iterative Closest Point algorithm

% Point Clouds from structure from motion and ultrasonic sensors array

availability in the system

If 2 point clouds data are available,

If area is reliable, continue; end;

else, wait for more data.

% Area is unreliable when it is at the corner of a number of objects or when it

reflects a complex object.

% Correspondence estimation

If both point clouds are projected in 2D space

Then corresponding point are the closest points in the 2D grid

% The points with the smallest distance between them in the 2D space are chosen.

% Distance from the ultrasonic reading to determine the range to the obstacle

For all points in the point clouds with similar neighborhood,

use the distance from the ultrasonic reading to determine the range to the obstacle

since ultrasonic sensors generated point clouds are sparse but more reliable;

% Transformation estimation

for all points:

1. apply transformation on both point clouds which consist of reducing

drastically the point-to-point distance

2. match points of the corresponding point cloud while changing the Z component

of the structure from motion point cloud with the distance from the obstacle

gathered by the ultrasonic sensors.

3. transform the reference point cloud using the estimation of the present location.

% Fusing with the second registered point cloud.

end;