navlie.lib.models.CameraProjection¶
- class navlie.lib.models.CameraProjection(pose_state_id: Any, landmark_state_id: Any, camera: PinholeCamera)¶
Bases:
MeasurementModelMeasurement model describing the projection of a landmark into a camera. In thihs measurement model, the robot pose and the landmark are both states.
The camera is assumed to be a fixed transformation from the robot body frame, where \((\mathbf{C}_{bc} \in SO(3), \mathbf{r}_b^{bc} \in \mathbb{R}^3)\) represent the extrinsic transformation from the body to the camera frame. The measurement model is then in the form
\[\mathbf{y} = \mathbf{g} (\mathbf{r}_c^{\ell c}),\]where \(\mathbf{r}_c^{\ell c}\) is the position of the landmark resolved in the camera frame, written as
\[\mathbf{r}_c^{\ell c} = \mathbf{C}_{bc}^T (\mathbf{C}_{ab}^T (\mathbf{r}_{\ell} - \mathbf{r}) - \mathbf{r}_b^{bc}).\]Here, the robot pose is given by \((\mathbf{C}_{ab} \in SO(3), \mathbf{r} \in \mathbb{R}^3 )\), and the landmark position is given by \(\mathbf{r}_\ell \in \mathbb{R}^3\). Also note that \(\mathbf{g} (\cdot)\) is the standard pinhole projection model, given by
\[g([x, y, z]^T) = [f_u * (x / z) + c_u, f_v * (y / z) + c_v]^T,\]where \(f_u, f_v, c_u, c_v\) are the camera intrinsics.
- Parameters:
pose_state_id (Any) – The state ID of the pose state
landmark_state_id (Any) – The state ID of the landmark state
camera (PinholeCamera model) – The PinholeCamera model, containing the information about the camera including the intrinsics and the extrinsics.
- evaluate(x: CompositeState) → ndarray¶
Evaluates the measurement model \(\mathbf{g}(\mathcal{X})\).
- jacobian(x: CompositeState) → ndarray¶
Evaluates the measurement model Jacobian with respect to the state.
\[\mathbf{G} = \frac{D \mathbf{g}(\mathcal{X})}{D \mathcal{X}}\]
- covariance(x: CompositeState) → ndarray¶
Returns the covariance \(\mathbf{R}\) associated with additive Gaussian noise.
- evaluate_with_jacobian(x: State) → Tuple[ndarray, ndarray]¶
Evaluates the measurement model and simultaneously returns the Jacobian as its second output argument. This is useful to override for performance reasons when the model evaluation and Jacobian have a lot of common calculations, and it is more efficient to calculate them in the same function call.