diff --git a/README b/README
new file mode 100644
index 0000000000000000000000000000000000000000..769ffa252905e358f905b39f7cbdcaaece8d9c8c
--- /dev/null
+++ b/README
@@ -0,0 +1,41 @@
+# Algorithm for Correction of Marker Rotation and Displacement Errors
+
+The algorithm is described and explained at report.pdf
+
+# Get mid Points (First Step)
+
+Gets last PE point before each marker detection.
+
+--md and --pe are the logs from marker detection and pose estimator from the path that needs to be optimized, this path needs the global pose update by markers turned on.
+
+```
+$ python script/get_mid_points.py --marker=data/logs/20180516-134720/marker/pose.csv --pe=data/logs/20180516-134720/pe/pose.csv > mid_data
+```
+
+# Parse Marker Error (Second Step)
+
+Parse log from path without global position update by marker detection
+
+```
+$ python script/parse_marker_error.py --map=data/gvz_ext.yml --md=data/logs/20180516-133414/marker/pose.csv > correction_data
+```
+
+If no such path was captured, then the --md option should receive the same marker log that was used in the first step, in this case the "True values" in the algorithm's plot shall be ignored.
+
+
+# Run Algorithm
+
+Will plot the result and also create a file "fixed_gvz.yml", which is the gvz yml with the new values for the theta
+
+```
+$ python script/algorithm.py --inter=mid_data --correction=correction_data
+```
+
+
+# Plotter
+
+Plots a path. The --show option specifies which logs need to be included in the plot (m=marker, p=pose, c=ctrl, i=inter). The inter log can be obtained from the First Step.
+
+```
+$ python script/plotter.py --map=data/gvz_ext.yml --log=data/logs/20180516-134720 --show=mpci
+```
diff --git a/graph/dijkstra.cpp b/graph/dijkstra.cpp
index fcaee37ceb60d0af505b012426617e4439708188..afb03e574387ccd0ac0b859befe0c0c9185423c6 100644
--- a/graph/dijkstra.cpp
+++ b/graph/dijkstra.cpp
@@ -13,10 +13,10 @@ int dijkstra(int o, int d) {
set<ii> pq;
int u, v, wt;
- memset(dist, inf, sizeof dist);
+ mset(dist, inf);
dist[o] = 0;
- pq.insert(make_pair(0, o));
+ pq.insert(ii(0, o));
while (pq.size() != 0) {
u = pq.begin()->second;
@@ -28,10 +28,10 @@ int dijkstra(int o, int d) {
if (dist[v] > dist[u] + wt) {
if (dist[v] != oo)
- pq.erase(pq.find(make_pair(dist[v], v)));
+ pq.erase(pq.find(ii(dist[v], v)));
dist[v] = dist[u] + wt;
- pq.insert(make_pair(dist[v], v));
+ pq.insert(ii(dist[v], v));
}
}
}
diff --git a/graph/kruskal.cpp b/graph/kruskal.cpp
index ea6be8b17d50a8783f45b942129acdf782a93af7..7cc272a79ca2001b3848dc53593f4dbb700d8887 100644
--- a/graph/kruskal.cpp
+++ b/graph/kruskal.cpp
@@ -9,7 +9,7 @@
* OBS: * = return maximum spanning tree
*/
-vector<iii> mst;
+vector<iii> mst; // Result
vector<iii> edges;
bool cmp(iii a, iii b) {
@@ -20,7 +20,7 @@ bool cmp(iii a, iii b) {
// Return value of MST and build mst vector with edges that belong to the tree
int kruskal() {
- sort(edges.begin(), edges.end(), cmp);
+ sort(all(edges), cmp);
int size = 0;
for (int i = 0; i < MAX; i++)
diff --git a/graph/topological_sort.cpp b/graph/topological_sort.cpp
index 9f77df1874891bcbeab8f9895e963ef354fddf38..d5b673967e2898d76a5ed9b2694a1b49decc6fd2 100644
--- a/graph/topological_sort.cpp
+++ b/graph/topological_sort.cpp
@@ -31,7 +31,7 @@ bool dfs(int x) {
// Returns if graph contains cycle or not, and fills tsort vector with
// topological sort of the graph
bool topological_sort(int n, vector<int> &tsort) {
- memset(cont, 0, sizeof cont);
+ mset(cont, 0);
bool cycle = false;
for (int i = 0; i < n; ++i)
@@ -42,7 +42,7 @@ bool topological_sort(int n, vector<int> &tsort) {
return true;
while (!S.empty()) {
- tsort.push_back(S.top());
+ tsort.pb(S.top());
S.pop();
}
diff --git a/structure/segment_tree.cpp b/structure/segment_tree.cpp
index dc0d56e15305ee420171d5fbab3c15442ffd0268..05d22eb05c986810f3cd82141ef9d518efee5267 100644
--- a/structure/segment_tree.cpp
+++ b/structure/segment_tree.cpp
@@ -12,8 +12,7 @@ int N, tree[4 * MAX], v[MAX];
// Build tree with elements from v
void build_tree(int node = 1, int a = 0, int b = N) {
- if (a > b)
- return;
+ if (a > b) return;
if (a == b) {
tree[node] = v[a];
@@ -22,15 +21,13 @@ void build_tree(int node = 1, int a = 0, int b = N) {
build_tree(node * 2, a, (a + b) / 2);
build_tree(node * 2 + 1, 1 + (a + b) / 2, b);
-
tree[node] = tree[node * 2] + tree[node * 2 + 1];
}
// Update position idx with value val
void update_tree(int idx, int val, int node = 1, int a = 0, int b = N) {
- if (a > b || a > idx || b < idx)
- return;
+ if (a > b || a > idx || b < idx) return;
if (a == b) {
tree[node] = val;
@@ -39,21 +36,16 @@ void update_tree(int idx, int val, int node = 1, int a = 0, int b = N) {
update_tree(idx, val, node * 2, a, (a + b) / 2);
update_tree(idx, val, node * 2 + 1, 1 + (a + b) / 2, b);
-
tree[node] = tree[node * 2] + tree[node * 2 + 1];
}
// Return sum of [i,j]
int query_tree(int i, int j, int node = 1, int a = 0, int b = N) {
- if (a > b || a > j || b < i)
- return 0;
-
- if (a >= i && b <= j)
- return tree[node];
+ if (a > b || a > j || b < i) return 0;
- int res1 = query_tree(i, j, node * 2, a, (a + b) / 2);
- int res2 = query_tree(i, j, node * 2 + 1, 1 + (a + b) / 2, b);
+ if (a >= i && b <= j) return tree[node];
- return res1 + res2;
+ return query_tree(i, j, node * 2, a, (a + b) / 2) + \
+ query_tree(i, j, node * 2 + 1, 1 + (a + b) / 2, b);
}