Systems and methods for VSLAM optimization

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.

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.

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.