From 86bc6d799a2e334def0b32643569544a572765cf Mon Sep 17 00:00:00 2001
From: "bruno.tissei" <bruno.tissei@arculus.de>
Date: Tue, 7 Aug 2018 12:16:17 +0200
Subject: [PATCH] test
Signed-off-by: bruno.tissei <bruno.tissei@arculus.de>
---
README | 41 ++++++++++++++++++++++++++++++++++++++
graph/dijkstra.cpp | 8 ++++----
graph/kruskal.cpp | 4 ++--
graph/topological_sort.cpp | 4 ++--
structure/segment_tree.cpp | 20 ++++++-------------
5 files changed, 55 insertions(+), 22 deletions(-)
create mode 100644 README
diff --git a/README b/README
new file mode 100644
index 0000000..769ffa2
--- /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 fcaee37..afb03e5 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 ea6be8b..7cc272a 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 9f77df1..d5b6739 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 dc0d56e..05d22eb 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);
}
--
GitLab