def solve_kpam(problem): # type: (OptimizationProblemkPAM) -> bool
result = problem.mp.Solve()
if not result is not SolutionResult.kSolutionFound:
problem.has_solution = False
return False
# Save the result to problem
problem.xyzrpy_sol = problem.mp.GetSolution(problem.xyzrpy)
T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy=problem.xyzrpy_sol)
problem.T_action = np.dot(problem.T_init, T_eps)
problem.has_solution = True
# OK
return True
def point2plane_cost(problem, from_keypoints, goal_keypoints, weight,
plane_normal, xyzrpy):
cost = 0.0
T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy)
T = np.dot(problem.T_init, T_eps)
# The iteration on keypoints
num_keypoints = from_keypoints.shape[0]
for i in range(num_keypoints):
q = SE3_utils.transform_point(T, from_keypoints[i, :])
q_goal = goal_keypoints[i, :]
delta = q - q_goal
delta_normal = np.dot(delta, plane_normal)
cost += np.dot(delta_normal, delta_normal)
return weight * cost
def keypoint_l2_cost(problem, from_keypoints, goal_keypoints, weight, xyzrpy):
# type: (OptimizationProblemkPAM, np.ndarray, np.ndarray, float, np.ndarray) -> np.ndarray
"""
:param from_keypoints: (N, 3) np.ndarray with float
:param goal_keypoints: (N, 3) np.ndarray with float
:param weight: (N, 3) np.ndarray with float
:param xyzrpy: np.ndarray with potentially symbolic variable
:return: The cost value
"""
cost = 0.0
T_eps = SE3_utils.xyzrpy_to_matrix(xyzrpy)
T = np.dot(problem.T_init, T_eps)
# The iteration on keypoints
num_keypoints = from_keypoints.shape[0]
for i in range(num_keypoints):
q = SE3_utils.transform_point(T, from_keypoints[i, :])
q_goal = goal_keypoints[i, :]
delta = q - q_goal
cost += np.dot(delta, delta)
return weight * cost