class Tracking(object):
def __init__(self, params):
# Set the variables:
# Minimize the reprojection error through Bundle Adjustment
# Set minimum number of measured points
# Set maximum number of loop interations for map correction
self.optimizer = BundleAdjustment()
self.min_measurements = params.pnp_min_measurements
self.max_iterations = params.pnp_max_iterations
# STEP - REFINE POSE
def refine_pose(self, pose, cam, measurements):
# Check if the minimum number of measured points is reached
assert len(measurements) >= self.min_measurements, (
'Not enough points')
# Clear Bundle Adjustment (reprojection error) and add the currently measured position of the robot
self.optimizer.clear()
self.optimizer.add_pose(0, pose, cam, fixed=False)
for i, m in enumerate(measurements):
self.optimizer.add_point(i, m.mappoint.position, fixed=True)
self.optimizer.add_edge(0, i, 0, m)
self.optimizer.optimize(self.max_iterations)
return self.optimizer.get_pose(0)
class Tracking(object):
def __init__(self, params):
self.optimizer = BundleAdjustment()
self.min_measurements = params.pnp_min_measurements
self.max_iterations = params.pnp_max_iterations
def refine_pose(self, pose, cam, measurements):
assert len(measurements) >= self.min_measurements, (
'Not enough points')
self.optimizer.clear()
self.optimizer.add_pose(0, pose, cam, fixed=False)
for i, m in enumerate(measurements):
self.optimizer.add_point(i, m.mappoint.position, fixed=True)
self.optimizer.add_edge(0, i, 0, m)
self.optimizer.optimize(self.max_iterations)
return self.optimizer.get_pose(0)