I am running ORB SLAM 2 on a drone with a downward facing, RGBD camera. I am having an issue with the vision_pose being incorrect because the pose is initialised as if the camera is pointing towards the horizon by default instead of straight down.
To try and initialise the pose to suit my initial conditions, in the Tracking::StereoInitialization() function, I have replaced the line:
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
With:
mCurrentFrame.SetPose(cv::Mat(4,4,CV_32F,quat_rot)); where quat_rot is a quaternion that represents a 90 degree pitch down on the Z axis.
I used an online calculator to find that the quaternion for the pitch down would be: (0.7071, 0, 0, -0.7071) or more simply, (0.7071 - 0.7071k).
I declared the quat_rot quaternion with those numbers:
float quat_rot[16] ={0.7071, 0, 0, -0.7071,0, 0.7071, -0.7071, 0,0, -0.7071, 0.7071, 0,-0.7071, 0, 0, 0.7071};
But when using Rviz to check the pose, it is only rotated down at 45 degrees. I have tried swapping the position of the scalar number in the quaternion after reading about the differences in how Eigen processes quaternions but Eigen isn't used in the declaration. I have also tried with some different numbers but none give a rotation past 45 degrees without throwing errors.
I'm out of ideas and would greatly appreciate any input, perhaps the basis of the question is wrong and I should be doing this elsewhere in the code, I don't know, maybe someone can point me in the right direction.
Hi,
I am running ORB SLAM 2 on a drone with a downward facing, RGBD camera. I am having an issue with the vision_pose being incorrect because the pose is initialised as if the camera is pointing towards the horizon by default instead of straight down.
To try and initialise the pose to suit my initial conditions, in the Tracking::StereoInitialization() function, I have replaced the line:
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));With:mCurrentFrame.SetPose(cv::Mat(4,4,CV_32F,quat_rot));wherequat_rotis a quaternion that represents a 90 degree pitch down on the Z axis.I used an online calculator to find that the quaternion for the pitch down would be: (0.7071, 0, 0, -0.7071) or more simply, (0.7071 - 0.7071k).
I declared the
quat_rotquaternion with those numbers:float quat_rot[16] ={0.7071, 0, 0, -0.7071,0, 0.7071, -0.7071, 0,0, -0.7071, 0.7071, 0,-0.7071, 0, 0, 0.7071};But when using Rviz to check the pose, it is only rotated down at 45 degrees. I have tried swapping the position of the scalar number in the quaternion after reading about the differences in how Eigen processes quaternions but Eigen isn't used in the declaration. I have also tried with some different numbers but none give a rotation past 45 degrees without throwing errors.
I'm out of ideas and would greatly appreciate any input, perhaps the basis of the question is wrong and I should be doing this elsewhere in the code, I don't know, maybe someone can point me in the right direction.
Thanks!