postprocess.h

Go to the documentation of this file.

#ifndef _POSTPROC_H
#define _POSTPROC_H
/*
#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <vector>
*/
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>

#define PI 3.14159

#include "lowerleg_struct.h"
#include "ekf.h"
#include "matrix_tools.h"
using namespace std; 

// Functions for loading measurement data

int load_collar_calibs(char *foldername, vector< vector<double> > &Hprev, vector< vector<double> >&Pvals, 
                       vector< vector<double> > &qvals, vector<double> &firstlink, vector<vector< double> > &Plate);
int load_opti_data(char *filename,vector< vector<double> > &datapoints,vector<string> &point_labels);
int load_collar_opti_data(vector<vector<double> > &datapoints,vector<string> &point_labels,vector<vector<int> > point_flag, tibia &tib);
int load_accl_data(char *filename, int num_accls, int num_accl_entries, vector< vector<double> > &datapoints);
int load_collar_accl_data(char *filename, tibia &tib, femur &fem);

int load_collar_active_data(vector<vector<double> > datapoints, tibia &tib, femur &fem);
int load_active_data(char *filename, int num_act, vector< vector<double> > &datapoints);
int load_passive_data(char *filename, int num_act, vector< vector<double> > &datapoints);
int load_collar_passive_data(vector<vector<double> > all_pass_data, tibia &tib, femur &fem);

int load_all_measures(tibia &tib, femur &fem,vector<vector<double> > &frame_refs);
// **********************************************************************
// Functions and data structures defined using GNU GSL by Julien A.D.S

struct angle_data
{
  double DH_mat[16];
  double Mat_loc[16];
  double Ptloc[3];
  double Ptglob[3];
};

//solver-tools.c

int solve_gsl_lm(gsl_multifit_function_fdf *f, gsl_vector_view xv, double precision, int nbre_iter, gsl_multifit_fdfsolver *s, gsl_matrix *covar);
void print_results_lm(gsl_multifit_fdfsolver *s, size_t n, size_t p, int status, gsl_matrix *covar);
//int mon_solveur_encaps(struct data *data, double *x_init, double precision, int nb_iter, double *x_sol);


//rotation-tools
void gsl_rot_to_rodrigues(gsl_matrix *Rot, gsl_vector *rodrig);
void gsl_rodrigues_to_rot(gsl_vector *rodrig, gsl_matrix *Rot);
void encaps_vector2rot(double *vrot, double *Rot);
void gramSchmidt(gsl_vector *v1, gsl_vector *v2, gsl_vector *v3, gsl_matrix *T);
void rotX(double alpha_rad, gsl_matrix *Rot);
void rotY(double alpha_rad, gsl_matrix *Rot);
void rotZ(double alpha_rad, gsl_matrix *Rot);
void rotEuler(double X, double Y, double Z, gsl_matrix *Rot);
void gsl_vector2Rot_matrix(gsl_matrix *Rot, gsl_vector *v);
void gsl_Rot_matrix2vector(gsl_matrix *Rot, gsl_vector *v);
void mat_passage_3pts(gsl_vector *v1, gsl_vector *v2, gsl_vector *v3, gsl_matrix *T);
int gsl_hom_matrix(gsl_vector *origin,gsl_matrix *Rot,gsl_matrix *H);

//quaternion-tools
void free_acc(double *accl, double *quat);
void quat_multiply(gsl_vector *M1,gsl_vector *M2,gsl_vector *ans0);
void quat_rotate(gsl_vector *quat1, gsl_vector *vect, gsl_vector *ans1);
void quat_invert(gsl_vector *quat);
void get_quaternion_avg(vector<vector<double> > quat_datas, gsl_vector *quat_avg);
void rodrigues_to_quat(gsl_vector *quat_rod, gsl_vector *quat);

// calc_joint_angle.cpp
int calc_joint_angle_err(double angle_rad, double &DH_mat, double &Mat_loc,double &Ptloc,double &Ptglob, double &err);
int calc_j_angle_wrap(const gsl_vector *angle_rad, struct angle_data *angleD, gsl_vector *err);

int jnt_angle_jacobian(double theta, double DH_mat[], double Mat_loc[],double Ptloc[3],double J[]);
int jnt_angle_jacob_wrap(const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_matrix *J);

int calc_jnt_angle_jacob_wrap(const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_vector *err, gsl_matrix *J);
int calc_joint_ang(struct angle_data *angle_d, double theta_init,double precision, int nb_iter, double &theta_sol);

//calc_plate1_frame.cpp
int calc_link_frame_opti(tibia &tib, string linkname, int fr_num, gsl_matrix *M);
int calc_trans_mat(double Ploc[], double Pglob[], gsl_matrix *H);

int DHtoFix(vector<double> &firstlink, gsl_matrix *DHF);
int DH_hayati(double beta, double alpha, double a, double theta, gsl_matrix *DHayati);
int gsl_pt_quat_matrix(vector<double> &Pvals, vector<double> &qvals,gsl_matrix *MatOP);

int get_link_number(collar &col,string linkname);
int calc_transf_matrix(gsl_matrix *Matlink,
                       int linknumber,
                       vector< vector<double> > &Hprev,
                       vector< vector<double> > &Pvals,
                       vector< vector<double> > &qvals, 
                       vector<double> &firstlink);


// clean up optical data
int clean_opti_data(vector< vector<double> > &datapoints,vector<vector<int> > &point_flag,vector<string> &point_labels);
double getdist(vector<double> &basepoint,vector<double> &newpoint);
int getmaxmin(vector<double> &currpt,vector<double> &max_pt,vector<double> &min_pt);
int checkinbox(vector<double> &currpt,vector<double> &max_pt,vector<double> &min_pt);
double getcang(vector<double> &oldpoint, vector<double> &basepoint,vector<double> &newpoint);

// filter_data.cpp
void extract_data_point(vector< vector<double> > &datapoints, int variable_id, double data[]);
int gsl_get_fourier_coeffs(double data[], gsl_vector *f_data,int num_frames,gsl_fft_real_wavetable * real, gsl_fft_real_workspace * work);
int get_fourier_dominant_estimate(double data[], gsl_vector * f_data, int num_dominant_freq);
void gsl_filter_data(double data[],int num_frames, int num_dominant_freq);
void gsl_low_pass_fourier(double  data[], gsl_vector * f_data, int num_freq);
void gsl_low_pass_filter_data(double data[],int num_frames, int num_freq);

//generate_jacob.cpp
int generate_jacobian_femur(gsl_matrix *J, gsl_vector *state_vec, vector<vector<double> > wire_a_points,
                            vector<double> thetas,vector<double> q);
int generate_mes_vec_femur(gsl_vector *mes_vector,femur &fem, int frame_count, int &wire_frame_count, gsl_vector *tot_wire_len,vector<double> &q);
int h_femur_encaps(gsl_vector *h, gsl_vector *state_vec, vector<vector<double> > wire_a_points,
                   vector<double> thetas,vector<double> q);
int h_measurement_femur(gsl_vector *h,double *sa, double *sddot,double *qa, double theta1, double theta2,
                        double theta3, double theta4, double theta5, double *a1, double *a2, double *a3,
                        double *a4,double *a5,double *a6);
int jacob_set_tot(gsl_matrix *H, double sa[3], double sddot[3],double qa[4], double theta1, double theta2,
                    double theta3, double theta4, double theta5, double *a1, double *a2, double *a3,
                 double *a4,double *a5,double *a6);

//load-mario-frame-opti.cpp
int load_mario_frame_opti_data(vector<vector<double> > &datapoints,vector<string> &point_labels, vector<vector<double> >  &frame);
int calc_mario_frame_opti(gsl_matrix *H, vector<vector<double> > &frame_mario, vector<vector<double> > &frame);

//convert_to_mario.cpp
int convert_opti_to_mario(tibia &tib, gsl_matrix *H);


// run ekf functions
int run_kalman_femur(femur &fem, vector<vector<double> > wire_A_points, gsl_vector *tot_wire_len,
                     vector<vector<double> > &state, vector<double> thetas);
int first_local_extrema(double *data, int num_iter,int frame_start);//, int indexes[]);
int opti_first_extrema(vector<vector<double> > &datapoints, int num_markers);
int first_local_extrema_vec(vector<vector<double> > data, int index, int index_step);

//sync_opti_data.cpp
double adj_labview_tmstamp(double tmstamp);
int sync_opti_active(vector<vector<double> > &active_wire_data, vector<vector<double> >&optical_data,int num_opti_markers);

int filter_sync_passive_active(vector<vector<double> > active_wire_data, vector<vector<double> > &passive_wire_data);


int read_all_wire_bases(vector<vector<double> > &wire_bases);

//ekf femure theta
int set_theta_err_cov(gsl_matrix * Q, int num_theta);
int set_initial_estimate(gsl_vector * state_estimate);
int set_process_mat(gsl_matrix *A, double deltaT);
int set_proc_cov_mat(gsl_matrix *Q);

//generate jacob_theta_femur..
int generate_jacobian_femur_theta(gsl_matrix *J, gsl_vector *state_vec); //, femur &fe
int generate_mes_vec_femur(gsl_vector *mes_vector, femur &fem, int frame_count);
int h_measurement_femur_theta(gsl_vector *h, double theta1, double theta2,double theta3, double theta4, double theta5);
int h_femur_theta_encaps(gsl_vector *h, gsl_vector *state_vec);

int run_kalman_femur_theta(femur &fem, vector<vector<double> > &state);


int filter_accl(vector<vector<double> > &accl_data);

//accl_mario_orient
int accl_mario_orient(double *quat_mario, double *quat);


//tibia lower collar funcs:
int generate_jacobian_tib_low_theta(gsl_matrix *J, gsl_vector *state_vec); //, tibia &fe
int generate_mes_vec_tib_low(gsl_vector *mes_vector, tibia &tib, int frame_count);
int h_measurement_tib_low_theta(gsl_vector *h, double theta1, double theta2,double theta3, double theta4);
int h_tib_low_theta_encaps(gsl_vector *h, gsl_vector *state_vec);

int run_kalman_tib_low_theta(tibia &tib, vector<vector<double> > &state);







#endif