[Python] How do I fix the issue in my constrained non-linear 6D pose optimization problem...

I am a robotics student and am quite new to the field of computer vision, especially with pose estimation. I am trying to estimate the best 6D (rotation plus translation) grasping pose on the top surface of a rectangular palette by using the camera 3D data and known geometrical relations. Here is the objective expression I am trying to implement.







Given:

1. the 4 contour lines on the image (with normalized image coordinates) corresponding to each edge of the box called as l tilde
2. the equation of the plane of the top surface of the palette as [nx, ny, nz, -d]
3. the dimensions of the palette top surface length and width

Parameters to Optimize For:

1. rotation quaternion elements [qx, qy, qz, qw] corresponding to the rotation matrix R_cb (camera to palette base)
2. translation vector elements [tx, ty, tz] for the T_cb

Objective for Minimization: The intuitive explanation is that each contour line on the image forms a plane in 3D when it is back-projected. In other words, a plane e when projected onto the image is a line on the image l. So for each adjacent contour line pairs, their back-projected planes B_e1 and B_e2 (in relation to base frame) intersect with the palette top surface plane B_ep to form a corner point of the top surface rectangle. And this corner point estimated should be at the location [length/2, width/2, 0, 1] from the base frame for example. Hence the error term which is the squared sum of each plane multiplied with the desired corner point vector [dim1, dim2, 0, 1].

**Problem: I have tried implementing this in python with the scipy.optimize.minimize with several variations of the objective function. However, the optimization does not work well. The resultant pose found always has a large error. The optimizer being used is SLSQP. I don't know why the optimizer is not working, if I have made any mistakes in the implementation, and what I should do, and hence requesting your support. The code is shared below. **

I reduce the 4 parameters of each plane to 3 by scaling the plane equation vectors [nx, ny, nz, -d] by 1/nx. So that I do not have to explicit constraints to normalize the normal vector elements (nx, ny, nz) for each plane. I have tried that approach but the optimizer does not work with all those plane normal constraints, hence using this approach right now. To my understanding, this should not affect the optimization really since the location of minima is not changed, only its value due to scaling. But I am not sure about this, and would appreciate correction if any. Is it necessary to normalize the final plane vectors?

Here is the implementation. I am using quaternion representation for rotation, since it has only 4 parameters instead of 9 for rotation matrix and the conversion between the two involves simple algebraic functions and not transcendental (like trigonometric functions) like with rotation vector. I tried with rotation vector representation but the quaternion implementation seems to yield better results as of now.

fx, fy, cx, cy = 729.7791748046875, 729.7791748046875, 965.4876098632812, 553.423828125 # not used anywhere here

opt_lines = np.load("synth_scene_data/optimized_lines_tilde.npy")
print("Optimized lines for the normalized image (l_tilde): \n", opt_lines)

l1_tilde = opt_lines[0]
l2_tilde = opt_lines[1]
l3_tilde = opt_lines[2]
l4_tilde = opt_lines[3]
lines_list = [l1_tilde, l2_tilde, l3_tilde, l4_tilde, l1_tilde] # for cyclic pairwise selection of the adjacent lines

C_ep = np.load("synth_scene_data/top_plane.npy") # top plane of the box
C_np = C_ep[:3].reshape(3, 1) # normal vector of the plane
C_dp = C_ep[3] # distance

dim_i1, dim_i2 = 0.205 / 2, 0.395 / 2 # half of box top face dimensions as length/2 x width/2
dim_i1_list = [-dim_i1, -dim_i1, dim_i1, dim_i1] # for iterative selection of correct dimensions for each corner point
dim_i2_list = [dim_i2, -dim_i2, -dim_i2, dim_i2]


# params for optimization = [qw, qx, qy, qz, tx, ty, tz]
def objective_T4(params):
t = params[4:].reshape(3, 1)
q = params[:4] # quaternion of the rotation (w, x, y, z)
q_norm = q / np.linalg.norm(q) # normalizing the quaternion
qw, qx, qy, qz = q_norm
total_error = 0.0

# quaternion to rotation matrix conversion
r11 = 1 - 2*(qy**2 + qz**2)
r12 = 2*(qx*qy - qw*qz)
r13 = 2*(qx*qz + qw*qy)
r21 = 2*(qx*qy + qw*qz)
r22 = 1 - 2*(qx**2 + qz**2)
r23 = 2*(qy*qz - qw*qx)
r31 = 2*(qx*qz - qw*qy)
r32 = 2*(qy*qz + qw*qx)
r33 = 1 - 2*(qx**2 + qy**2)
r1 = np.array([r11, r21, r31]).reshape(3, 1)
r2 = np.array([r12, r22, r32]).reshape(3, 1)
r3 = np.array([r13, r23, r33]).reshape(3, 1)

for i in range(4):
dim1 = dim_i1_list # first dimension of the box top face (half-length)
dim2 = dim_i2_list # second dimension of the box top face (half-width)

l1 = lines_list.reshape(3, 1) # first contour line
l2 = lines_list[i+1].reshape(3, 1) # second contour line

# the planes found explicitly, only for better readability, not used right now
#B_e1 = np.array([1, ((l1.T @ r2)/(l1.T @ r1)).item(), ((l1.T @ r3)/(l1.T @ r1)).item(), ((l1.T @ t)/(l1.T @ r1)).item()]).reshape(4, 1)
#B_e2 = np.array([1, ((l2.T @ r2)/(l2.T @ r1)).item(), ((l2.T @ r3)/(l2.T @ r1)).item(), ((l2.T @ t)/(l2.T @ r1)).item()]).reshape(4, 1)
#B_ep = np.array([1, ((C_np.T @ r2)/(C_np.T @ r1)).item(), ((C_np.T @ r3)/(C_np.T @ r1)).item(), ((C_np.T @ t - C_dp)/(C_np.T @ r1)).item()]).reshape(4, 1)
#plane_mat = np.vstack((B_e1.T, B_e2.T, B_ep.T))
#corner_pt_error = plane_mat @ np.array([dim1, dim2, 0, 1]).reshape(4, 1)
#error = np.sum(np.square(corner_pt_error)).item() # sum of squares of the corner point error
error = ((dim1 + dim2*(l1.T @ r2)/(l1.T @ r1) + (l1.T @ t)/(l1.T @ r1))**2 + (dim1 + dim2*(l2.T @ r2)/(l2.T @ r1) + (l2.T @ t)/(l2.T @ r1))**2 + (dim1 + dim2*(C_np.T @ r2)/(C_np.T @ r1) + (C_np.T @ t - C_dp)/(C_np.T @ r1))**2).item()
total_error += error

return total_error

# Constraints
def quat_norm_constraint(params):
"""Ensures the quaternion is unit-length"""
q = params[:4]
return (np.linalg.norm(q) - 1.0) #* 1e6 # scale for numerical stability

# Ground truth pose (currently for testing, not actual ground truth)
RotMat_GT = np.load("synth_scene_data/gt_rot_mat.npy")
T_vec_GT = np.load("synth_scene_data/gt_trans_vec.npy")


# Callback
losses = []
rot_errors = []
trans_errors = []
opt_params = []

def callback(params, RotMat_GT, T_vec_GT):
losses.append(objective_T4(params))
RotMat = quatToRotMat(params[:4])
T_vec = params[4:]
rot_errors.append(uf.angularError(RotMat, RotMat_GT))
trans_errors.append(translationalError(T_vec, T_vec_GT))
opt_params.append(params)


# Initial guess
quat_GT = rotMatToQuat(RotMat_GT) # Convert rotation matrix to quaternion
print("Ground truth parameters: ", quat_GT, T_vec_GT.flatten())
# Add randomization to the ground truth pose to create an initial guess
rot_deviation = 0.08 # Standard deviation factor for rotation randomization
trans_deviation = 0.05 # Standard deviation factor for translation randomization
rand_init_quat = quat_GT.flatten() + np.random.normal(-rot_deviation, rot_deviation, quat_GT.size)
rand_init_quat = rand_init_quat / np.linalg.norm(rand_init_quat) # Normalize the quaternion
rand_init_trans = T_vec_GT.flatten() + np.random.normal(-trans_deviation, trans_deviation, T_vec_GT.size)
x0_rand = np.concatenate((rand_init_quat, rand_init_trans)) # Randomized initial guess
print("Randomized initial guess: ", x0_rand)
fixed_init_quat = quat_GT.flatten() + np.array([0.1, 0.1, 0.1, 0.1])
fixed_init_quat = fixed_init_quat / np.linalg.norm(fixed_init_quat) # Normalize the quaternion
fixed_init_trans = T_vec_GT.flatten() + np.array([0.07, 0.07, -0.1])
x0_fix = np.concatenate((fixed_init_quat, fixed_init_trans))
print("Fixed initial guess: ", x0_fix)

# Bounds for the parameters (for translation vector only here)
bounds = [(None, None), (None, None), (None, None), (None, None), (-2, 2), (-2, 2), (-2, 2)] # translations should be within the 4 by 4 by 4 meter cube space

constraints = [
{'type': 'eq', 'fun': quat_norm_constraint}, # Valid rotation matrix constraints
]

result = minimize(
objective_T4,
x0_fix,
constraints=constraints,
bounds=bounds,
method='SLSQP',
options={'disp': True,
'ftol': 1e-10,
'eps': 1e-8,
'maxiter': 1000
},
callback=partial(callback, RotMat_GT=RotMat_GT, T_vec_GT=T_vec_GT)
)

np.save("synth_scene_data/intermed_params.npy", opt_params)
np.save("synth_scene_data/opt_params.npy", result.x)


Here are the optimization plots for objective loss, rotation loss and translation loss:

The loss curve many a times does not converge well, and depends highly on the initial guess. And even if the loss reaches to very low value like 0.012, the rotation and translation errors are still large. This does not sound good. Hence requesting help

Continue reading...