Improved 3-D navigation

[davecrawley]

No description provided.

[davecrawley]

It seems that we need to do an improved 3-D navigation that utilizes a few assumptions.
a) That the floor is mostly flat and horizontal
b) That the robot is on the floor
(1&2 should be switchable assumptions)
c) That the distance between the corners of the fiducials is known
d) That where 3 or more fiducials appear to be on the same plane within the measurement accuracy they are likely on a wall or ceiling that is flat
e) That flow vectors extracted from near a fiducial the angular motion of which is similar to the surrounding fiducials is coming from features on a wall or ceiling that is the same distance away as the fidicual and that this remains true until you see a discontinuity in the angular motion of nearby flow vectors
f) That the robot can-not move outside the realm of realistic physics
g) That the odometery is reasonably accurate
h) That the camera motion blur is dependent on the speed of the robot.
i) That IMU input is accurate

Fiducial position
1-> Camera input
2-> undistort
3-> Angular positions of the corners of the fiducials + anguar movement of motion vectors
4-> True angular positions based on motion deblur + true angular movement of motion vectors + most likely robot location

5-> Likely distance from robot of each corner based on
-relative angular distance of other corners on the same marker
-probability of being on a flat ceiling or horizontal wall of known distance from the robot

6-> Likely distance from visual odometery based on
- Probability of being on a flat ceiling or horizontal wall of known distance from the robot

7- Estimate position of robot
- Wheel odometery since last step
- Dynamic model of robot
- Preliminary visual odometery since last step
- Position relative to known fiducials based on
-Current estimated positions of all fiducials including computation based on probability of being on a likely flat floor

8 Estimate of position of fiducial corners based on
-solve of multiple observations of relative angular distance of other corners of other markers visible in the frame that a 3-D fix has already been possible for
-distance to fiducials as previously estimated, angular position as previously estimated and estimated robot position
-previously estimated position of corners in database

9 iterate 5,6, 7 and 8

Write improved positions to database