# Using Solvepnp with rvec fixed and only optimizing tvec? **URL:** \ **Category:** Python\ **Tags:** calib3d\ **Created:** [April 5, 2023, 2:01pm UTC](https://forum.opencv.org/t/using-solvepnp-with-rvec-fixed-and-only-optimizing-tvec/12679 "2023-04-05T14:01:37Z")\ **Posts on this page:** 4\ **Page:** 1
**Author:** ![lel](https://sea2.discourse-cdn.com/flex020/user_avatar/forum.opencv.org/lel/32/6641_2.png) [@lel](https://forum.opencv.org/u/lel)\ **Post date:** [April 5, 2023, 2:01pm UTC](https://forum.opencv.org/t/using-solvepnp-with-rvec-fixed-and-only-optimizing-tvec/12679/1 "2023-04-05T14:01:37Z")
Is it possible to use Solvepnp to only optimize for the camera translation vector while having a fixed rvec ? ---
**Author:** ![Eduardo](https://avatars.discourse-cdn.com/v4/letter/e/6a8cbe/32.png) [@Eduardo](https://forum.opencv.org/u/Eduardo)\ **Post date:** [April 7, 2023, 10:53pm UTC](https://forum.opencv.org/t/using-solvepnp-with-rvec-fixed-and-only-optimizing-tvec/12679/2 "2023-04-07T22:53:08Z")
You cannot fix the rotation and only refine for the translation with `SOLVEPNP_ITERATIVE`. But you can for sure code it yourself (or modify the OpenCV code): - [16. Gauss–Newton method](https://www.seas.ucla.edu/~vandenbe/236C/lectures/gn.pdf) - [Pose Estimation for Augmented Reality: A Hands-On Survey](https://hal.inria.fr/hal-01246370/document) - since the Jacobian when optimizing for the full 3D pose is: ![image](https://us1.discourse-cdn.com/flex020/uploads/opencv/original/2X/6/63b39e1ba8ba98aff2d0bb747713c141565ddded.png) - you can also optimise for just the translation part ---
**Author:** ![Eduardo](https://avatars.discourse-cdn.com/v4/letter/e/6a8cbe/32.png) [@Eduardo](https://forum.opencv.org/u/Eduardo)\ **Post date:** [May 1, 2023, 6:02pm UTC](https://forum.opencv.org/t/using-solvepnp-with-rvec-fixed-and-only-optimizing-tvec/12679/3 "2023-05-01T18:02:19Z")
Some references: - [Pose Estimation for Augmented Reality: A Hands-On Survey, Eric Marchand, Hideaki Uchiyama, Fabien Spindler](https://inria.hal.science/hal-01246370/document) - [Visual servo control, Part I: Basic approaches, François Chaumette, S. Hutchinson](https://inria.hal.science/inria-00350283/document) - [An Invitation to 3-D Vision - From Images to Models, Yi Ma, Stefano Soatto, J. Kosecka, S. Sastry](https://www.eecis.udel.edu/~cer/arv/readings/old_mkss.pdf) - [A tutorial on SE(3) transformation parameterizations and on-manifold optimization, Jose-Luis Blanco](https://arxiv.org/abs/2103.15980) * * * As an exercise of style, I wanted to implement myself a pose refinement method, and estimating only the translation part (known rotation), or only the rotation or both. Not sure if what I did is fully correct, but at least it was a good exercise. If it can be useful to someone, I am happy. ```plaintext #! python3 # -*- coding: utf-8 -*- from enum import Enum import numpy as np from numpy.linalg import pinv from pathlib import Path import argparse import cv2 as cv class RefinementType(Enum): TRANSLATION = 1 ROTATION = 2 BOTH = 3 def project(pt3d, K): pt2d = K @ pt3d u = pt2d[0] / pt2d[2] v = pt2d[1] / pt2d[2] pt2d = np.empty((2,1)) pt2d[0] = u pt2d[1] = v return pt2d def calcJacobian(pt3d_objs, cTo_est, refinement_type): trans_idx_start = 0 if refinement_type == RefinementType.TRANSLATION or refinement_type == RefinementType.ROTATION: J_cols = 3 else: J_cols = 6 trans_idx_start = 3 J = np.empty((2*pt3d_objs.shape[0], J_cols)) for i in range(pt3d_objs.shape[0]): pt3d = pt3d_objs[i,:] pt3d_in_cam = transform(pt3d, cTo_est) Zi = pt3d_in_cam[2] xi = pt3d_in_cam[0] / Zi yi = pt3d_in_cam[1] / Zi if refinement_type == RefinementType.ROTATION or refinement_type == RefinementType.BOTH: J[2*i, 0] = xi * yi J[2*i, 1] = -(1 + xi**2) J[2*i, 2] = yi J[2*i + 1, 0] = 1 + yi**2 J[2*i + 1, 1] = -xi * yi J[2*i + 1, 2] = -xi if refinement_type == RefinementType.TRANSLATION or refinement_type == RefinementType.BOTH: J[2*i, trans_idx_start + 0] = -1 / Zi J[2*i, trans_idx_start + 1] = 0 J[2*i, trans_idx_start + 2] = xi / Zi J[2*i + 1, trans_idx_start + 0] = 0 J[2*i + 1, trans_idx_start + 1] = -1 / Zi J[2*i + 1, trans_idx_start + 2] = yi / Zi return J def computeResiduals(pt2d_imgs_true, pt2d_imgs_est): residuals = (pt2d_imgs_true.ravel() - pt2d_imgs_est.ravel()) return residuals def transform(pt3d_obj, cTo): pt3d_obj_cam = pt3d_obj @ cTo[0:3, 0:3] + cTo[0:3, 3] return pt3d_obj_cam def compute2d(pt3d_objs, cTo, K): pt3d_objs_in_cam = np.empty((4,3)) for i in range(pt3d_objs.shape[0]): pt3d = pt3d_objs[i,:] pt3d_in_cam = transform(pt3d, cTo) pt3d_objs_in_cam[i,:] = pt3d_in_cam pt2d_imgs = np.empty((4,2)) for i in range(pt3d_objs_in_cam.shape[0]): pt3d = pt3d_objs_in_cam[i,:] pt2d = project(pt3d, K) pt2d_imgs[i,:] = pt2d.ravel() return pt2d_imgs def main(): parser = argparse.ArgumentParser(description='Pose refinement testing.', formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('--niters', type=int, default=10, help='Number of iterations for the minimisation loop.') parser.add_argument('--theta-perturb', type=float, default=5.4583, help='Perturbation value for theta (rotation part) in degree.') parser.add_argument('--u-perturb', nargs='+', type=float, default=[0.03, -0.02, 0.044], help='Perturbation value for u (rotation part).') parser.add_argument('--translation-perturb', nargs='+', type=float, default=[0.2, 0.3, -0.4], help='Perturbation value for translation.') parser.add_argument('--refinement', default='translation', choices=['translation', 'rotation', 'both'], help='Type of refinement (translation, rotation, both), ex: translation will refineme only the translation part.') args = parser.parse_args() niters = args.niters print(f"niters: {niters}") theta_perturb = args.theta_perturb print(f"theta perturbation: {theta_perturb}") u_perturb = args.u_perturb print(f"u perturbation: {u_perturb}") translation_perturb = args.translation_perturb print(f"translation perturbation: {translation_perturb}") if args.refinement == 'rotation': refinement_type = RefinementType.ROTATION elif args.refinement == 'both': refinement_type = RefinementType.BOTH else: refinement_type = RefinementType.TRANSLATION print(f"refinement type: {refinement_type}") K = np.eye(3) print(f"K={K}") pt3d_objs = np.empty((4,3)) pt3d_objs[0,0] = 2.5 pt3d_objs[0,1] = 3.4 pt3d_objs[0,2] = 10 pt3d_objs[1,0] = -2.5 pt3d_objs[1,1] = 2.75 pt3d_objs[1,2] = 8.4 pt3d_objs[2,0] = 1.4 pt3d_objs[2,1] = 4.5 pt3d_objs[2,2] = 3.7 pt3d_objs[3,0] = -0.75 pt3d_objs[3,1] = 3.5 pt3d_objs[3,2] = 2.75 theta_true = 24.5 u_true = np.array([1, 2, 3]) u_norm_true = u_true / np.linalg.norm(u_true) thetau_true = np.deg2rad(theta_true) * u_norm_true cRo_true, _ = cv.Rodrigues(thetau_true) cTo_true = np.eye(4) cTo_true[0:3, 0:3] = cRo_true cTo_true[0, 3] = -1.45 cTo_true[1, 3] = 3.75 cTo_true[2, 3] = 4.12 print(f"thetau_true: {thetau_true}") print(f"cTo_true:\n{cTo_true}") cTo_est = np.eye(4) if refinement_type == RefinementType.ROTATION or refinement_type == RefinementType.BOTH: theta_est = theta_true + theta_perturb u_est = u_true + u_perturb u_norm_est = u_est / np.linalg.norm(u_est) thetau_est = np.deg2rad(theta_est) * u_norm_est cRo_est, _ = cv.Rodrigues(thetau_est) cTo_est[0:3, 0:3] = cRo_est cTo_est[0:3,3] = cTo_true[0:3,3] if refinement_type == RefinementType.TRANSLATION or refinement_type == RefinementType.BOTH: cTo_est[0:3, 0:3] = cTo_true[0:3, 0:3] cTo_est[0:3,3] = cTo_true[0:3,3] + translation_perturb print(f"cTo_est:\n{cTo_est}") pt2d_imgs_true = compute2d(pt3d_objs, cTo_true, K) # print(f"pt2d_imgs_true:\n{pt2d_imgs_true}") pt2d_imgs_est = compute2d(pt3d_objs, cTo_est, K) # print(f"pt2d_imgs_est:\n{pt2d_imgs_est}") err = computeResiduals(pt2d_imgs_true, pt2d_imgs_est) print(f"initial error: {np.linalg.norm(err)}") for nb in range(niters): print(f"\nIter: {nb}") J = calcJacobian(pt3d_objs, cTo_est, refinement_type) pt2d_imgs_est = compute2d(pt3d_objs, cTo_est, K) # print(f"pt2d_imgs_est:\n{pt2d_imgs_est}") err = computeResiduals(pt2d_imgs_true, pt2d_imgs_est) err_norm = np.linalg.norm(err) print(f"err_norm: {err_norm}") dq = -np.linalg.pinv(J) @ err print(f"dq: {dq}") if refinement_type == RefinementType.ROTATION or refinement_type == RefinementType.BOTH: rotation_vector_, _ = cv.Rodrigues(cTo_est[0:3, 0:3]) rotation_vector = rotation_vector_.T rotation_vector -= dq[:3] R, _ = cv.Rodrigues(rotation_vector) cTo_est[0:3, 0:3] = R if refinement_type == RefinementType.TRANSLATION or refinement_type == RefinementType.BOTH: cTo_est[0:3,3] += dq if refinement_type == RefinementType.TRANSLATION else dq[3:] print(f"cTo_est:\n{cTo_est}") if err_norm < 1e-4: print("err_norm < 1e-4, break") break print(f"\ncTo_true:\n{cTo_true}") print(f"cTo_est:\n{cTo_est}") if __name__ == ' __main__': main() ``` ---
**Author:** ![Eduardo](https://avatars.discourse-cdn.com/v4/letter/e/6a8cbe/32.png) [@Eduardo](https://forum.opencv.org/u/Eduardo)\ **Post date:** [May 1, 2023, 6:06pm UTC](https://forum.opencv.org/t/using-solvepnp-with-rvec-fixed-and-only-optimizing-tvec/12679/4 "2023-05-01T18:06:31Z")
On a similar topic: [Feed known translation vector to SolvePnP](https://forum.opencv.org/t/feed-known-translation-vector-to-solvepnp/9177)