cameraTriangulation#
Executive Summary#
This module estimates the position of a camera using triangulation. Given a point cloud of feature locations (such as boulders on an asteroid), and the pixel coordinates of these features (key points) obtained from an image that the camera took, the camera location is estimated by triangulation. The triangulation algorithm is based on this paper by S. Henry and J. A. Christian (Equation 20). The covariance matrix of the estimated camera location is also computed according to Equation A5 in the appendix of the paper.
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.
Msg Variable Name |
Msg Type |
Description |
|---|---|---|
pointCloudInMsg |
PointCloudMsgPayload |
point cloud input message |
keyPointsInMsg |
PairedKeyPointsMsgPayload |
key points (features) input message |
cameraConfigInMsg |
CameraConfigMsgPayload |
camera configuration input message |
cameraLocationOutMsg |
CameraLocalizationMsgPayload |
camera location output message |
Module Assumptions and Limitations#
The module assumes the number of features in the provided point cloud and the number of key points from the camera image are equal, and that the features and corresponding key points are in the same order. 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 camera location \({}^N\mathbf{r}\), expressed in the inertial frame N, is estimated by solving the linear least squares problem
where \({}^N\mathbf{p}_i\) are the known points (point cloud), \(\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\).
User Guide#
The module is first initialized as follows:
module = cameraTriangulation.CameraTriangulation()
module.modelTag = "cameraTriangulation"
module.uncertaintyImageMeasurement = 0.25 # optional, default = 0.
unitTestSim.AddModelToTask(unitTaskName, module)
The input messages are then connected:
module.pointCloudInMsg.subscribeTo(pointCloudInMsg)
module.keyPointsInMsg.subscribeTo(keyPointsInMsg)
module.cameraConfigInMsg.subscribeTo(cameraConfigInMsg)
Class CameraTriangulation#
-
class CameraTriangulation : public SysModel#
This module triangulates a camera position from a point cloud.
Public Functions
-
CameraTriangulation()#
This is the constructor for the module class. It sets default variable values and initializes the various parts of the model
-
~CameraTriangulation()#
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<PointCloudMsgPayload> pointCloudInMsg#
point cloud input message
-
ReadFunctor<PairedKeyPointsMsgPayload> keyPointsInMsg#
key (feature) points input message
-
ReadFunctor<CameraConfigMsgPayload> cameraConfigInMsg#
camera configuration input message
-
Message<CameraLocalizationMsgPayload> cameraLocationOutMsg#
estimated camera location output message
-
BSKLogger bskLogger#
— BSK Logging
-
double uncertaintyImageMeasurement = 0.#
[pixel] standard deviation of image point errors (about 1/4 to 1/2 px)
-
CameraTriangulation()#