getting viewing ray vector out of the pose

[doryashar]

hi, for a project i'm doing in school i'm using ORB SLAM2. i have a robot that is driving and mapping. the robot needs to drive from it's location to another location with known coordinates, now, i have a destination location and self location so i can represent the vector of driving, but i need to know how much i should turn, that is the angle between the robot pose/sight and the driving vector. i have the current pose from the tracker. how can i find the viewing ray vector / angle i need to turn? thank you very much for helping

[hashten]

Hello @doryashar Take the difference between your goal position and your current position. Now you take this vector and your vector of driving and calculate the angle you need to turn by taking the dot product of the normalized vectors: Taking arccos of the left-hand expression will give you the angle you need to turn.

[doryashar]

My question is how to get the vector of driving?

On Dec 30, 2016 3:07 PM, "Joel Harsten" notifications@github.com wrote:

Hello @doryashar https://github.com/doryashar You can take the vector in which your goal is by taking the difference between your goal position and your current position. Now you have this vector and your vector of driving, calculate the angle you need to turn by taking the dot product of the normalized vectors: [image: image] https://cloud.githubusercontent.com/assets/11462475/21565506/3b0fdf1c-ce99-11e6-8350-1f1a29a3214d.png Taking arccos of the left-hand expression will give you the angle you need to turn.

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/raulmur/ORB_SLAM2/issues/224#issuecomment-269769737, or mute the thread https://github.com/notifications/unsubscribe-auth/ANijCqqvOB_Ybdncam3JQmg5vPcyQzrjks5rNQIsgaJpZM4LXOm4 .

[hashten]

Good morning @doryashar ! Then I misunderstood your question. So given that you have your pose T_cw specified with a SE(3) transform and your robots forward direction is in its frame's x-direction you could calculate the angle of turning in much the same way as I said in my previous answer. Use the vector towards your target location "vector of driving" and the unit-vector in the direction where the robot is looking, and calculate the angle as in my previous answer. I scrabbled down briefly to illustrate: (lowercase t_cw is the translational part of the T_cw matrix)

[doryashar]

First of all, thanks a lot. as I understand, I just need the first row of the Rotation Matrix (which is T_cw[1:3,1]), right? this is assuming that main forward direction is in frame's x direction as you mentioned, is it always true ?

On Mon, Jan 2, 2017 at 12:10 PM, Joel Harsten notifications@github.com wrote:

Good morning @doryashar https://github.com/doryashar ! Then I misunderstood your question. So given that you have your pose T_cw specified with a SE(3) transform and your robots forward direction is in its frame's x-direction you could calculate the angle of turning in much the same way as I said in my previous answer. Use the vector towards your target location "vector of driving" and the unit-vector in the direction where the robot is looking, and calculate the angle as in my previous answer. I scrabbled down briefly to illustrate: [image: image] https://cloud.githubusercontent.com/assets/11462475/21587353/9fbcffaa-d0db-11e6-89f5-c48dc1a58d7f.png

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/raulmur/ORB_SLAM2/issues/224#issuecomment-269953102, or mute the thread https://github.com/notifications/unsubscribe-auth/ANijCvfxQSwSu6_G1YNam5SsuTTrWJK2ks5rOM0ngaJpZM4LXOm4 .

[hashten]

You need the whole rotation matrix. It is common to have the camera pointing in the z-direction so the image x- and y-axis is the same as the cameras. This seems to be the case in ORB-SLAM as well!

[AlejandroSilvestri]

That's right. Camera points in Z direction, X to the right, and Y points DOWN!