pose_optimize.cpp File Reference

PnP pose optimization using ceres. More...

#include "util/u_logging.h"
#include "tracking/t_camera_models.hpp"
#include "tracking/t_constellation.h"
#include "math/m_api.h"
#include "math/m_eigen_interop.hpp"
#include "math/m_quatexpmap.hpp"
#include "math/m_quatexpmap_bigceres.hpp"
#include "constellation/camera_model.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include "ceres/autodiff_cost_function.h"
#include "ceres/problem.h"
#include "ceres/solver.h"
#include "pose_optimize.hpp"
#include "ransac.hpp"
#include "math.hpp"
#include <iostream>
#include <stdio.h>

Include dependency graph for pose_optimize.cpp:

Macros
#define	BAD_COVARIANCE_MATRIX   (PoseStateCovarianceMatrix::Identity() * 1e6)
#define	BAD_WHITENING_MATRIX   (PoseStateCovarianceMatrix::Identity() * 1e-3)

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).

Detailed Description

PnP pose optimization using ceres.

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().