Structure::Structure(string &filename,Random &ran,int oneDelta)
{
ifstream in;
in.open(filename.c_str());
// if (in.fail())
// {
// //cout << "ERROR: Unable to open file " << filename.c_str() << ". Aborting.\n\n";
// //exit(-1);
// }
Q = 0;
in >> Q;
// if (Q <= 0)
// {
// cout << "ERROR: Illegal number of studies, Q = " << Q << ", read in file " << filename << ". Aborting.\n\n";
// exit(-1);
// }
S.resize(Q);
vector<string> fileData(Q);
vector<string> filePsi(Q);
int q;
for (q = 0; q < Q; q++)
{
in >> fileData[q];
// if (in.fail())
// {
// cout << "ERROR: Unable to read expression value file number " << q + 1 << " from file " << filename << ". Aborting.\n\n";
// exit(-1);
// }
in >> filePsi[q];
// if (in.fail())
// {
// cout << "ERROR: Unable to read clinical value file number " << q + 1 << " from file " << filename << ". Aborting.\n\n";
// exit(-1);
// }
}
in.close();
for (q = 0; q < Q; q++)
{
ifstream inData;
inData.open(fileData[q].c_str());
// if (inData.fail())
// {
// cout << "ERROR: Error when reading " << filename.c_str() << ". Unable to open file " << fileData[q] << ". Aborting.\n\n";
// exit(-1);
// }
ifstream inPsi;
inPsi.open(filePsi[q].c_str());
// if (inPsi.fail())
// {
// cout << "ERROR: Error when reading " << filename.c_str() << ". Unable to open file " << filePsi[q] << ". Aborting.\n\n";
// exit(-1);
// }
int GG = 0;
inData >> GG;
// if (GG <= 0)
// {
// cout << "ERROR: Illegal number of genes, G = " << G << ", read in file " << fileData[q] << ". Aborting.\n\n";
// exit(-1);
// }
if (q == 0)
G = GG;
// else
// {
// if (GG != G)
// {
// cout << "ERROR: Inconsistent number of genes in file " << fileData[0] << " and file " << fileData[q] << ". Aborting.\n\n";
// exit(-1);
// }
// }
S[q] = 0;
inData >> S[q];
// if (S[q] <= 0)
// {
// cout << "ERROR: Illegal number of samples, S = " << S[q] << ", read from " << fileData[q] << ". Aborting.\n\n";
// exit(-1);
// }
int SS = 0;
inPsi >> SS;
// if (SS <= 0)
// {
// cout << "ERROR: Illegal number of samples, S = " << SS << ", read from " << filePsi[q] << ". Aborting.\n\n";
// exit(-1);
// }
// if (SS != S[q])
// {
// cout << "ERROR: Inconsistent number of samples read in " << fileData[q] << " and " << filePsi[q] << ". Aborting.\n\n";
// exit(-1);
// }
inData.close();
inPsi.close();
}
//
// Print data size
//
// cout << "Size of data set:\n\n";
// cout << "P: " << Q << "\n";
// cout << "G: " << G << "\n";
// cout << "S: ";
// for (q = 0; q < Q; q++)
// cout << S[q] << " ";
// cout << "\n\n\n";
//
// allocate neccesary space
//
allocateSpace();
//
// read expression values from files
//
for (q = 0; q < Q; q++)
{
ifstream in;
in.open(fileData[q].c_str());
int GG,SS;
in >> GG;
in >> SS;
int g,s;
for (g = 0; g < G; g++)
for (s = 0; s < S[q]; s++)
in >> x[q][g][s];
in.close();
}
//
// read clinical variables from files
//
for (q = 0; q < Q; q++)
{
ifstream in;
in.open(filePsi[q].c_str());
int SS;
in >> SS;
int s;
for (s = 0; s < S[q]; s++)
{
in >> psi[q][s];
// if (psi[q][s] != 0 && psi[q][s] != 1)
// {
// cout << "ERROR: value different from 0 or 1 found in \"" << filePsi[q] << "\". Aborting.\n";
// exit(-1);
// }
}
in.close();
}
//
// initialise remaining variables
//
initialiseVariables(ran,oneDelta);
return;
}
// Accumulate imu data and store to buffer at desired rate
void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang,
float *delta_vel)
{
if (!_initialised) {
initialiseVariables(time_usec);
_initialised = true;
_start_predict_enabled = true;
}
float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
dt = math::max(dt, 1.0e-4f);
dt = math::min(dt, 0.02f);
_time_last_imu = time_usec;
if (_time_last_imu > 0) {
_dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
}
// copy data
imuSample imu_sample_new = {};
memcpy(&imu_sample_new.delta_ang._data[0], delta_ang, sizeof(imu_sample_new.delta_ang._data));
memcpy(&imu_sample_new.delta_vel._data[0], delta_vel, sizeof(imu_sample_new.delta_vel._data));
imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
imu_sample_new.time_us = time_usec;
imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0);
imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1);
imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2);
imu_sample_new.delta_ang -= _state.gyro_bias * imu_sample_new.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);
imu_sample_new.delta_vel(2) -= _state.accel_z_bias * imu_sample_new.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);;
// store the new sample for the complementary filter prediciton
_imu_sample_new = imu_sample_new;
_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
Quaternion delta_q;
delta_q.rotate(imu_sample_new.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled.normalize();
matrix::Dcm<float> delta_R(delta_q.inversed());
_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
_imu_down_sampled.delta_vel += imu_sample_new.delta_vel;
_imu_ticks++;
if ((_dt_imu_avg * _imu_ticks >= (float)(FILTER_UPDATE_PERRIOD_MS) / 1000 && _start_predict_enabled)
|| (_dt_imu_avg * _imu_ticks >= 0.02f)) {
_imu_down_sampled.delta_ang = _q_down_sampled.to_axis_angle();
_imu_down_sampled.time_us = time_usec;
_imu_buffer.push(_imu_down_sampled);
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
_imu_down_sampled.delta_vel_dt = 0.0f;
_q_down_sampled(0) = 1.0f;
_q_down_sampled(1) = _q_down_sampled(2) = _q_down_sampled(3) = 0.0f;
_imu_ticks = 0;
_imu_updated = true;
} else {
_imu_updated = false;
}
_imu_sample_delayed = _imu_buffer.get_oldest();
}
Structure::Structure(int Q,int G,int *S,double *x,int *psi,Random &ran,int checkinput,int oneDelta)
{
this->Q = Q;
this->G = G;
this->S.resize(this->Q);
int q;
for (q = 0; q < this->Q; q++)
this->S[q] = S[q];
//
// allocate necessary space
//
allocateSpace();
//
// set expression values
//
int g,s;
int nr = 0;
for (q = 0; q < this->Q; q++)
for (g = 0; g < this->G; g++)
for (s = 0; s < this->S[q]; s++)
{
this->x[q][g][s] = x[nr];
nr++;
}
//
// set clinical variables
//
nr = 0;
for (q = 0; q < this->Q; q++)
for (s = 0; s < this->S[q]; s++)
{
this->psi[q][s] = psi[nr];
nr++;
}
// if (checkinput)
// {
//cout << "Expression values:\n";
// for (q = 0; q < this->Q; q++)
// {
// cout << "first values of study " << q << "\n";
// cout << this->x[q][0][0] << " " << this->x[q][0][1] << "\n";
// cout << this->x[q][1][0] << " " << this->x[q][1][1] << "\n";
// }
// cout << "\n";
// cout << "Clinical values:\n";
// for (q = 0; q < this->Q; q++)
// {
// cout << "study " << q << ": ";
// for (s = 0; s < this->S[q]; s++)
// cout << this->psi[q][s] << " ";
// cout << "\n";
// }
// cout << "\n";
// }
//
// initialise remaining parameters
//
initialiseVariables(ran,oneDelta);
return;
}
