Levenberg-Marquardt kinematic optimizer. More...

#include "math/m_api.h"
#include "math/m_vec3.h"
#include "os/os_time.h"
#include "util/u_misc.h"
#include "util/u_trace_marker.h"
#include "tinyceres/tiny_solver.hpp"
#include "tinyceres/tiny_solver_autodiff_function.hpp"
#include "lm_rotations.inl"
#include <iostream>
#include <cmath>
#include <random>
#include "lm_interface.hpp"
#include "lm_optimizer_params_packer.inl"
#include "lm_defines.hpp"
#include "../hg_numerics_checker.hpp"
#include "lm_hand_init_guesser.hpp"

Include dependency graph for lm_main.cpp:

Functions
template<typename T >
static void	xrt::tracking::hand::mercury::lm::eval_hand_set_rel_translations (const OptimizerHand< T > &opt, Translations55< T > &rel_translations)

template<typename T >
void	xrt::tracking::hand::mercury::lm::eval_hand_set_rel_orientations (const OptimizerHand< T > &opt, Orientations54< T > &rel_orientations)

template<typename T >
void	xrt::tracking::hand::mercury::lm::eval_hand_with_orientation (const KinematicHandLM &state, const OptimizerHand< T > &opt, const bool is_right, Translations55< T > &translations_absolute, Orientations54< T > &orientations_absolute)

float	xrt::tracking::hand::mercury::lm::get_avg_curl_value (const one_frame_input &obs, const int finger)

HandScalar	xrt::tracking::hand::mercury::lm::calc_stability_curl_multiplier (const OptimizerFinger< HandScalar > &finger_last, HandScalar obs_curl)

template<typename T >
void	xrt::tracking::hand::mercury::lm::computeResidualStability_Finger (const one_frame_input &observation, const HandStability &stab, const OptimizerHand< T > &hand, const OptimizerHand< HandScalar > &last_hand, int finger_idx, ResidualHelper< T > &helper)

template<bool optimize_hand_size, typename T >
void	xrt::tracking::hand::mercury::lm::computeResidualStability (const OptimizerHand< T > &hand, const OptimizerHand< HandScalar > &last_hand, const KinematicHandLM &state, ResidualHelper< T > &helper)

template<typename T >
void	xrt::tracking::hand::mercury::lm::normalize_vector_inplace (Vec3< T > &vector)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::unit_vector_stereographic_projection (const Vec3< T > &in, Vec2< T > &out)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::unit_xrt_vec3_stereographic_projection (const xrt_vec3 in, Vec2< T > &out)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::calc_joint_rel_camera (const Vec3< T > &model_joint_pos, const Vec3< T > &move_joint_translation, const Quat< T > &move_joint_orientation, const Quat< T > &after_orientation, Vec3< T > &out_position)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::diff_stereographic (const Vec3< T > &model_joint_pos_rel_camera_, const vec2_5 &observed_ray_sg, const HandScalar confidence_xy, const HandScalar stereographic_radius, ResidualHelper< T > &helper)

template<typename T >
void	xrt::tracking::hand::mercury::lm::cjrc (const KinematicHandLM &state, const OptimizerHand< T > &hand, const Translations55< T > &translations_absolute, const int view, Vec3< T > out_model_joints_rel_camera[21])

template<typename T >
void	xrt::tracking::hand::mercury::lm::CostFunctor_PositionsPart (const OptimizerHand< T > &hand, const Translations55< T > &translations_absolute, const KinematicHandLM &state, ResidualHelper< T > &helper)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::diff_stereographic_reprojection_error (const Vec3< T > &model_joint_pos_rel_camera_, const vec2_5 &observed_ray_sg, const HandScalar confidence_xy, const HandScalar stereographic_radius, ResidualHelper< T > &helper)

template<typename T >
void	xrt::tracking::hand::mercury::lm::simple_reprojection_error (const OptimizerHand< T > &hand, const Translations55< T > &translations_absolute, const Orientations54< T > &orientations_absolute, const KinematicHandLM &state, ResidualHelper< T > &helper)

template<typename T >
void	xrt::tracking::hand::mercury::lm::print_residual_part (T *residual, int residual_size)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::zldtt_ori_right (Quat< T > &orientation, xrt_quat *out)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::zldtt_ori_left (Quat< T > &orientation, xrt_quat *out)

template<typename T >
static void	xrt::tracking::hand::mercury::lm::zldtt (Vec3< T > &trans, Quat< T > &orientation, bool is_right, xrt_space_relation &out)

static void	xrt::tracking::hand::mercury::lm::eval_to_viz_hand (KinematicHandLM &state, OptimizerHand< HandScalar > &opt, Translations55< HandScalar > translations_absolute, Orientations54< HandScalar > orientations_absolute, xrt_hand_joint_set &out_viz_hand)

template<bool optimize_hand_size>
float	xrt::tracking::hand::mercury::lm::opt_run (KinematicHandLM &state, one_frame_input &observation, xrt_hand_joint_set &out_viz_hand)

void	xrt::tracking::hand::mercury::lm::optimizer_finish (KinematicHandLM &state, xrt_hand_joint_set &out_viz_hand, float &out_reprojection_error)

void	xrt::tracking::hand::mercury::lm::optimizer_run (KinematicHandLM *hand, one_frame_input &observation, bool hand_was_untracked_last_frame, float smoothing_factor, bool optimize_hand_size, float target_hand_size, float hand_size_err_mul, float amt_use_depth, xrt_hand_joint_set &out_hand, float &out_hand_size, float &out_reprojection_error)
	The main tracking code calls this function with some 2D(ish) camera observations of the hand, and this function calculates a good 3D hand pose and writes it to out_viz_hand. More...

void	xrt::tracking::hand::mercury::lm::optimizer_create (xrt_pose left_in_right, bool is_right, u_logging_level log_level, KinematicHandLM **out_kinematic_hand)

void	xrt::tracking::hand::mercury::lm::optimizer_destroy (KinematicHandLM **hand)

Detailed Description

Levenberg-Marquardt kinematic optimizer.

Author: Moses Turner moses.nosp@m.@col.nosp@m.labor.nosp@m.a.co.nosp@m.m; Charlton Rodda charl.nosp@m.ton..nosp@m.rodda.nosp@m.@col.nosp@m.labor.nosp@m.a.co.nosp@m.m

Function Documentation

◆ calc_stability_curl_multiplier()

HandScalar xrt::tracking::hand::mercury::lm::calc_stability_curl_multiplier	(	const OptimizerFinger< HandScalar > &	finger_last,
		HandScalar	obs_curl
	)

Todo:: Use the neural net's output variance somehow

References xrt::tracking::hand::mercury::lm::calc_stability_curl_multiplier().

Referenced by xrt::tracking::hand::mercury::lm::calc_stability_curl_multiplier().

◆ diff_stereographic_reprojection_error()

template<typename T >

static void xrt::tracking::hand::mercury::lm::diff_stereographic_reprojection_error	(	const Vec3< T > &	model_joint_pos_rel_camera_,
		const vec2_5 &	observed_ray_sg,
		const HandScalar	confidence_xy,
		const HandScalar	stereographic_radius,
		ResidualHelper< T > &	helper
	)

static

Todo:: This works well but we can get a way more "rooted in math" way of increasing repro error with low-confidence measurements than this.

References xrt::tracking::hand::mercury::lm::diff_stereographic_reprojection_error(), and xrt::tracking::hand::mercury::lm::normalize_vector_inplace().

Referenced by xrt::tracking::hand::mercury::lm::diff_stereographic_reprojection_error().

◆ eval_hand_set_rel_orientations()

template<typename T >

void xrt::tracking::hand::mercury::lm::eval_hand_set_rel_orientations	(	const OptimizerHand< T > &	opt,
		Orientations54< T > &	rel_orientations
	)

inline

Todo:: : In this version of our tracking, this is always constant.

References xrt::tracking::hand::mercury::lm::eval_hand_set_rel_orientations().

Referenced by xrt::tracking::hand::mercury::lm::eval_hand_set_rel_orientations().

◆ normalize_vector_inplace()

template<typename T >

void xrt::tracking::hand::mercury::lm::normalize_vector_inplace ( Vec3< T > & vector )

inline

Todo:: any good template ways to get epsilon for float,double,jet?

References xrt::tracking::hand::mercury::lm::normalize_vector_inplace().

Referenced by xrt::tracking::hand::mercury::lm::diff_stereographic_reprojection_error(), and xrt::tracking::hand::mercury::lm::normalize_vector_inplace().

◆ opt_run()

template<bool optimize_hand_size>

float xrt::tracking::hand::mercury::lm::opt_run	(	KinematicHandLM &	state,
		one_frame_input &	observation,
		xrt_hand_joint_set &	out_viz_hand
	)

inline

Todo:: We don't yet know what "good" termination conditions are.

Todo:: We need to do a parameter sweep on initial_trust_region_radius.

Todo:: Is there a zero-copy way of doing this?

References xrt::tracking::hand::mercury::lm::opt_run().

Referenced by xrt::tracking::hand::mercury::lm::opt_run().

◆ optimizer_run()

void xrt::tracking::hand::mercury::lm::optimizer_run	(	KinematicHandLM *	hand,
		one_frame_input &	observation,
		bool	hand_was_untracked_last_frame,
		float	smoothing_factor,
		bool	optimize_hand_size,
		float	target_hand_size,
		float	hand_size_err_mul,
		float	amt_use_depth,
		xrt_hand_joint_set &	out_hand,
		float &	out_hand_size,
		float &	out_reprojection_error
	)

The main tracking code calls this function with some 2D(ish) camera observations of the hand, and this function calculates a good 3D hand pose and writes it to out_viz_hand.

Parameters

	observation	The observation of the hand joints. Warning, this function will mutate the observation unpredictably. Keep a copy of it if you need it after.
	hand_was_untracked_last_frame	If the hand was untracked last frame (it was out of view, obscured, ML models failed, etc.) - if it was, we don't want to enforce temporal consistency because we have no good previous hand state with which to do that.
	optimize_hand_size	Whether or not it's allowed to tweak the hand size - when we're calibrating the user's hand size, we want to do that; afterwards we don't want to waste the compute.
	target_hand_size	The hand size we want it to get close to
	hand_size_err_mul	A multiplier to help determine how close it has to get to that hand size
[out]	out_hand	The xrt_hand_joint_set to output its result to
[out]	out_hand_size	The hand size it ended up at
[out]	out_reprojection_error	The reprojection error it ended up at

Parameters

smoothing_factor - Unused if this is the first frame

References xrt::tracking::hand::mercury::lm::optimizer_run().

Referenced by xrt::tracking::hand::mercury::lm::optimizer_run().

Functions

Detailed Description

Function Documentation

◆ calc_stability_curl_multiplier()

◆ diff_stereographic_reprojection_error()

◆ eval_hand_set_rel_orientations()

◆ normalize_vector_inplace()

◆ opt_run()

◆ optimizer_run()