Arbitrary points method for robot relocalization.¶
- compas_mrr.arbitrary_pts_localization.arbitrary_pts_localization(rcs_coords: List[List[float]], wcs_coords: List[List[float]], plot_results: bool = False, maxiter: int = 200) List[List[float]][source]¶
Calculate the RCS origin frame.
Finding the origin is formulated as an optimization problem where we want to find the origin and two orthonormal vectors defining the coordinate system. At the same time, the position of the localization points in this new coordinate system should match the measurements as close as possible. Therefore the deviation from the measurements is used as the cost function. The only constraints are that the x and y vector need to have length 1 and be orthogonal to each other. The optimization variable is a vector with 9 entries: X = [o, x, y] where o is the origin of the coordinate system and x, y the vectors spanning the x-y-plane. Each of them is a 3 dimensional vector. Important: Ensure that the order of rcs_coords and measurements is identical. I.e. the i-th entry in measurements is the measurement of the i-th localization point.
- Parameters:
rcs_coords – The points where the robot endeffector was positioned to take measurements. These points are in the RCS.
wcs_coords – The measurements taken in the world coordinate system (WCS) with the total station. These are the coordinates of the rcs_coords in the WCS.
- Returns:
A tuple of 3 vectors (lists with 3 elements) where the first represents the origin of the RCS, the second is the direction of the x axis and the third the direction of the y axis. The x and y axis are vectors with length 1.
- Return type: