Systems and methods for VSLAM optimization
First Claim
1. A method for localization and mapping in a system comprising a processor and a camera, wherein the processor is configured to generate a graph with a plurality of pose nodes, a plurality of landmark nodes, and a plurality of edges, wherein:
- (i) a pose node comprises a pose of a robot;
(ii) a landmark node comprises a pose of the robot, a landmark identifier corresponding to one or more objects, and an estimate of the location of each of the one or more objects; and
(iii) at least one of the plurality of edges comprises a rigid transformation relating position and orientation of the robot at two locations;
the method comprising;
updating the graph if the number of pose nodes in the graph exceeds a first threshold, comprising;
i) identifying a pose node directly linked to associated Markov blanket nodes with two or more incident edges;
ii) composing said incident edges to generate one or more new edges between pairs of said associated Markov blanket nodes; and
iii) removing the identified pose node and said two or more incident edges;
removing at least one edge of said plurality of edges present in the graph if the total number of edges in the graph exceeds a second threshold; and
updating an estimate of a location of the remaining pose nodes based at least in part on the plurality of edges present in the graph.
6 Assignments
0 Petitions
Accused Products
Abstract
The invention is related to methods and apparatus that use a visual sensor and dead reckoning sensors to process Simultaneous Localization and Mapping (SLAM). These techniques can be used in robot navigation. Advantageously, such visual techniques can be used to autonomously generate and update a map. Unlike with laser rangefinders, the visual techniques are economically practical in a wide range of applications and can be used in relatively dynamic environments, such as environments in which people move. Certain embodiments contemplate improvements to the front-end processing in a SLAM-based system. Particularly, certain of these embodiments contemplate a novel landmark matching process. Certain of these embodiments also contemplate a novel landmark creation process. Certain embodiments contemplate improvements to the back-end processing in a SLAM-based system. Particularly, certain of these embodiments contemplate algorithms for modifying the SLAM graph in real-time to achieve a more efficient structure.
135 Citations
18 Claims
-
1. A method for localization and mapping in a system comprising a processor and a camera, wherein the processor is configured to generate a graph with a plurality of pose nodes, a plurality of landmark nodes, and a plurality of edges, wherein:
-
(i) a pose node comprises a pose of a robot; (ii) a landmark node comprises a pose of the robot, a landmark identifier corresponding to one or more objects, and an estimate of the location of each of the one or more objects; and (iii) at least one of the plurality of edges comprises a rigid transformation relating position and orientation of the robot at two locations; the method comprising; updating the graph if the number of pose nodes in the graph exceeds a first threshold, comprising; i) identifying a pose node directly linked to associated Markov blanket nodes with two or more incident edges; ii) composing said incident edges to generate one or more new edges between pairs of said associated Markov blanket nodes; and iii) removing the identified pose node and said two or more incident edges; removing at least one edge of said plurality of edges present in the graph if the total number of edges in the graph exceeds a second threshold; and updating an estimate of a location of the remaining pose nodes based at least in part on the plurality of edges present in the graph. - View Dependent Claims (2, 3, 4, 5, 6, 7, 8)
-
-
9. A mobile electronic device comprising:
-
a camera configured to capture an image; at least one processor for executing sets of instructions; memory containing a navigation application; wherein execution of the navigation application by at least one processor directs the processor to; maintain a graph comprising a plurality of pose nodes, a plurality of landmark nodes, and edges, wherein; (i) a pose node comprises a pose of a robot; (ii) a landmark node comprises a pose of the robot, a landmark identifier corresponding to one or more objects, and an estimate of the location of each of the one or more objects; and (iii) an edge comprises a rigid transformation relating position and orientation of the robot at two locations; update the graph if the number of pose nodes in the graph exceeds a first threshold, comprising; i) identifying a pose node directly linked to associated Markov blanket nodes with two or more incident edges; ii) composing said incident edges to generate one or more new edges between pairs of said associated Markov blanket nodes; and iii) removing the identified pose node and said two or more incident edges; remove at least one edge of said plurality of edges present in the graph if the total number of edges in the graph exceeds a second threshold; and update an estimate of a location of each of the remaining pose nodes based at least in part on the plurality of edges present in the graph. - View Dependent Claims (10, 11, 12, 13, 14, 15, 16)
-
-
17. A method for localization and mapping in a system comprising a processor and a camera, wherein the processor is configured to generate a graph with a plurality of pose nodes and a plurality of edges, the method comprising:
-
updating the graph if the number of pose nodes in the graph exceeds a first threshold, comprising; i) identifying a pose node directly linked to associated Markov blanket nodes with two or more incident edges; ii) composing said incident edges to generate one or more new edges between pairs of said associated Markov blanket nodes; and iii) removing the identified pose node and said two or more incident edges; removing at least one edge of said plurality of edges present in the graph if the total number of edges in the graph exceeds a second threshold, comprising; (i) generating a residual value for each edge in the graph, the residual value being based at least in part on the difference between the relative pose of the nodes connected by the edge in the graph and the relative pose given by the transformation value associated with the same edge; (ii) identifying the edge with lowest residual value; and (iii) removing the identified edge from the graph; updating an estimate of a location of the remaining pose nodes based at least in part on the plurality of edges present in the graph.
-
-
18. A mobile electronic device comprising:
-
a camera configured to capture an image; at least one processor for executing sets of instructions; memory containing a navigation application; wherein execution of the navigation application by at least one processor directs the processor to; maintain a graph comprising a plurality of pose nodes and edges; update the graph if the number of pose nodes in the graph exceeds a first threshold, comprising; i) identifying a pose node directly linked to associated Markov blanket nodes with two or more incident edges; ii) composing said incident edges to generate one or more new edges between pairs of said associated Markov blanket nodes; and iii) removing the identified pose node and said two or more incident edges; remove at least one edge of said plurality of edges present in the graph if the total number of edges in the graph exceeds a second threshold, comprising; (i) generating a residual value for each edge in the graph, the residual value being based at least in part on the difference between the relative pose of the nodes connected by the edge in the updated graph and the relative pose given by the mean of the same edge; (ii) identifying the edge with lowest residual value; and (iii) removing the identified edge from the graph; update an estimate of a location of each of the remaining pose nodes based at least in part on the plurality of edges present in the graph.
-
Specification