pointCloudTriangulation#

Executive Summary#

This module creates a point cloud using triangulation. Given the pixel coordinates of features (key points, such as boulders on an asteroid) obtained from two images that the camera took at two known camera locations, the point cloud is estimated by triangulation. The triangulation algorithm is based on this paper by S. Henry and J. A. Christian (Equation 20).

Message Connection Descriptions#

The following table lists all the module input and output messages. The module msg connection is set by the user from python. The msg type contains a link to the message structure definition, while the description provides information on what this message is used for.

Module I/O Messages#
Msg Variable Name	Msg Type	Description
ephemerisInMsg	EphemerisMsgPayload	ephemeris input message
navTransInMsg	NavTransMsgPayload	translational navigation input message
directionOfMotionInMsg	DirectionOfMotionMsgPayload	direction of motion input message
keyPointsInMsg	PairedKeyPointsMsgPayload	key points (features) input message
cameraConfigInMsg	CameraConfigMsgPayload	camera configuration input message
pointCloudOutMsg	PointCloudMsgPayload	point cloud output message

Module Assumptions and Limitations#

The module assumes the number of key points (features) in both “images” (at camera location 1 and 2) of the PairedKeyPointsMsgPayload are equal, and that the key points are in the same order in both images. The module also assumes that all key points come from a single camera, so the camera calibration matrix \([K]\) is the same for all key points.

Algorithm#

The unknown feature location (point of point cloud) \({}^N\mathbf{r}\), expressed in the inertial frame N, is estimated by solving the linear least squares problem

(1)#\[\begin{split}\begin{bmatrix} [\tilde{\bar{\mathbf{x}}_1}][CN_1] \\ [\tilde{\bar{\mathbf{x}}_2}][CN_2] \\ \vdots \\ [\tilde{\bar{\mathbf{x}}_n}][CN_n] \end{bmatrix} {}^N\mathbf{r} = \begin{bmatrix} [\tilde{\bar{\mathbf{x}}_1}][CN_1] {}^N\mathbf{p}_1 \\ [\tilde{\bar{\mathbf{x}}_2}][CN_2] {}^N\mathbf{p}_2 \\ \vdots \\ [\tilde{\bar{\mathbf{x}}_n}][CN_n] {}^N\mathbf{p}_n \end{bmatrix}\end{split}\]

where \({}^N\mathbf{p}_i\) are the known camera locations, \(\bar{\mathbf{x}}_i = [K_i] \bar{\mathbf{u}}_i\) with \(\bar{\mathbf{u}}_i = [\mathbf{u}_i, 1]^T\) and image point in pixel space \(\mathbf{u}_i\). The tilde \([\tilde{\mathbf{x}}]\) indicates the skew-symmetric matrix that is equivalent to the cross product \(\mathbf{x} \times\).

The module solves this least squares problem in a loop for all points of the point cloud.

User Guide#

The module is first initialized as follows:

module = pointCloudTriangulation.PointCloudTriangulation()
module.modelTag = "pointCloudTriangulation"
module.numberTimeStepsInitialPhase = 3  # optional (defaults to 5)
unitTestSim.AddModelToTask(unitTaskName, module)

The input messages are then connected:

module.ephemerisInMsg.subscribeTo(ephemerisInMsg)
module.navTransInMsg.subscribeTo(navTransInMsg)
module.directionOfMotionInMsg.subscribeTo(directionOfMotionInMsg)
module.keyPointsInMsg.subscribeTo(keyPointsInMsg)
module.cameraConfigInMsg.subscribeTo(cameraConfigInMsg)

Class PointCloudTriangulation#

class PointCloudTriangulation : public SysModel#

This module triangulates a camera position from a point cloud.

Public Functions

PointCloudTriangulation()#: This is the constructor for the module class. It sets default variable values and initializes the various parts of the model

~PointCloudTriangulation()#: Module Destructor

void reset(uint64_t currentSimNanos) override#

This method is used to reset the module and checks that required input messages are connected.

Parameters:: currentSimNanos – current simulation time in nano-seconds
Returns:: void

void updateState(uint64_t currentSimNanos) override#

This is the main method that gets called every time the module is updated.

Parameters:: currentSimNanos – current simulation time in nano-seconds
Returns:: void

Public Members

ReadFunctor<EphemerisMsgPayload> ephemerisInMsg#: ephemeris input message

ReadFunctor<NavTransMsgPayload> navTransInMsg#: translational navigation input message

ReadFunctor<DirectionOfMotionMsgPayload> directionOfMotionInMsg#: direction of motion input message

ReadFunctor<PairedKeyPointsMsgPayload> keyPointsInMsg#: key (feature) points input message

ReadFunctor<CameraConfigMsgPayload> cameraConfigInMsg#: camera configuration input message

Message<PointCloudMsgPayload> pointCloudOutMsg#: point cloud output message

BSKLogger bskLogger#

— BSK Logging

number of times (time steps) the module should use the ephemeris message before using the navigation message