Cost of Paths

[carlosjoserg]

Two ideas to compute a cumulative value to determine the cost of path from the root chart until the query point chart:

1. Sum of variances of chart centers
2. Max or average curvature along the path, that is, at chart centers

[carlosjoserg]

Another idea that come from this:

.3. Compute the geodesic using the root chart and the direction pointing at the respective initial branches, and measure the deviation of each path from the RRT w.r.t. to its geodesic