diff --git a/rt_gene/launch/estimate_gaze.launch b/rt_gene/launch/estimate_gaze.launch
index 29761b7..21d51ff 100644
--- a/rt_gene/launch/estimate_gaze.launch
+++ b/rt_gene/launch/estimate_gaze.launch
@@ -1,16 +1,17 @@
 <launch>
     <arg name="pytorch_num_threads" default="8" />
     <arg name="use_face_encoding_tracker" default="True" />
-    <arg name="face_encoding_threshold" default="0.6"/>
-    <arg name="start_rviz" default="False"/>
+    <arg name="face_encoding_threshold" default="0.6" />
+    <arg name="start_rviz" default="False" />
     <arg name="visualise_headpose" default="True" />
     <arg name="visualise_eyepose" default="True" />
 
-    <arg name="video_namespace" default="/kinect2/hd"/>
-    <arg name="video_image_topic" default="image_color_rect"/>
-    <arg name="video_info_topic" default="camera_info"/>
+    <arg name="video_namespace" default="/kinect2/hd" />
+    <arg name="video_image_topic" default="image_color_rect" />
+    <arg name="video_info_topic" default="camera_info" />
 
-    <arg name="ros_tf_frame" default="/kinect2_nonrotated_link"/>
+    <arg name="camera_frame" default="/kinect2_link" />
+    <arg name="ros_frame" default="/kinect2_ros_frame" />
 
     <arg name="group_name" default="gaze" />
     <arg name="tf_prefix" default="$(arg group_name)" />
@@ -26,7 +27,7 @@
           <param name="device_id_facedetection"  value="$(arg device_id_facedetection)" />
           <param name="use_face_encoding_tracker" value="$(arg use_face_encoding_tracker)" />
           <param name="face_encoding_threshold" value="$(arg face_encoding_threshold)" />
-          <param name="ros_tf_frame" value="$(arg ros_tf_frame)" />
+          <param name="camera_frame" value="$(arg camera_frame)" />
           <param name="tf_prefix" value="$(arg tf_prefix)" />
           <param name="visualise_headpose" value="$(arg visualise_headpose)" />
 
@@ -36,12 +37,13 @@
 
       <node pkg="rt_gene" type="estimate_gaze.py" name="estimate_gaze_twoeyes" output="screen">
           <rosparam param="model_files">['model_nets/Model_allsubjects1.h5']</rosparam>
-          <!-- rosparam param="model_files">['model_nets/all_subjects_mpii_prl_utmv_0_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_1_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_2_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_3_02.h5']</rosparam-->
+          <!-- rosparam param="model_files">['model_nets/all_subjects_mpii_prl_utmv_0_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_1_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_2_02.h5', 'model_nets/all_subjects_mpii_prl_utmv_3_02.h5']</rosparam -->
           <param name="tf_prefix" value="$(arg tf_prefix)" />/
-          <param name="ros_tf_frame" value="$(arg ros_tf_frame)" />
+          <param name="ros_tf_frame" value="$(arg ros_frame)" />
           <param name="device_id_gazeestimation"  value="$(arg device_id_gazeestimation)" />
           <param name="visualise_eyepose" value="$(arg visualise_eyepose)" />
-      </node>      
+      </node>
+
     </group>
 
     <node pkg="rviz" type="rviz" name="rviz_test" args="-d $(find rt_gene)/rviz_cfg/gaze_following.rviz" if="$(arg start_rviz)" />
diff --git a/rt_gene/launch/start_kinect.launch b/rt_gene/launch/start_kinect.launch
index f5d1c27..828dd50 100644
--- a/rt_gene/launch/start_kinect.launch
+++ b/rt_gene/launch/start_kinect.launch
@@ -7,6 +7,6 @@
     </include>
     <!-- publish nonrotated link for Kinect -->
     <node pkg="tf2_ros" type="static_transform_publisher" name="kinect2_link_broadcaster"
-          args="0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 /kinect2_nonrotated_link /kinect2_link" />
+          args="0.0 0.0 0.0 -0.5 0.5 -0.5 0.5 /kinect2_ros_frame /kinect2_link" />
 </launch>
 
diff --git a/rt_gene/launch/start_webcam.launch b/rt_gene/launch/start_webcam.launch
index cb99700..8b22fdc 100644
--- a/rt_gene/launch/start_webcam.launch
+++ b/rt_gene/launch/start_webcam.launch
@@ -7,7 +7,7 @@
   <arg name="video_info_topic" default="camera_info"/>
 
   <arg name="rgb_frame_id" default="/kinect2_link"/>
-  <arg name="rgb_frame_id_ros" default="/kinect2_nonrotated_link"/> <!-- must match ros_tf_frame in estimate_gaze.launch -->
+  <arg name="rgb_frame_id_ros" default="/kinect2_ros_frame"/>
   <arg name="publish_ros_static_frame" default="True"/>
 
   <node pkg="tf2_ros" type="static_transform_publisher" name="ros_static_transform_publisher"
diff --git a/rt_gene/scripts/estimate_gaze.py b/rt_gene/scripts/estimate_gaze.py
index e5981a7..71d8ac7 100755
--- a/rt_gene/scripts/estimate_gaze.py
+++ b/rt_gene/scripts/estimate_gaze.py
@@ -39,7 +39,7 @@ def __init__(self, device_id_gaze, model_files):
 
         self.tf_prefix = rospy.get_param("~tf_prefix", "gaze")
         self.headpose_frame = self.tf_prefix + "/head_pose_estimated"
-        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_nonrotated_link")
+        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_ros_frame")
 
         self.image_subscriber = rospy.Subscriber("/subjects/images", MSG_SubjectImagesList, self.image_callback, queue_size=3, buff_size=2**24)
         self.subjects_gaze_img = rospy.Publisher("/subjects/gazeimages", Image, queue_size=3)
diff --git a/rt_gene/scripts/extract_landmarks_node.py b/rt_gene/scripts/extract_landmarks_node.py
index 46dd369..cb673a4 100755
--- a/rt_gene/scripts/extract_landmarks_node.py
+++ b/rt_gene/scripts/extract_landmarks_node.py
@@ -39,7 +39,7 @@ def __init__(self, img_proc=None):
         self.bridge = CvBridge()
         self.__subject_bridge = SubjectListBridge()
 
-        self.ros_tf_frame = rospy.get_param("~ros_tf_frame", "/kinect2_nonrotated_link")
+        self.camera_frame = rospy.get_param("~camera_frame", "/kinect2_link")
 
         self.tf_broadcaster = TransformBroadcaster()
         self.tf_prefix = rospy.get_param("~tf_prefix", default="gaze")
@@ -145,10 +145,11 @@ def get_head_pose(self, landmarks, subject_id):
         dist_coeffs = self.img_proc.distortionCoeffs()
 
         try:
-            success, rotation_vector_unstable, translation_vector_unstable = cv2.solvePnP(self.model_points,
-                                                                                          landmarks.reshape(len(self.model_points), 1, 2),
-                                                                                          cameraMatrix=camera_matrix,
-                                                                                          distCoeffs=dist_coeffs, flags=cv2.SOLVEPNP_DLS)
+            success, rodrigues_rotation, translation_vector, _ = cv2.solvePnPRansac(self.model_points,
+                                                                                    landmarks.reshape(len(self.model_points), 1, 2),
+                                                                                    cameraMatrix=camera_matrix,
+                                                                                    distCoeffs=dist_coeffs, flags=cv2.SOLVEPNP_DLS)
+
         except cv2.error as e:
             print('Could not estimate head pose', e)
             return False, None, None
@@ -157,12 +158,17 @@ def get_head_pose(self, landmarks, subject_id):
             print('Could not estimate head pose')
             return False, None, None
 
-        rotation_vector, translation_vector = self.apply_kalman_filter_head_pose(subject_id, rotation_vector_unstable, translation_vector_unstable)
+        # this is generic point stabiliser, the underlying representation doesn't matter
+        rotation_vector, translation_vector = self.apply_kalman_filter_head_pose(subject_id, rodrigues_rotation, translation_vector / 1000.0)
 
-        translation_vector = np.array([translation_vector[2], -translation_vector[0], -translation_vector[1]]) / 1000.0
-        rotation_vector_swapped = [-rotation_vector[2], -rotation_vector[0] + self.head_pitch, rotation_vector[1] + np.pi]
+        _rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
+        _rotation_matrix = np.matmul(_rotation_matrix, np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))
+        _m = np.zeros((4, 4))
+        _m[:3, :3] = _rotation_matrix
+        _m[3, 3] = 1
+        _rpy_rotation = np.array(transformations.euler_from_matrix(_m)).reshape(rodrigues_rotation.shape)
 
-        return success, rotation_vector_swapped, translation_vector
+        return success, _rpy_rotation, translation_vector
 
     def apply_kalman_filter_head_pose(self, subject_id, rotation_vector_unstable, translation_vector_unstable):
         stable_pose = []
@@ -185,7 +191,7 @@ def publish_subject_list(self, timestamp, subjects):
     def publish_pose(self, timestamp, nose_center_3d_tf, head_rpy, subject_id):
         self.tf_broadcaster.sendTransform(nose_center_3d_tf, transformations.quaternion_from_euler(*head_rpy), timestamp,
                                           self.tf_prefix + "/head_pose_estimated" + str(subject_id),
-                                          self.ros_tf_frame)
+                                          self.camera_frame)
 
     def update_subject_tracker(self, color_img):
         faceboxes = self.get_face_bb(color_img)
diff --git a/rt_gene/src/rt_gene/extract_landmarks_method_base.py b/rt_gene/src/rt_gene/extract_landmarks_method_base.py
index 7a12c10..1f65ec1 100644
--- a/rt_gene/src/rt_gene/extract_landmarks_method_base.py
+++ b/rt_gene/src/rt_gene/extract_landmarks_method_base.py
@@ -66,8 +66,6 @@ def get_full_model_points(self, model_points_file=None):
                 raw_value.append(line)
         model_points = np.array(raw_value, dtype=np.float32)
         model_points = np.reshape(model_points, (3, -1)).T
-        # model_points *= 4
-        model_points[:, -1] *= -1
 
         # index the expansion of the model based.
         model_points = model_points * (self.interpupillary_distance * self.model_size_rescale)