pose_optimize.hpp File Reference

RANSAC PnP pose refinement. More...

#include "xrt/xrt_config_have.h"
#include "xrt/xrt_defines.h"
#include "tracking/t_constellation.h"
#include "constellation/camera_model.h"

Include dependency graph for pose_optimize.hpp:

Go to the source code of this file.

Typedefs
typedef double	xrt::tracking::constellation::optimizer::RawPoseCovarianceMatrix[kPoseCovarianceSize *kPoseCovarianceSize]

Enumerations
enum class	PoseStateIndex : int {   PosX = 0 , PosY = 1 , PosZ = 2 , RotX = 3 ,   RotY = 4 , RotZ = 5 , RotW = 6 , NumIndices }
enum class	CovarianceIndex : int {   PosX = 0 , PosY = 1 , PosZ = 2 , RotX = 3 ,   RotY = 4 , RotZ = 5 , NumIndices }

Functions
bool	xrt::tracking::constellation::optimizer::optimizePose (u_logging_level log_level, bool deterministic, const t_camera_model_params &params, xrt_pose init_pose, t_blob *blobs, uint32_t num_blobs, t_constellation_tracker_led_model *leds_model, t_constellation_device_id_t device_id, xrt_pose &out_pose, RawPoseCovarianceMatrix out_covariance, RawPoseCovarianceMatrix out_whitening)
	Does a pose optimization and RANSAC refinement on the given blobs and LED model, returning the optimized pose and covariance.
void	xrt::tracking::constellation::optimizer::computePoseCovariance (u_logging_level log_level, const t_camera_model_params &params, xrt_pose init_pose, t_blob *blobs, uint32_t num_blobs, t_constellation_tracker_led_model *leds_model, t_constellation_device_id_t device_id, RawPoseCovarianceMatrix out_covariance, RawPoseCovarianceMatrix out_whitening)
	Computes the covariance of a pose given the initial pose, blobs, and LED model.
void	xrt::tracking::constellation::optimizer::covariancePositionRadius (const RawPoseCovarianceMatrix &raw_covariance, xrt_vec3 &radii, xrt_quat &Q_cam_cov)
	Computes the radii and covariance orientation from the 3x3 position part of the pose covariance matrix (3x eigenvalue, so 99%-ish probability containment).

Variables
constexpr int	xrt::tracking::constellation::optimizer::kPoseStateSize = static_cast<int>(PoseStateIndex::NumIndices)
constexpr int	xrt::tracking::constellation::optimizer::kPoseCovarianceSize = static_cast<int>(CovarianceIndex::NumIndices)

Detailed Description

RANSAC PnP pose refinement.

Author

Philipp Zabel phili.nosp@m.pp.z.nosp@m.abel@.nosp@m.gmai.nosp@m.l.com

Jan Schmidt jan@c.nosp@m.entr.nosp@m.icula.nosp@m.r.co.nosp@m.m

Beyley Cardellio ep1cm.nosp@m.1n10.nosp@m.n123@.nosp@m.gmai.nosp@m.l.com

Function Documentation

computePoseCovariance()

void xrt::tracking::constellation::optimizer::computePoseCovariance	(	u_logging_level	log_level,
		const t_camera_model_params &	params,
		xrt_pose	init_pose,
		t_blob *	blobs,
		uint32_t	num_blobs,
		t_constellation_tracker_led_model *	leds_model,
		t_constellation_device_id_t	device_id,
		RawPoseCovarianceMatrix	out_covariance,
		RawPoseCovarianceMatrix	out_whitening
	)

Computes the covariance of a pose given the initial pose, blobs, and LED model.

Parameters

	log_level	The logging level to use for debug output.
	params	The camera model parameters.
	init_pose	The initial pose estimate to start the optimization from.
	blobs	The array of blobs to use for the optimization.
	num_blobs	The number of blobs in the array.
	leds_model	The LED model to use for the optimization.
	device_id	The device ID to compute the covariance for.
[out]	out_covariance	The covariance of the final pose will be written to this variable.
[out]	out_whitening	The whitening matrix of the pose, must be non-nullptr if out_covariance is set.

Returns

void

References xrt::tracking::constellation::optimizer::computePoseCovariance().

Referenced by xrt::tracking::constellation::optimizer::computePoseCovariance().

covariancePositionRadius()

void xrt::tracking::constellation::optimizer::covariancePositionRadius	(	const RawPoseCovarianceMatrix &	raw_covariance,
		xrt_vec3 &	radii,
		xrt_quat &	Q_cam_cov
	)

Computes the radii and covariance orientation from the 3x3 position part of the pose covariance matrix (3x eigenvalue, so 99%-ish probability containment).

Parameters

	raw_covariance	The raw pose covariance matrix to compute the radii and orientation from.
[out]	radii	The radii of the position covariance will be written to this variable.
[out]	Q_cam_cov	The orientation of the covariance will be written to this variable.

References xrt::tracking::constellation::optimizer::covariancePositionRadius(), xrt::auxiliary::math::map_quat(), xrt::auxiliary::math::map_vec3(), and U_LOG_E.

Referenced by xrt::tracking::constellation::optimizer::covariancePositionRadius().

optimizePose()

bool xrt::tracking::constellation::optimizer::optimizePose	(	u_logging_level	log_level,
		bool	deterministic,
		const t_camera_model_params &	params,
		xrt_pose	init_pose,
		t_blob *	blobs,
		uint32_t	num_blobs,
		t_constellation_tracker_led_model *	leds_model,
		t_constellation_device_id_t	device_id,
		xrt_pose &	out_pose,
		RawPoseCovarianceMatrix	out_covariance,
		RawPoseCovarianceMatrix	out_whitening
	)

Does a pose optimization and RANSAC refinement on the given blobs and LED model, returning the optimized pose and covariance.

Parameters

	log_level	The logging level to use for debug output.
	params	The camera model parameters.
	init_pose	The initial pose estimate to start the optimization from.
	blobs	The array of blobs to use for the optimization, will write to here to unmark outliers.
	num_blobs	The number of blobs in the array.
	leds_model	The LED model to use for the optimization.
	device_id	The device ID to optimize the pose for.
[out]	out_pose	The optimized pose will be written to this variable.
[out]	out_covariance	The covariance of the final pose will be written to this variable, if not nullptr.
[out]	out_whitening	The whitening matrix of the pose, must be non-nullptr if out_covariance is set.

Returns

true if the optimization was successful, false otherwise.

References xrt::tracking::constellation::optimizer::optimizePose(), xrt::tracking::constellation::optimizer::ransacPose(), U_LOG_IFL_D, and U_LOG_IFL_E.

Referenced by xrt::tracking::constellation::optimizer::optimizePose().