FHWS/Fusion2016
Archived
3
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

initial commit before ownership transfer

Browse Source
This commit is contained in:
FrankE
2016-01-25 17:57:49 +01:00
parent 36056ad002
commit 353bba8342
37 changed files with 7639 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
code/lukas/ReadMe.txt Executable file
View File

@@ -0,0 +1,14 @@
Python Skripte:
StepDetector.py TurnDetector.py
Benötigt wird Python2.7, scipy und numpy, sowie zum plotten matplotlib
Benötigte Parameter:
1. Input-Datei
2. Output-Datei
Beispiel:
python StepDetector.py ./FH_Sensor.csv Steps.txt
python TurnDetector.py ./FH_Sensor.csv Turns.txt
Weitere optimale Parameter mit -h aufrufbar

278
code/lukas/StepDetector.py Executable file
View File

@@ -0,0 +1,278 @@
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import argrelmax
import sys
import math
import argparse
def rotate_data_fhws(data, data_t, rotation, rotation_t):
#Invert rotationmatrix
np.linalg.inv(rotation)
#Align rotation time according to data time
tmp = []
for t in data_t:
# Find indices of roation matrix that are earlier
#than the current time of the sensor value
ind = np.where(rotation_t <= t)[0]
#Use the last index
if len(ind) != 0:
tmp.append(ind[-1])
else:
tmp.append(0)
#Only use the values of the rotation matrix that are aligned with the sensor data
rotation = rotation[tmp]
# Multiply data with rotationmatrix
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def rotate_data_lukas(data, rotation):
#Invert rotationmatrix
np.linalg.inv(rotation)
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def magnitude(x, y, z):
ret = [math.sqrt(i) for i in (x**2 + y**2 + z**2)]
mean = np.mean(ret)
ret -= mean
return ret
def count_steps(time, signal, lt, ht, dead):
"""
Find steps in the accelerometer signal.
After a step was found, a "dead" period exists, where no step can be found again.
This is to avoid too many steps
Parameters
----------
time: array_like
Timestaps of accelerometer signal
Must have same length as signal
signal: array_like
Accelerometer signal of all three axis.
Must have same length as time
lt: float
Low threshold, which must be exceeded by the accelerometer signal to be counted as step
ht: float
High treshold, which must not be exceeded by the accelerometer signal to be counted as step
dead: float
After a step was detected, during the dead time no other step will be found.
Given in milliseconds
"""
time_signal = zip(time, signal)
dead_time = 0
steps = []
for i in time_signal:
if lt < i[1] < ht and i[0] > dead_time:
steps.append(i[0])
dead_time = i[0] + dead
return np.array(steps)
def write_steps_to_file(fname, steps):
f = open(fname, 'w')
print steps
for s in steps:
f.write(str(s) + "\n")
f.close()
def plot_steps(time, signal, steps):
plt.title("Step detection")
plt.xlabel("ms")
plt.ylabel("Accelerometer magnitude")
plt.plot(time, signal, label="Accelerometer")
s = []
for i,t in enumerate(time):
if t in steps:
s.append((t, signal[i]))
s = np.array(s)
plt.plot(s[:,0], s[:,1], 'ro', label = "Steps")
plt.legend(numpoints=1)
plt.show()
def read_data(fname):
time = np.loadtxt(fname,
delimiter=";",
usecols=[0],
unpack=True)
f = open(fname, 'r')
accls = []
accls_t = []
rotations = []
rotations_t = []
start = time[0]
for line in f:
line = line.split(";")
t = int(line[0]) - start
#Lin Accel
if line[1] == "2":
accls_t.append(t)
accls.append((line[2], line[3], line[4]))
#Rotation
elif line[1] == "7":
rotations_t.append(t)
rotations.append((line[2], line[3], line[4], line[5],
line[6], line[7], line[8], line[9],
line[10], line[11], line[12],line[13],
line[14], line[15], line[16], line[17]))
accls = np.array(accls, dtype=float)
accls_t = np.array(accls_t, dtype=int)
rotations = np.array(rotations, dtype=float)
rotations = [row.reshape((4,4)) for row in rotations]
rotations = np.array(rotations)
rotations_t = np.array(rotations_t, dtype=int)
return accls, accls_t, rotations, rotations_t
def main():
parser = argparse.ArgumentParser()
parser.add_argument("fname_sensor",
help = "Accelerometer file")
parser.add_argument("fname_output",
help = "Output file, where timestamps of steps will be saved")
parser.add_argument("--dead",
help = "Time span (in ms) after a detected step in which no additional step will be detected (default=600)",
type=int)
parser.add_argument("--lt",
help = "Low threshold, which must be exceeded by the accelerometer signal to be counted as step (default=1.5)",
type=float)
parser.add_argument("--ht",
help = "High treshold, which must not be exceeded by the accelerometer signal to be counted as step(default=6.5)",
type=float)
parser.add_argument("--plot",
help = "Plot step detection",
action="store_true")
parser.add_argument("--file_format",
help = "Sensor data file format [fhws|lukas] (default: fhws)",
type = str)
args = parser.parse_args()
file_format = "fhws"
if args.file_format:
file_format = args.file_format
#My own data format
if file_format == "lukas":
delimiter = ','
time_cols = [40]
accel_cols = [6,7,8]
time = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=time_cols,
skiprows=2,
unpack=True)
accelX, accelY, accelZ = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=accel_cols,
skiprows=2,
unpack=True)
rotation = np.loadtxt(args.fname_sensor,
delimiter = delimiter,
usecols=range(18,34),
skiprows=1,
unpack=True)
rotations = rotation.T
rotations = [row.reshape((4,4)) for row in rotations]
accl = np.array([accelX, accelY, accelZ]).T
world_accl = rotate_data_lukas(accl, rotations)
#FHWS file format
else:
accls, time, rotation, rotation_t = read_data(args.fname_sensor)
world_accl = rotate_data_fhws(accls, time, rotation, rotation_t)
accelX = world_accl[:,0]
accelY = world_accl[:,1]
accelZ = world_accl[:,2]
accel_mag = magnitude(accelX, accelY, accelZ)
lt = 1.5
ht = 6.5
dead = 600
if args.dead:
dead = args.dead
if args.lt:
lt = args.lt
if args.ht:
ht = args.ht
steps = count_steps(time, accel_mag, lt, ht, dead)
print("#Steps detected: ", len(steps))
write_steps_to_file(args.fname_output, steps)
if args.plot:
plot_steps(time, accel_mag, steps)
if __name__ == "__main__":
main()

51
code/lukas/StepEvaluation.h Executable file
View File

@@ -0,0 +1,51 @@
#ifndef STEPEVALUATION_H
#define STEPEVALUATION_H
#include "../particles/MyState.h"
#include "StepObservation.h"
#include <math.h>
static double mu_walk = 40;
static double sigma_walk = 15;
static double mu_stop = 0;
static double sigma_stop = 5;
class StepEvaluation {
public:
double getProbability(const MyState& state, const StepObservation* obs) const {
double distance = state.distanceWalkedCM;
double a = 1.0;
double mu_distance = 0; //cm
double sigma_distance = 10.0; //cm
if(obs->step) {
a = 1.0;
mu_distance = mu_walk;//80.0; //cm
sigma_distance = sigma_walk;//40.0; //cm
}
else {
a = 0.0;
mu_distance = mu_stop; //cm
sigma_distance = sigma_stop; //cm
}
//Mixed Gaussian model: 1st Gaussian = step, 2nd Gaussian = no step
const double p = a * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance) +
(1.0-a) * K::NormalDistribution::getProbability(mu_distance, sigma_distance, distance);
return p;
}
};
#endif // STEPEVALUATION_H

14
code/lukas/StepObservation.h Executable file
View File

@@ -0,0 +1,14 @@
#ifndef STEPOBSERVATION_H
#define STEPOBSERVATION_H
struct StepObservation {
float ts;
bool step;
StepObservation() {;}
StepObservation(const float ts) : ts(ts), step(false){;}
};
#endif // STEPOBSERVATION_H

25
code/lukas/StepReader.h Executable file
View File

@@ -0,0 +1,25 @@
#ifndef STEPREADER_H
#define STEPREADER_H
#endif // STEPREADER_H
#include "../SensorReaderStep.h"
class StepReader {
public:
static StepObservation* readStep(const SensorEntryStep& se) {
std::string tmp = se.data;
StepObservation* obs = new TurnObservation();
while(!tmp.empty()) {
std::string angle = tmp;
StepObservation t(std::stof(angle));
}
return obs;
}
};

445
code/lukas/TurnDetector.py Executable file
View File

@@ -0,0 +1,445 @@
import numpy as np
import sys
import scipy.integrate
import math
import argparse
from sklearn.decomposition import PCA
import scipy.signal as signal
def project(v1, v2):
"""
Project vector v1 on v2
Return projected vector
"""
p = [np.dot(a, g) / np.dot(g,g) for a,g in zip(v1, v2)]
p = np.array(p)
p = [p*g for p,g in zip(p, v2)]
p = np.array(p)
return p
def motion_axis(time, lin_accel, gravity, interval = 500):
"""
Returns the motion axis, which is the axis with the biggest variance
lin_accel -- Linear acceleration
gravity -- Gravity
Lin_accel and gravity should have equal length
"""
p = project(lin_accel, gravity)
#add time to vector p
p = np.array([time, p[:,0], p[:,1], p[:,2]]).T
start = 0
end = start + interval
end_time = p[:,0][-1] #last timestep
pca = PCA(n_components=1)
result = []
while start < end_time:
indices = np.where((p[:,0] >= start) & (p[:,0] < end))
Z = p[indices, 1:3][0]
Z[:,0] = signal.medfilt(Z[:,0],31)
Z[:,1] = signal.medfilt(Z[:,1],31)
pca.fit(Z)
x1 = pca.components_[0][0]
y1 = pca.components_[0][1]
result.append((end, x1, y1))
start += interval
end += interval
return np.array(result)
def angle_between(v1, v2):
l_a = np.linalg.norm(v1)
l_b = np.linalg.norm(v2)
cos_ab = np.dot(v1, v2 / (l_a * l_b))
angle = np.arccos(cos_ab) * 180/math.pi
return min([180 - angle, angle])
def rotate_data_fhws(data, data_t, rotation, rotation_t):
#Invert rotationmatrix
np.linalg.inv(rotation)
#Align rotation time according to data time
tmp = []
for t in data_t:
# Find indices of roation matrix that are earlier
#than the current time of the sensor value
ind = np.where(rotation_t <= t)[0]
#Use the last index
if len(ind) != 0:
tmp.append(ind[-1])
else:
tmp.append(0)
#Only use the values of the rotation matrix that are aligned with the sensor data
rotation = rotation[tmp]
# Multiply data with rotation matrix
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def rotate_data_lukas(data, rotation):
#Invert rotationmatrix
np.linalg.inv(rotation)
rot_data = []
for i, row in enumerate(data):
row = np.append(row, 1)
rot_data.append(np.dot(rotation[i], row))
return np.array(rot_data)
def read_data(fname):
"""
Read the data out of the file provided by FHWS sensor reader app
"""
time = np.loadtxt(fname,
delimiter=";",
usecols=[0],
unpack=True)
f = open(fname, 'r')
lin_accel = []
gyros = []
rotations = []
gravity = []
start = time[0]
time = []
gyro_tmp = [0, 0, 0]
lin_accel_tmp = [0, 0, 0]
gravity_tmp = [0, 0, 9.81]
rotations_tmp = 16*[-1]
s = 0
for line in f:
line = line.split(";")
t = int(line[0]) - start
#Gyro-Data
if line[1] == "3":
gyro_tmp[0] = line[2]
gyro_tmp[1] = line[3]
gyro_tmp[2] = line[4]
#Linear Acceleration-Data
elif line[1] == "2":
lin_accel_tmp[0] = line[2]
lin_accel_tmp[1] = line[3]
lin_accel_tmp[2] = line[4]
#Gravity data
elif line[1] == "1":
gravity_tmp[0] = line[2]
gravity_tmp[1] = line[3]
gravity_tmp[2] = line[4]
#Rotation-Data
elif line[1] == "7":
for i in range(0,16):
rotations_tmp[i] = line[i+2]
if s != t:
gyros.append(gyro_tmp[:])
lin_accel.append(lin_accel_tmp[:])
gravity.append(gravity_tmp[:])
rotations.append(rotations_tmp[:])
time.append(t)
s = t
gyros = np.array(gyros, dtype=float)
lin_accel = np.array(lin_accel, dtype=float)
gravity = np.array(gravity, dtype=float)
rotations = np.array(rotations, dtype=float)
time = np.array(time, dtype = int)
#HACK
#In the first timestamps the rotation matrix is all zero, because
#no measurements are available yet.
#Avoid this by replacing these lines with the first measured
#rotation matrix
ind = np.where(rotations[:,0] == -1)[0]
if len(ind) != 0:
index = ind[-1] + 1
rotations[ind] = rotations[index]
#Reshape matrix
rotations = [row.reshape((4,4)) for row in rotations]
rotations = np.array(rotations)
return time, gyros, lin_accel, gravity, rotations
def detect_turns(time, signal, interval):
n_intervals = int(time[-1] / interval) + 1
result = []
for i in range(n_intervals):
start = i * interval
end = start + interval
tmp = integrate(start, end, zip(time, signal)) * 180.0/math.pi
result.append((end, tmp))
return np.array(result)
def integrate(time_from, time_to, signal):
"""Integrate signal from time_from to time_to. Signal should be two dimensional.
First dimension is the timestamp, second dimension is the signal value.
dt is the interval between two recordings
"""
sum = 0
last_time = 0
#go through signal
for value in signal:
#check if time is in the given timespan
if time_from <= value[0] < time_to:
#multiply value with dt and add it to the sum = integral
# sum += value[1] * dt
sum += value[1] * ((value[0] - last_time)/1000.)
last_time = value[0]
#sum is the integral over rad/s
return sum
def write_to_file(fname, turns, motion):
f = open(fname, 'w')
for index, t in enumerate(turns):
f.write(str(t[0]) + "," + str(t[1]) + "," + str(motion[index][1]) + "\n")
f.close()
def deg_to_rad(deg):
return deg * math.pi / 180.0
def rad_to_deg(rad):
return rad * 180.0 / math.pi
def main():
parser = argparse.ArgumentParser()
parser.add_argument("fname_sensor",
help = "Gyroscope file")
parser.add_argument("fname_output",
help = "Output file, where timestamps and angle of heading will be saved")
parser.add_argument("--time",
help = "Time interval, over which gyroscope will be integrated (default=500ms)",
type=int)
parser.add_argument("--rad",
help = "Output angles in rad (default in degree)",
action = "store_true")
parser.add_argument("--file_format",
help = "Sensor data file format [fhws|lukas] (default: fhws)",
type = str)
parser.add_argument("--cosy",
help = "Coordinate system of the gyroscope data [world|device] (default: device). If given in device, the data will automatically be rotated in world coordinates.",
type = str)
args = parser.parse_args()
#Choose between file format of sensor data and coordinate system
file_format = "fhws"
cosy = "device"
if args.file_format:
file_format = args.file_format
if args.cosy:
cosy = args.cosy
#My own data format
if file_format == "lukas":
delimiter = ","
time_cols = [40]
time = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=time_cols,
skiprows = 1,
unpack=True)
if cosy == "device":
gyros_cols = [9, 10, 11]
lin_accel_cols = [6, 7, 8]
else:
gyros_cols = [34, 35,36]
lin_accel_cols = [37, 38, 39]
grav_cols = [3, 4, 5]
gyroX, gyroY, gyroZ = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=gyros_cols,
skiprows = 1,
unpack=True)
rotation = np.loadtxt(args.fname_sensor,
delimiter = delimiter,
usecols=range(18,34),
skiprows=1,
unpack=True)
lin_accel_X, lin_accel_Y, lin_accel_Z = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=lin_accel_cols,
skiprows=1,
unpack=True)
gravity_X, gravity_Y, gravity_Z = np.loadtxt(args.fname_sensor,
delimiter=delimiter,
usecols=grav_cols,
skiprows=1,
unpack=True)
rotation = rotation.T
rotation = [row.reshape((4,4)) for row in rotation]
# rotation = np.array(rotation).T
print rotation
gyro = np.array([gyroX, gyroY, gyroZ]).T
lin_accel = np.array([lin_accel_X, lin_accel_Y, lin_accel_Z]).T
gravity = np.array([gravity_X, gravity_Y, gravity_Z]).T
if cosy == "device":
world_gyro = rotate_data_lukas(gyro, rotation)
world_lin_accel = rotate_data_lukas(lin_accel, rotation)
else:
world_gyro = gyro
world_lin_accel = lin_accel
#FHWS file format
else:
time, gyro, lin_accel, gravity, rotation = read_data(args.fname_sensor)
if cosy == "device":
world_gyro = rotate_data_lukas(gyro, rotation)
world_lin_accel = rotate_data_lukas(lin_accel, rotation)
else:
print "Option 'fhws' in combination with 'world' not available"
return
gyroX = world_gyro[:,0]
gyroY = world_gyro[:,1]
gyroZ = world_gyro[:,2]
lin_accel_X = world_lin_accel[:,0]
lin_accel_Y = world_lin_accel[:,1]
lin_accel_Z = world_lin_accel[:,2]
#Parameters
#---------
time_interval = 500
if args.time:
time_interval = args.time
turns = detect_turns(time, gyroZ, time_interval)
motion = motion_axis(time, lin_accel, gravity, 500)
angles = []
for index, axis in enumerate(motion):
if index == 0:
angle = 0
else:
x_1 = motion[index-1][1]
y_1 = motion[index-1][2]
x_2 = axis[1]
y_2 = axis[2]
a = np.array([x_1, y_1])
b = np.array([x_2, y_2])
angle = angle_between(a,b)
angles.append((axis[0], angle))
np.set_printoptions(suppress=True)
turns = np.array(turns)
angles = np.array(angles)
if args.rad:
turns[:,1] = deg_to_rad(turns[:,1])
print "Sum of all angles: ", np.sum(turns[:,1])
write_to_file(args.fname_output, turns, angles)
if __name__ == "__main__":
main()

76
code/lukas/TurnEvaluation.h Executable file
View File

@@ -0,0 +1,76 @@
#ifndef TURNEVALUATION_H
#define TURNEVALUATION_H
#include "../particles/MyState.h"
#include "TurnObservation.h"
#include <boost/math/special_functions/bessel.hpp>
#include <math.h>
static double sigma_heading = 35;
class TurnEvaluation {
//All calculations use degree not rad!!!
public:
double getProbability(const MyState& state, const TurnObservation* obs, bool simple = false) const {
//Particle's heading change
double delta_heading_particle = state.heading - state.heading_old;
//Correct offset of the heading change
if (delta_heading_particle < -180) {
delta_heading_particle += 360;
}
else if (delta_heading_particle > 180) {
delta_heading_particle -= 360;
}
//Switch between simple and improved evaluation
//"Simple" only evaluates the deviation between the measured heading and the particle heading change using
//normal distribution
if(simple) {
double sigma_delta_heading = sigma_heading;
const double p = K::NormalDistribution::getProbability(obs->delta_heading, sigma_delta_heading, delta_heading_particle);
return p;
}
//use the von Mises distribution
else {
//Here some calculations must be done in rad
double delta_heading_obs_rad = obs->delta_heading * 3.14159265359 / 180.0;
double delta_motion_rad = obs -> delta_motion * 3.14159265359 / 180.0;
//Equation for estimating kappa value of von Mises distribution
//empirically estimated
double kappa = 0.0;
kappa = 5.0 / exp(2 * delta_motion_rad);
double delta_heading_particle_rad = delta_heading_particle * 3.14159265359 / 180.0;
//pdf von mises distribution (http://en.wikipedia.org/wiki/Von_Mises_distribution)
const double p = exp(kappa * cos(delta_heading_obs_rad - delta_heading_particle_rad)) / (2.0 * 3.14159265359 * boost::math::cyl_bessel_i(0, kappa));
return p;
}
}
};
#endif // TURNEVALUATION_H

21
code/lukas/TurnObservation.h Executable file
View File

@@ -0,0 +1,21 @@
#ifndef TURNOBSERVATION_H
#define TURNOBSERVATION_H
#include <vector>
struct TurnObservation {
float ts;
float delta_heading; //measured change of heading direction (given by Gyroskop)
float delta_motion; //measured change of motion direction (given by PCA)
TurnObservation() {;}
TurnObservation(const float delta_heading, const float motion_angle) : delta_heading(delta_heading), delta_motion(delta_motion) {;}
};
#endif // TURNOBSERVATION_H

36
code/lukas/TurnReader.h Executable file
View File

@@ -0,0 +1,36 @@
#ifndef TURNREADER_H
#define TURNREADER_H
#include "../SensorReaderTurn.h"
#include "TurnObservation.h"
class TurnReader {
public:
static TurnObservation* readTurn(const SensorEntryTurn& se) {
std::string tmp = se.data;
TurnObservation* obs = new TurnObservation();
while(!tmp.empty()) {
int pos = tmp.find(',');
std::string heading = tmp.substr(0, pos);
tmp = tmp.substr(pos);
assert(tmp[0] == ';'); tmp = tmp.substr(1);
std::string motion = tmp;
TurnObservation t(std::stof(heading), std::stof(motion));
}
return obs;
}
};
#endif // TURNREADER_H

21
code/lukas/detection.sh Executable file
View File

@@ -0,0 +1,21 @@
#!/bin/bash
FILES=$(find ../measurements/18/{Galaxy,Nexus}/ -name "*.csv")
for f in $FILES
do
echo $f
filename=$(basename $f)
directory=$(dirname $f)
#echo $filename
#echo $directory
step=$directory/Steps2.txt
turn=$directory/Turns.txt
echo $step
echo $turn
python StepDetector.py $f $step --lt -1.2 --ht 1.2
python TurnDetector.py $f $turn
done