Lidar2DMatchScansGrid
- class quanser.image_processing.lidar_match_scans.Lidar2DMatchScansGrid(resolution=20.0, max_range=5.0)
A Python wrapper for the Quanser LIDAR 2D Match Scans Grid API.
- Parameters
resolution (float) – The resolution.
max_range (float) – The maximum range.
- Raises
ImageProcessingError – On non-zero return code. A suitable error message may be retrieved using get_error_message.
Examples
Construct a new 2D LIDAR scan matcher.
>>> from quanser.image_processing import Lidar2DMatchScanGrid >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> # ... ... >>> lidar2d_match_scan.close()
- close()
Closes the 2D LIDAR scan matcher and frees up resources allocated by the matcher. Scan matching can consume a significant amount of memory and system resources.
- Raises
ImageProcessingError – On non-zero return code. A suitable error message may be retrieved using get_error_message.
Example
Close the 2D LIDAR scan matcher.
>>> from quanser.image_processing import Lidar2DMatchScanGrid >>> resolution = 13.0 >>> max_range = 8.0 >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> lidar2d_match_scan.open(resolution, max_range) >>> # ... ... >>> lidar2d_match_scan.close()
- match(ref_ranges, ref_angles, num_ref_points, ranges, angles, num_points, initial_pose, translation_search_range, rotation_search_range, pose, score, covariance)
Match the current scan to the reference scan, returning the pose of the current scan relative to the reference scan. If the score and/or covariance outputs are non-NULL then it will return the confidence in the match as a scalar score and/or 3x3 covariance matrix. For the fastest results, an initial pose may be specified, that may be derived from IMU data, for instance. In that case, if a translation search range and/or rotation search range is indicated then it will limit the search to those ranges.
If the translation_search_range is NULL then values of 4 meters will be used. If the rotation_search_range is zero then a search range of 2*PI will be used.
- Parameters
ref_ranges (array_like) – A vector of range values for the reference scan.
ref_angles (array_like) – A vector of angles or heading values for the reference scan.
num_ref_points (int) – The number of valid points in reference scan as specified in the “ref_ranges” and “ref_angles” array.
ranges (array_like) – A vector of range values for the current scan.
angles (array_like) – A vector of angle or heading values for the current scan.
num_points (int) – The number of valid points in the current scan.
initial_pose (array_like) – The initial pose as a 3-vector at which to start the grid search. The “translation_search_range” and “rotation_search_range” are relative to this initial pose. The pose consists of the elements [x, y, theta] where x and y denote the translation in meters and theta denotes the rotation in radians.
translation_search_range (array_like) – The translation search range as a 2-vector [x,y] indicating the cartesian search range in meters.
rotation_search_range (float) – The rotation search range as a scalar indicating the angular search range in radians.
pose (array_like) – The pose as a 3-vector [x,y,theta] representing the transformation between the current scan and the reference scan. The (x,y) components are the translation and the theta component is the rotation.
score (array_like) – A score representing the confidence in the match. Higher values denote higher confidence. The value is not normalized so it is highly dependent on the input scans and block parameters.
covariance (array_like) – A 3x3 matrix representing an estimate of the covariance of the result. This output is currently set to zero but may be added in future revisions.
- Raises
ImageProcessingError – On non-zero return code. A suitable error message may be retrieved using get_error_message.
Example
Read the 2D LIDAR scan matcher.
>>> from quanser.image_processing import Lidar2DMatchScanGrid >>> resolution = 13.0 >>> max_range = 8.0 >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> lidar2d_match_scan.open(resolution, max_range) >>> # ... ...#TODO: <to be filled in> >>> lidar2d_match_scan.close()
Examples
Read the 2D LIDAR scan matcher.
Using array:
>>> from array import array >>> from quanser.image_processing import Lidar2DMatchScanGrid >>> resolution = 13.0 >>> max_range = 8.0 >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> lidar2d_match_scan.open(resolution, max_range) >>> try: >>> ref_ranges = array('f', [2.0110, 1.9910, 2.0350, 2.0220, 2.0730, 2.0600, 2.1130, 2.0850, 2.1400, 2.1270, 2.1870]) >>> ref_angles = array('f', [-0.8238, -0.8348, -0.8432, -0.8538, -0.8623, -0.8732, -0.8814, -0.8923, -0.9008, -0.9114, -0.9198]) >>> num_ref_points = len(ref_ranges) >>> >>> ranges = array('f', [3.0750, 3.0120, 3.0460, 3.0670, 3.0770, 3.0450, 3.1970, 3.1380, 3.2460, 3.2230, 3.2700]) >>> angles = array('f', [-1.0963, -1.1110, -1.1154, -1.1304, -1.1347, -1.1497, -1.1541, -1.1688, -1.1735, -1.1882, -1.1926]) >>> num_points = len(ranges) >>> >>> initial_pose = array('f', [0, 0, 0]) >>> translation_search_range = array('f', [4, 4]) >>> rotation_search_range = 2 * math.PI >>> >>> pose = array('f', [0.0] * 3) >>> score = array('f', [0.0]) >>> covariance = array('f', [0.0] * 9) >>> >>> lidar2d_match_scan.match( >>> ref_ranges, ref_angles, num_ref_points, >>> ranges, angles, num_points, >>> initial_pose, translation_search_range, rotation_search_range, >>> pose, score, covariance) >>> # ... ... >>> finally: >>> lidar2d_match_scan.close()
Using numpy:
>>> import numpy as np >>> from quanser.image_processing import Lidar2DMatchScanGrid >>> resolution = 13.0 >>> max_range = 8.0 >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> lidar2d_match_scan.open(resolution, max_range) >>> try: >>> ref_ranges = np.array([2.0110, 1.9910, 2.0350, 2.0220, 2.0730, 2.0600, 2.1130, 2.0850, 2.1400, 2.1270, 2.1870], dtype=np.float32) >>> ref_angles = np.array([-0.8238, -0.8348, -0.8432, -0.8538, -0.8623, -0.8732, -0.8814, -0.8923, -0.9008, -0.9114, -0.9198], dtype=np.float32) >>> num_ref_points = len(ref_ranges) >>> >>> ranges = np.array([3.0750, 3.0120, 3.0460, 3.0670, 3.0770, 3.0450, 3.1970, 3.1380, 3.2460, 3.2230, 3.2700], dtype=np.float32) >>> angles = np.array([-1.0963, -1.1110, -1.1154, -1.1304, -1.1347, -1.1497, -1.1541, -1.1688, -1.1735, -1.1882, -1.1926], dtype=np.float32) >>> num_points = len(ranges) >>> >>> initial_pose = np.array([0, 0, 0], dtype=np.float32) >>> translation_search_range = np.array([4, 4], dtype=np.float32) >>> rotation_search_range = 2 * math.PI >>> >>> pose = np.zeros(3, dtype=np.float32) >>> score = np.zeros(1, dtype=np.float32) >>> covariance = np.zeros(9, dtype=np.float32) >>> >>> lidar2d_match_scan.match( >>> ref_ranges, ref_angles, num_ref_points, >>> ranges, angles, num_points, >>> initial_pose, translation_search_range, rotation_search_range, >>> pose, score, covariance) >>> # ... ... >>> finally: >>> lidar2d_match_scan.close()
- open(resolution, max_range)
Opens a 2D LIDAR scan matcher.
- Parameters
resolution (float) – The resolution.
max_range (float) – The maximum range.
- Raises
ImageProcessingError – On non-zero return code. A suitable error message may be retrieved using get_error_message.
Examples
Open the 2D LIDAR scan matcher with 13 grid cells per meter and any LIDAR scan data beyond 8 metre to be discarded.
>>> from quanser.image_processing import Lidar2DMatchScanGrid >>> resolution = 13.0 >>> max_range = 8.0 >>> lidar2d_match_scan = Lidar2DMatchScanGrid() >>> lidar2d_match_scan.open(resolution, max_range) >>> # ... ... >>> lidar2d_match_scan.close()