Generic math helpers for constellation optimizer code. More...

#include "tracking/t_camera_models.hpp"
#include "math/m_eigen_interop.hpp"
#include "math/m_quatexpmap.hpp"
#include "math/m_quatexpmap_bigceres.hpp"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "ceres/jet.h"
#include "ceres/product_manifold.h"
#include "ceres/autodiff_manifold.h"
#include "ceres/ceres.h"
#include "pose_optimize.hpp"
#include "math.hpp"
#include <ceres/crs_matrix.h>
#include <cstring>
#include <cmath>

Include dependency graph for internal_math.hpp:

Go to the source code of this file.

Data Structures
struct	xrt::tracking::constellation::optimizer::SeedableVector< T, N >

struct	xrt::tracking::constellation::optimizer::WorldGravity< T >
	The strength of gravity in the world, as a parameter block of its own. More...

struct	xrt::tracking::constellation::optimizer::PitchRoll< T >
	A parameter block for a camera with 2 degrees of freedom, pitch and roll. Yaw is fixed to 0, and it has no position. More...

struct	xrt::tracking::constellation::optimizer::Pose< T >
	Pose with 6 degrees of freedom, translation and rotation. More...

struct	xrt::tracking::constellation::optimizer::PoseWithVelocity< T >
	Pose with 9 degrees of freedom, translation, rotation, and linear velocity. More...

struct	xrt::tracking::constellation::optimizer::ImuExtrinsics< T >

struct	xrt::tracking::constellation::optimizer::QuaternionManifoldFunctor
	Right-multiplying quaternion manifold, matches our local convention. More...

Typedefs
typedef Eigen::Matrix< double, kPoseCovarianceSize, kPoseCovarianceSize >	xrt::tracking::constellation::optimizer::PoseStateCovarianceMatrix

typedef ceres::AutoDiffManifold< QuaternionManifoldFunctor, 4, 3 >	xrt::tracking::constellation::optimizer::QuaternionManifold
	Our quaternion manifold functor.

typedef ceres::ProductManifold< ceres::EuclideanManifold< 3 >, QuaternionManifold >	xrt::tracking::constellation::optimizer::PoseManifold
	Pose manifold, (tX, tY, tZ), (qX, qY, qZ, qW)

typedef ceres::ProductManifold< ceres::EuclideanManifold< 3 >, QuaternionManifold, ceres::EuclideanManifold< 3 > >	xrt::tracking::constellation::optimizer::PoseWithVelocityManifold
	Pose plus linear velocity, (tX, tY, tZ), (qX, qY, qZ, qW), (vX, vY, vZ). 10 ambient, 9 tangent.

Functions
Eigen::Isometry3f	xrt::tracking::constellation::optimizer::isometryFromPose (const xrt_pose &p)
	Converts an xrt_pose into an Eigen Isometry3f.

static int32_t	xrt::tracking::constellation::optimizer::findLedIndexById (const t_constellation_tracker_led_model *leds_model, t_constellation_led_id_it led_id)
	Finds a LED in the model by its id.

FactorFitness	xrt::tracking::constellation::optimizer::evaluateFactorFitness (ceres::Problem &problem, const std::vector< ceres::ResidualBlockId > &residual_blocks)
	Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.

void	xrt::tracking::constellation::optimizer::matrixCrsToEigen (const ceres::CRSMatrix &mat_crs, Eigen::MatrixXd &mat)
	Converts a CRS matrix to a dense Eigen matrix.

void	xrt::tracking::constellation::optimizer::marginalize (const VectorXd &residual, const MatrixXd &jacobian, const int num_base_states, VectorXd &out_e0, MatrixXd &out_j)
	Marginalize the first `num_base_states` states out of the linearized system given by `jacobian` and `residual`, handing back the linear prior that replaces them.

template<typename T >
static void	xrt::tracking::constellation::optimizer::project_led (const camera_model &params, const Eigen::Quaternion< T > &Q_cam_model, const Eigen::Vector3< T > &T_cam_model, const Eigen::Vector3< T > &T_model_led, Eigen::Vector2< T > &out_projected_point, T *led_depth_m)

template<typename T , typename Derived >
static void	xrt::tracking::constellation::optimizer::computeLedResidual (const camera_model &params, const Eigen::Vector3< T > &T_cam_model, const Eigen::Quaternion< T > &Q_cam_model, const Eigen::Vector2< T > &blob_position_2d, const Eigen::Vector3< T > &T_model_led, Eigen::MatrixBase< Derived > &residual)

Variables
constexpr bool	xrt::tracking::constellation::optimizer::kOptimizeUndistortedPoints = true
	Optimize directly on the pre-undistorted points, don't compute residuals in distorted pixel space.

constexpr bool	xrt::tracking::constellation::optimizer::kResidualDebugPrint = false
	Print debug info about the residuals as we're computing them.

constexpr double	xrt::tracking::constellation::optimizer::kMaxGravity = MATH_GRAVITY_M_S2 * 1.01

constexpr double	xrt::tracking::constellation::optimizer::kMinGravity = MATH_GRAVITY_M_S2 * 0.99

constexpr size_t	xrt::tracking::constellation::optimizer::kMinCorrespondencesPerObservation = 4
	Fewer correspondences than this and the observation is dropped.

constexpr double	xrt::tracking::constellation::optimizer::kCameraHuberDeltaSigmas = 3.0
	Where a camera observation stops being treated as data and starts being treated as a mistake, in standard deviations.

constexpr int	xrt::tracking::constellation::optimizer::kNumLedResiduals = 2
	X and Y in the image, for one LED.

constexpr double	xrt::tracking::constellation::optimizer::kBlobPositionSigmaPixels = 0.16
	How well a blob centroid locates the LED that lit it, in pixels.

Detailed Description

Generic math helpers for constellation optimizer code.

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

Typedef Documentation

◆ PoseManifold

typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, QuaternionManifold> xrt::tracking::constellation::optimizer::PoseManifold

Pose manifold, (tX, tY, tZ), (qX, qY, qZ, qW)

◆ PoseWithVelocityManifold

typedef ceres::ProductManifold<ceres::EuclideanManifold<3>, QuaternionManifold, ceres::EuclideanManifold<3> > xrt::tracking::constellation::optimizer::PoseWithVelocityManifold

Pose plus linear velocity, (tX, tY, tZ), (qX, qY, qZ, qW), (vX, vY, vZ). 10 ambient, 9 tangent.

◆ QuaternionManifold

typedef ceres::AutoDiffManifold<QuaternionManifoldFunctor, 4, 3> xrt::tracking::constellation::optimizer::QuaternionManifold

Our quaternion manifold functor.

Function Documentation

◆ evaluateFactorFitness()

FactorFitness xrt::tracking::constellation::optimizer::evaluateFactorFitness	(	ceres::Problem &	problem,
		const std::vector< ceres::ResidualBlockId > &	residual_blocks
	)

Measures how well one kind of factor was fit, at whatever the problem's parameters currently hold.

Both factor types are whitened, so this is directly comparable between them and against 1.

Parameters

problem	The problem, evaluated at its current parameter values.
residual_blocks	The blocks belonging to this kind of factor.

References xrt::tracking::constellation::optimizer::evaluateFactorFitness(), and xrt::tracking::constellation::optimizer::FactorFitness::num_residuals.

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

◆ findLedIndexById()

static int32_t xrt::tracking::constellation::optimizer::findLedIndexById	(	const t_constellation_tracker_led_model *	leds_model,
		t_constellation_led_id_it	led_id
	)

inlinestatic

Finds a LED in the model by its id.

A LED's id is an opaque identifier chosen by the driver, not its position in the model: drivers are free to derive it from device-specific numbering (the Rift uses the headset's own LED indices, which include a slot for the IMU). So a blob's matched_device_led_id has to be resolved through here rather than used as an index.

Returns: The index of the LED in the model, or -1 when no LED carries that id.

References xrt::tracking::constellation::optimizer::findLedIndexById(), t_constellation_tracker_led::id, t_constellation_tracker_led_model::led_count, and t_constellation_tracker_led_model::leds.

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

◆ isometryFromPose()

Eigen::Isometry3f xrt::tracking::constellation::optimizer::isometryFromPose ( const xrt_pose & p )

inline

Converts an xrt_pose into an Eigen Isometry3f.

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

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

◆ marginalize()

void xrt::tracking::constellation::optimizer::marginalize	(	const VectorXd &	residual,
		const MatrixXd &	jacobian,
		const int	num_base_states,
		VectorXd &	out_e0,
		MatrixXd &	out_j
	)

Marginalize the first num_base_states states out of the linearized system given by jacobian and residual, handing back the linear prior that replaces them.

Inspired/specialized from MarginalizationError::marginalizeOut() from OKVIS.

References xrt::tracking::constellation::optimizer::marginalize(), and xrt::tracking::constellation::optimizer::robustComputeSqrtAndPinvSqrt().

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

◆ matrixCrsToEigen()

void xrt::tracking::constellation::optimizer::matrixCrsToEigen	(	const ceres::CRSMatrix &	mat_crs,
		Eigen::MatrixXd &	mat
	)

Converts a CRS matrix to a dense Eigen matrix.

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

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

Variable Documentation

◆ kBlobPositionSigmaPixels

constexpr double xrt::tracking::constellation::optimizer::kBlobPositionSigmaPixels = 0.16

constexpr

How well a blob centroid locates the LED that lit it, in pixels.

The camera residuals are reprojection errors, so they are only comparable to the whitened IMU residuals once they are divided by the noise that produced them. This is the same floor the per-frame PnP puts on its own residual sigma, since a blob has about this much pixels of noise, at least on CV1.

◆ kCameraHuberDeltaSigmas

constexpr double xrt::tracking::constellation::optimizer::kCameraHuberDeltaSigmas = 3.0

constexpr

Where a camera observation stops being treated as data and starts being treated as a mistake, in standard deviations.

Used to size the Huber loss function.

The loss is applied to a whole observation rather than to each LED of it, so this is measured against the spread of the observation's chi-square rather than against a single reprojection error, and the threshold it produces grows with the number of correspondences.

◆ kMinCorrespondencesPerObservation

constexpr size_t xrt::tracking::constellation::optimizer::kMinCorrespondencesPerObservation = 4

constexpr

Fewer correspondences than this and the observation is dropped.

Any less and this is likely a degenerate solve.

Referenced by xrt::tracking::constellation::optimizer::rig_calibration::RigCalibration::pushCameraSample().

◆ kNumLedResiduals

constexpr int xrt::tracking::constellation::optimizer::kNumLedResiduals = 2

constexpr

X and Y in the image, for one LED.

◆ kOptimizeUndistortedPoints

constexpr bool xrt::tracking::constellation::optimizer::kOptimizeUndistortedPoints = true

constexpr

Optimize directly on the pre-undistorted points, don't compute residuals in distorted pixel space.

We don't have a "project" routine for the CV1 distortion algorithm, so we work in undistorted space, instead.

Referenced by xrt::tracking::constellation::optimizer::rig_calibration::RigCalibration::pushCameraSample().

◆ kResidualDebugPrint

constexpr bool xrt::tracking::constellation::optimizer::kResidualDebugPrint = false

constexpr

Print debug info about the residuals as we're computing them.

Data Structures

Typedefs

Functions

Variables

Detailed Description

Typedef Documentation

◆ PoseManifold

◆ PoseWithVelocityManifold

◆ QuaternionManifold

Function Documentation

◆ evaluateFactorFitness()

◆ findLedIndexById()

◆ isometryFromPose()

◆ marginalize()

◆ matrixCrsToEigen()

Variable Documentation

◆ kBlobPositionSigmaPixels

◆ kCameraHuberDeltaSigmas

◆ kMinCorrespondencesPerObservation

◆ kNumLedResiduals

◆ kOptimizeUndistortedPoints

◆ kResidualDebugPrint