Correcting positions after loop closure in simultaneous localization and mapping algorithm

Patent metadata

Field	Value
Publication number	US-11927692-B2
Application number	US-202017070330-A
Country	US
Kind code	B2
Filing date	Oct 14, 2020
Priority date	Nov 13, 2019
Publication date	Mar 12, 2024
Grant date	Mar 12, 2024

Abstract

A method and system for generating a three-dimensional (3D) map of an environment is provided. An example method includes receiving a 3D scan and portions of a 2D map of the environment and receiving coordinates of the scan position in the 2D map. The method further includes associating the coordinates of the scan position with the portion of the 2D map. The method further includes linking the coordinates with the portion of the 2D map. The method further includes storing submap data for each of the plurality of submaps into a data object associated respective submaps. The method further includes performing a loop closure algorithm on each of the plurality of submaps. The method further includes, for each of the plurality of submaps for which the position anchor of the submap changed during performing the loop closure algorithm, determining a new data object position for the data objects.

First claim

What is claimed is:

1. A three-dimensional (3D) measuring device comprising:
a processor system including at least one of a 3D scanner controller and a two-dimensional (2D) scanner processor;
a 3D scanner operable to cooperate with the processor system to determine 3D coordinates;
a 2D scanner accessory including a 2D scanner operable to cooperate with the processor system to determine 2D coordinates;
a moveable platform operable to carry the 3D scanner and the 2D scanner, the 3D scanner being fixed relative to the 2D scanner;
wherein the processor system is responsive to executable instructions which when executed by the processor system is operable to:
cause the 3D scanner, while fixedly located at a scan position, to cooperate with the processor system to acquire a 3D scan of an environment, wherein the 3D scan comprises a global position and is partitioned into a plurality of submaps, each of the plurality of submaps comprising a position anchor;
cause the 2D scanner to cooperate with the processor system to acquire a portion of a 2D map of the environment;
cause the 2D scanner to determine coordinates of the scan position in the 2D map in response to the 3D scanner initiating the acquisition of the 3D scan;
link the coordinates of the scan position with the portion of the 2D map;
store submap data for each of the plurality of submaps into a data object associated with each of the plurality of submaps, wherein the submap data for each of the plurality of submaps comprises a data object position of the data object relative to the position anchor of the respective submap;
perform a loop closure algorithm on each of the plurality of submaps; and
for each of the plurality of submaps for which the position anchor of the submap changed during performing the loop closure algorithm, determine a new data object position for the data objects associated with each of the plurality of submaps for which the position anchor changed.

2. The 3D measuring device of claim 1 , wherein the processor system further operable to create the data object for each of the plurality of submaps.

3. The 3D measuring device of claim 1 , wherein determining the new relative position for the data objects associated with each of the plurality of submaps for which the position anchor changed is performed continuously or iteratively while acquiring the 3D scan of the environment.

4. The 3D measuring device of claim 1 , wherein linking the coordinates of the scan position with the portion of the 2D map is performed in real-time and offline using timestamps and feature points.

5. The 3D measuring device of claim 1 , wherein the processor system further operable to perform an initial registration of adjacent submaps of the plurality of submaps.

6. The 3D measuring device of claim 1 , wherein performing loop closure further comprises adjusting a trajectory.

7. The 3D measuring device of claim 1 , wherein the 2D scanner accessory further includes a position/orientation sensor, the position orientation sensor includes at least one sensor selected from the group consisting of an inclinometer, a gyroscope, a magnetometer, and an altimeter.

8. The 3D measuring device of claim 1 , wherein the moveable platform is a tripod having wheels and a brake.

9. The 3D measuring device of claim 1 , wherein the 3D scanner comprises a first light source, a first beam steering unit, a first angle measuring device, a second angle measuring device, and a first light receiver, the first light source operable to emit a first beam of light, the first beam steering unit operable to steer the first beam of light to a first direction onto a first object point, the first direction determined by a first angle of rotation about a first axis and a second angle of rotation about a second axis, the first angle measuring device operable to measure the first angle of rotation and the second angle measuring device operable to measure the second angle of rotation, the first light receiver operable to receive first reflected light, the first reflected light being a portion of the first beam of light reflected by the first object point, the first light receiver operable to produce a first electrical signal in response to the first reflected light, the first light receiver operable to cooperate with the processor system to determine a first distance to the first object point based at least in part on the first electrical signal, the 3D scanner operable to cooperate with the processor system to determine 3D coordinates of the first object point based at least in part on the first distance, the first angle of rotation and the second angle of rotation.

10. The 3D measuring device of claim 9 , wherein the 2D scanner comprises a second light source, a second beam steering unit, a third angle measuring device, and a second light receiver, the second light source operable to emit a second beam of light, the second beam steering unit operable to steer the second beam of light to a second direction onto a second object point, the second direction determined by a third angle of rotation about a third axis, the third angle measuring device operable to measure the third angle of rotation, the second light receiver operable to receive second reflected light, the second reflected light being a portion of the second beam of light reflected by the second object point, the second light receiver operable to produce a second electrical signal in response to the second reflected light, the 2D scanner operable to cooperate with the processor system to determine a second distance to the second object point based at least in part on the second electrical signal, the 2D scanner further operable to cooperate with the processor system to determine 2D coordinates of the second object point based at least in part on the second distance and the third angle of rotation.

11. The 3D measuring device of claim 9 , wherein the first beam steering unit includes a first mirror operable to rotate about a horizontal axis and a carriage that holds the first mirror operable to rotate about a vertical axis, the rotation about the horizontal axis being driven by a first motor and the rotation about the vertical axis being driven by a second motor.

12. The 3D measuring device of claim 1 , wherein the loop closure algorithm determines a displacement vector, which is representative of an error in estimating the coordinates of the scan position.

13. A method for generating a three-dimensional (3D) map of an environment, the method comprising:
receiving, by a processor system, via a 3D scanner, which is fixedly located at a scan position, a 3D scan of the environment, wherein the 3D scan comprises a global position and is partitioned into a plurality of submaps, each of the plurality of submaps comprising a position anchor;
receiving, by the processor system, via a two-dimensional (2D) scanner accessory, a portion of a 2D map of the environment;
receiving, by the processor system, coordinates of the scan position in the 2D map in response to the 3D scanner initiating the acquisition of the 3D scan;
associating, by the processor system, the coordinates of the scan position with the portion of the 2D map;
linking, by the processor system, the coordinates of the scan position with the portion of the 2D map;
storing, by the processor system, submap data for each of the plurality of submaps into a data object associated with each of the plurality of submaps, wherein the submap data for each of the plurality of submaps comprises a data object position of the data object relative to the position anchor of the respective submap;
performing, by the processor system, a loop closure al

Assignees

• Faro Tech Inc

Inventors

• Zweigle Oliver
• Waheed Mufassar
• Ramadneh Ahmad
• Frank Aleksej

Classifications

• G01S7/4808 Primary

  Evaluating distance, position or velocity data · CPC title

• G01S17/86

  Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders · CPC title

• G01S17/87

  Combinations of systems using electromagnetic waves other than radio waves · CPC title

• G01S17/89 Primary

  for mapping or imaging · CPC title