A method and device determines an optimization solution for an optimization problem. The method includes receiving the optimization problem having cost functions and variables where the cost functions have a relationship with the variables and receiving a landmark indicating a point that an agent is to visit while moving, a cost being associated with ignoring the landmark. The method includes generating a first message for the cost functions for the corresponding variable based upon the relationship and a second message for each of the variables for the corresponding cost function based upon the relationship. The method includes generating a disagreement variable for each corresponding pair of variables and cost functions measuring a disagreement value between the first and second beliefs. The method includes repeating steps (c), (d), and (e) until a consensus is formed between the first and second messages until the optimization solution is determined based upon the consensus.