Problem Setting An Initial Pose In ORB_SLAM2
I am running ORB SLAM 2 on a drone with a downward facing, RGBD camera. I'm using Gazebo to simulate and Rviz to see the vision_pose ROS topic. I am having an issue with the vision_pose being incorrect because the pose is initialised as if the camera is pointing straight forward towards the horizon by default instead of straight down.
If anyone is familiar with the ORB_SLAM2 code, 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 it doesn't seem like Eigen is used for this particular part. 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!
