Okay.
Before diving into the actual motion planning, let's address the elephant in the room.
We don't have the robot orientation, so how are we going to get it?
Let's ask the board itself that.
What is it looking at?
So addressing the query, we gather what we already know about the robot.
We have its location at each iteration.
But how do we get its orientation?
We found the robot location using background subtraction.
We identified the foreign object and detected its location.
But for robot orientation, we require some extra information about the robot structure so that we could
identify what is facing at any moment.
Asking the original question to the robot, we can get some related information out of it that what
is its orientation in the simulation?
But we require its orientation in the image because of consistency of frame of reference as the location
extracted earlier was in the image.
So this means we need to find some relationship between the orientation and simulation with the car
orientation image.
If we do that, we could use this relationship to compute the car orientation image from the car orientation
in simulation.
Let's look at the algorithm that does exactly that.
You start off by extracting the orientation and simulation.
Then we compute the orientation image by moving the car forward for a few frames and then computing
the angle using trigonometry.
Then we find offset between the patient's simulation and the orientation image.
Finally, we use this compute offset to transform the orientation simulation to the orientation in image.
Once we do that, we now have the complete robot pose both its location and orientation.
This means that we could move forward to define the go to function.