scalingIterativeClosestPoint#

Executive Summary#

Scaling Iterative Closest Point is an implementation of the Iterative Closest Point algorith, with a scale factor added to the optimization. The algorithm seeks to identify the rotation, scale, and translation in order to best overlap two point clouds. This implementation produces the measured point cloud after it has been fit to the reference as well as the geometric transformations that were found by SICP in a separate message.

Message Connection Descriptions#

The input messages are both of the same type since they are both point clouds. One is a measured point cloud (which could be output by a LIDAR for instance) and the reference is input given the space model of the target. The output messages are the geometrical transformations identified by the algorithm (translation, scale, and rotation), and the resulting measured point cloud after it has been transformed to fit the reference.

Module I/O Messages#
Msg Variable Name	Msg Type	Description
measuredPointCloud	PointCloudMsgPayload	input point cloud that is measured and needs to be fit
referencePointCloud	PointCloudMsgPayload	input reference point cloud that the measured cloud is trying to fit to
outputPointCloud	PointCloudMsgPayload	output point cloud once transformed to fit the reference
outputSICPData	SICPMsgPayload	output transformations to apply to the input measured cloud to best fit the reference

Detailed Module Description#

The algorithm is described and derived in the reference paper: SICP by Du et al The summary can be found in pseudo-code pages 444-445 and similar notation is used in the implementation.

User Guide#

This section is to outline the steps needed to setup a SICPin Python.

Import the SICP class:

from Basilisk.fswAlgorithms import scalingIterativeClosestPoint

Create an instantiation of the module:

sicp = scalingIterativeClosestPoint.ScalingIterativeClosestPoint()

#. Define all physical parameters. The max number of iterations describes how times SICP will try and fit the data if the min errors are not met. The errorTolerance defines that maximum error, scalingMin and Max are the max allowable scaling range for the scalar term:

sicp.maxIterations = 20
sicp.errorTolerance = 1E-5
sicp.scalingMax = 1.1
sicp.scalingMin = 0.9

Subscribe to the input cloud messages

inputPointCloud = messaging.PointCloudMsgPayload()
referencePointCloud = messaging.PointCloudMsgPayload()

Class ScalingIterativeClosestPoint#

class ScalingIterativeClosestPoint : public SysModel#

Scaling iterative Closest Point Algorithm.

Public Functions

void updateState(uint64_t currentSimNanos) override#

This module reads the data and checks for validity. If new information is present it

Parameters:: currentSimNanos – The clock time at which the function was called (nanoseconds)
Returns:: void

void reset(uint64_t currentSimNanos) override#

This method performs a complete reset of the module. Local module variables that retain time varying states between function calls are reset to their default values.

Parameters:: currentSimNanos – The clock time at which the function was called (nanoseconds)
Returns:: void

Public Members

Message<PointCloudMsgPayload> outputPointCloud#: The output fitted point cloud.

Message<SICPMsgPayload> outputSICPData#: The output algorithm data.

ReadFunctor<SICPMsgPayload> initialCondition#: The input measured data.

ReadFunctor<PointCloudMsgPayload> measuredPointCloud#: The input measured data.

ReadFunctor<PointCloudMsgPayload> referencePointCloud#: The input reference data.

BSKLogger bskLogger#: — BSK Logging

double scalingMax = 1.1#: Scaling maximums.

double scalingMin = 0.9#: Scaling minimums.

double errorTolerance = 1e-10#: Error tolerance for convergence.

int maxIterations = MAX_ITERATIONS#: Max iterations.

int numberScalePoints = 100#

Number of points in order to find the scale factor.

Initial conditions that could be set by user to better start off the SICP apgorithm