Classes
class	VSPlot

Functions
ColVector	fx (ColVector x, float dt)
ColVector	hx (ColVector x)
ColVector	add_state_vectors (a, b)
ColVector	residual_state_vectors (a, b)
Matrix	generate_Q_matrix (float dt, float sigma)
None	read_data (ImageRGBa I_rgba, ImageUInt16 I_depth, rs.pipeline pipe, rs.align align)
Tuple[CameraParameters, int, int]	cam_from_rs_profile (profile)
	has_class_box (box)

Variables
	parser = argparse.ArgumentParser('Centering task using a YOLO network')
	type
	int
	help
	args = parser.parse_args()
	detection_model = YOLO('yolov8n.pt')
	plotter = VSPlot()
	pipe = rs.pipeline()
	config = rs.config()
int	fps = 30
	cfg = pipe.start(config)
	cam
	h
	w
	depth_scale = cfg.get_device().first_depth_sensor().get_depth_scale()
	I = ImageRGBa(h, w)
	I_depth = ImageUInt16(h, w)
	Idisp = ImageRGBa(h, w)
	align = rs.align(rs.stream.color)
	get_images = partial(read_data, pipe=pipe, align=align)
	robot = RobotAfma6()
	r = PoseVector(0.06706274856, 0.3844766362, -0.04551332622 , 0.3111005431, 0.3031078532, 0.01708581392)
	cTw = HomogeneousMatrix(r)
	drawer = UKSigmaDrawerMerwe(6, alpha=0.0001, beta=2, kappa=-3, resFunc=residual_state_vectors, addFunc=add_state_vectors)
int	pixel_noise = 1
	noise_x
	noise_y
int	noise_vel = 1e-8
int	noise_accel = 1e-12
	measure_covariance
	process_covariance
	kalman = UnscentedKalman(generate_Q_matrix(1 / fps, sigma=1e-8), measure_covariance, drawer, fx, hx)
	xd
	yd
	Zd = I_depth[h // 2, w // 2] * depth_scale
	sd = FeaturePoint()
	s = FeaturePoint()
	task = Servo()
	target_class = args.class_id
	v = ColVector(6, 0.0)
	d = get_display()
	_ = detection_model(np.array(I.numpy()[..., 2::-1]))
int	error_norm = 1e10
float	last_detection_time = -1.0
bool	first_iter = True
	start = time.time()
	results = detection_model(np.array(I.numpy()[..., 2::-1]))[0]
	boxes = results.boxes
float	max_conf = 0.0
int	idx = -1
	bb = None
	u
	x
	y
	initial_state = ColVector([x, 0, 0, y, 0, 0])
	kalman_state = kalman.getXest()
	kalman_pred = kalman.getXpred()
ColVector	error = task.getError()
	current_color = ColorBlindFriendlyPalette(ColorBlindFriendlyPalette.SkyBlue).to_vpColor()
	i
	j
	width
	height
	color
	fill
	False
	thickness

Function Documentation

◆ add_state_vectors()

ColVector yolo-centering-task-afma6.add_state_vectors	(		a,
			b )

@brief Method that permits to add two state vectors.

@param a The first state vector to which another state vector must be added.
@param b The other state vector that must be added to a.

@return ColVector The sum a + b.

Definition at line 64 of file yolo-centering-task-afma6.py.

◆ cam_from_rs_profile()

Tuple[CameraParameters, int, int] yolo-centering-task-afma6.cam_from_rs_profile ( profile )

Definition at line 117 of file yolo-centering-task-afma6.py.

◆ fx()

ColVector yolo-centering-task-afma6.fx	(	ColVector	x,
		float	dt )

@brief Process function that projects in time the internal state of the UKF.

@param x The internal state of the UKF.
@param dt The sampling time: how far in the future are we projecting x.

@return ColVector The updated internal state, projected in time, also known as the prior.

Definition at line 32 of file yolo-centering-task-afma6.py.

◆ generate_Q_matrix()

Matrix yolo-centering-task-afma6.generate_Q_matrix	(	float	dt,
		float	sigma )

@brief Method that generates the process covariance matrix for a process for which the
state vector can be written as (x, dx/dt)^T

@param dt The sampling period.

@return Matrix The corresponding process covariance matrix.

Definition at line 86 of file yolo-centering-task-afma6.py.

◆ has_class_box()

yolo-centering-task-afma6.has_class_box ( box )

Definition at line 287 of file yolo-centering-task-afma6.py.

◆ hx()

ColVector yolo-centering-task-afma6.hx ( ColVector x )

@brief Measurement function that expresses the internal state of the UKF in the measurement space.

@param x The internal state of the UKF.

@return ColVector The internal state, expressed in the measurement space.

Definition at line 51 of file yolo-centering-task-afma6.py.

◆ read_data()

None yolo-centering-task-afma6.read_data	(	ImageRGBa	I_rgba,
		ImageUInt16	I_depth,
		rs.pipeline	pipe,
		rs.align	align )

Definition at line 106 of file yolo-centering-task-afma6.py.

◆ residual_state_vectors()

ColVector yolo-centering-task-afma6.residual_state_vectors	(		a,
			b )

@brief Method that permits to subtract a state vector to another.

@param a The first state vector to which another state vector must be subtracted.
@param b The other state vector that must be subtracted to a.

@return ColVector The subtraction a - b.

Definition at line 75 of file yolo-centering-task-afma6.py.

Variable Documentation

◆ _

yolo-centering-task-afma6._ = detection_model(np.array(I.numpy()[..., 2::-1]))

protected

Definition at line 277 of file yolo-centering-task-afma6.py.

◆ align

yolo-centering-task-afma6.align = rs.align(rs.stream.color)

Definition at line 211 of file yolo-centering-task-afma6.py.

◆ args

yolo-centering-task-afma6.args = parser.parse_args()

Definition at line 185 of file yolo-centering-task-afma6.py.

◆ bb

yolo-centering-task-afma6.bb = None

Definition at line 295 of file yolo-centering-task-afma6.py.

◆ boxes

yolo-centering-task-afma6.boxes = results.boxes

Definition at line 292 of file yolo-centering-task-afma6.py.

◆ cam

yolo-centering-task-afma6.cam

Definition at line 202 of file yolo-centering-task-afma6.py.

◆ cfg

yolo-centering-task-afma6.cfg = pipe.start(config)

Definition at line 199 of file yolo-centering-task-afma6.py.

◆ color

yolo-centering-task-afma6.color

Definition at line 330 of file yolo-centering-task-afma6.py.

◆ config

yolo-centering-task-afma6.config = rs.config()

Definition at line 195 of file yolo-centering-task-afma6.py.

◆ cTw

yolo-centering-task-afma6.cTw = HomogeneousMatrix(r)

Definition at line 221 of file yolo-centering-task-afma6.py.

◆ current_color

yolo-centering-task-afma6.current_color = ColorBlindFriendlyPalette(ColorBlindFriendlyPalette.SkyBlue).to_vpColor()

Definition at line 327 of file yolo-centering-task-afma6.py.

◆ d

yolo-centering-task-afma6.d = get_display()

Definition at line 273 of file yolo-centering-task-afma6.py.

◆ depth_scale

yolo-centering-task-afma6.depth_scale = cfg.get_device().first_depth_sensor().get_depth_scale()

Definition at line 203 of file yolo-centering-task-afma6.py.

◆ detection_model

yolo-centering-task-afma6.detection_model = YOLO('yolov8n.pt')

Definition at line 187 of file yolo-centering-task-afma6.py.

◆ drawer

yolo-centering-task-afma6.drawer = UKSigmaDrawerMerwe(6, alpha=0.0001, beta=2, kappa=-3, resFunc=residual_state_vectors, addFunc=add_state_vectors)

Definition at line 232 of file yolo-centering-task-afma6.py.

◆ error

ColVector yolo-centering-task-afma6.error = task.getError()

Definition at line 322 of file yolo-centering-task-afma6.py.

◆ error_norm

ColVector yolo-centering-task-afma6.error_norm = 1e10

Definition at line 278 of file yolo-centering-task-afma6.py.

◆ False

yolo-centering-task-afma6.False

Definition at line 330 of file yolo-centering-task-afma6.py.

◆ fill

yolo-centering-task-afma6.fill

Definition at line 330 of file yolo-centering-task-afma6.py.

◆ first_iter

bool yolo-centering-task-afma6.first_iter = True

Definition at line 280 of file yolo-centering-task-afma6.py.

◆ fps

int yolo-centering-task-afma6.fps = 30

Definition at line 196 of file yolo-centering-task-afma6.py.

◆ get_images

yolo-centering-task-afma6.get_images = partial(read_data, pipe=pipe, align=align)

Definition at line 212 of file yolo-centering-task-afma6.py.

◆ h

yolo-centering-task-afma6.h

Definition at line 202 of file yolo-centering-task-afma6.py.

◆ height

yolo-centering-task-afma6.height

Definition at line 329 of file yolo-centering-task-afma6.py.

◆ help

yolo-centering-task-afma6.help

Definition at line 184 of file yolo-centering-task-afma6.py.

◆ I

yolo-centering-task-afma6.I = ImageRGBa(h, w)

Definition at line 206 of file yolo-centering-task-afma6.py.

◆ i

yolo-centering-task-afma6.i

Definition at line 329 of file yolo-centering-task-afma6.py.

◆ I_depth

yolo-centering-task-afma6.I_depth = ImageUInt16(h, w)

Definition at line 207 of file yolo-centering-task-afma6.py.

◆ Idisp

yolo-centering-task-afma6.Idisp = ImageRGBa(h, w)

Definition at line 208 of file yolo-centering-task-afma6.py.

◆ idx

yolo-centering-task-afma6.idx = -1

Definition at line 294 of file yolo-centering-task-afma6.py.

◆ initial_state

yolo-centering-task-afma6.initial_state = ColVector([x, 0, 0, y, 0, 0])

Definition at line 306 of file yolo-centering-task-afma6.py.

◆ int

yolo-centering-task-afma6.int

Definition at line 184 of file yolo-centering-task-afma6.py.

◆ j

yolo-centering-task-afma6.j

Definition at line 329 of file yolo-centering-task-afma6.py.

◆ kalman

yolo-centering-task-afma6.kalman = UnscentedKalman(generate_Q_matrix(1 / fps, sigma=1e-8), measure_covariance, drawer, fx, hx)

Definition at line 249 of file yolo-centering-task-afma6.py.

◆ kalman_pred

yolo-centering-task-afma6.kalman_pred = kalman.getXpred()

Definition at line 318 of file yolo-centering-task-afma6.py.

◆ kalman_state

yolo-centering-task-afma6.kalman_state = kalman.getXest()

Definition at line 310 of file yolo-centering-task-afma6.py.

◆ last_detection_time

yolo-centering-task-afma6.last_detection_time = -1.0

Definition at line 279 of file yolo-centering-task-afma6.py.

◆ max_conf

yolo-centering-task-afma6.max_conf = 0.0

Definition at line 293 of file yolo-centering-task-afma6.py.

◆ measure_covariance

yolo-centering-task-afma6.measure_covariance

Initial value:

=  Matrix([
    [noise_x ** 2, 0.0],
    [0.0, noise_y ** 2]
  ])

Definition at line 237 of file yolo-centering-task-afma6.py.

◆ noise_accel

int yolo-centering-task-afma6.noise_accel = 1e-12

Definition at line 236 of file yolo-centering-task-afma6.py.

◆ noise_vel

int yolo-centering-task-afma6.noise_vel = 1e-8

Definition at line 235 of file yolo-centering-task-afma6.py.

◆ noise_x

yolo-centering-task-afma6.noise_x

Definition at line 234 of file yolo-centering-task-afma6.py.

◆ noise_y

yolo-centering-task-afma6.noise_y

Definition at line 234 of file yolo-centering-task-afma6.py.

◆ parser

yolo-centering-task-afma6.parser = argparse.ArgumentParser('Centering task using a YOLO network')

Definition at line 183 of file yolo-centering-task-afma6.py.

◆ pipe

yolo-centering-task-afma6.pipe = rs.pipeline()

Definition at line 194 of file yolo-centering-task-afma6.py.

◆ pixel_noise

int yolo-centering-task-afma6.pixel_noise = 1

Definition at line 233 of file yolo-centering-task-afma6.py.

◆ plotter

yolo-centering-task-afma6.plotter = VSPlot()

Definition at line 189 of file yolo-centering-task-afma6.py.

◆ process_covariance

yolo-centering-task-afma6.process_covariance

Initial value:

=  Matrix([
    [noise_x ** 2, 0, 0, 0, 0, 0],
    [0, noise_vel, 0, 0, 0, 0],
    [0, 0, noise_accel, 0, 0, 0],
    [0, 0, 0, noise_y ** 2, 0,0],
    [0, 0, 0, 0, noise_vel,0],
    [0, 0, 0, 0, 0, noise_accel],
  ])