% 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; |