lighthouse_geometry_solver

Classes

LighthouseGeometrySolution

LighthouseGeometrySolution()

Represents a solution from the geometry solver.

Some data in the object is also used during the solving process for context.

LighthouseGeometrySolver

LighthouseGeometrySolver()

Finds the poses of base stations and Crazyflie samples given a list of matched samples. The solver is iterative and uses least squares fitting to minimize the distance from the lighthouse sensors to each “ray” measured in the samples.

The equation system that is solved is defined as:

Columns are the estimated poses (what we solve for). Each pose is composed of 6 numbers (often referred to as parameters in the code): rotation vector (3) and position (3).

Rows are representing one angle from one base station. The number of rows for each sample is given by the number of bs in the sample * n_sensors * 2.

An examples matrix:

                bs0_pose, bs1_pose, bs2_pose, bs3_pose, cf1_pose, cf2_pose, ...

cf0/bs2/sens0/ang0 X cf0/bs2/sens0/ang1 X cf0/bs2/sens1/ang0 X cf0/bs2/sens1/ang1 X … cf0/bs3/sens0/ang0 X cf0/bs3/sens0/ang1 X cf0/bs3/sens1/ang0 X cf0/bs3/sens1/ang1 X … cf1/bs1/sens0/ang0 X X cf1/bs1/sens0/ang1 X X cf1/bs1/sens1/ang0 X X cf1/bs1/sens1/ang1 X X … cf1/bs2/sens0/ang0 X X cf1/bs2/sens0/ang1 X X cf1/bs2/sens1/ang0 X X cf1/bs2/sens1/ang1 X X … cf2/bs1/sens0/ang0 X X cf2/bs1/sens0/ang1 X X cf2/bs1/sens1/ang0 X X cf2/bs1/sens1/ang1 X X … cf2/bs3/sens0/ang0 X X cf2/bs3/sens0/ang1 X X cf2/bs3/sens1/ang0 X X cf2/bs3/sens1/ang1 X X …

Static methods

def solve(initial_guess: LhBsCfPoses, matched_samples: list[LhCfPoseSample], sensor_positions: npt.ArrayLike) ‑> cflib.localization.lighthouse_geometry_solver.LighthouseGeometrySolution

Solve for the pose of base stations and CF samples. The pose of the CF in sample 0 defines the global reference frame.

Iteration is terminated acceptable solution is found. If no solution is found after a fixed number of iterations the solver is terminated. The success member of the result will indicate if a solution was found or not.

Parameters