int main() {
std::vector<double> dataz( 8, 0.0f );
buffer_view<double> view8x1( dataz );
buffer_view<double, 2> view2x4( dataz, 2, 4 );
array_print( dataz );
view8x1[0] = 2.0f;
view8x1[1] = 2.0f;
array_print( dataz );
view2x4[{1, 1}] = 3.0f;
view2x4[{1, 0}] = 3.0f;
array_print( dataz );
// calls will fail
//buffer_view<double, 2> view2x4fail( dataz );
//view2x4[ 1 ] = 50.0f;
}
void SMFModel::parseSMFFile(std::string name)
{
std::map<Vector4D, std::vector<Vector3D>*, vec4Compare> vertexNormals;
std::string filename = "./";
filename += name;
std::vector<Vector4D*> vertices;
std::vector<Vector3D*> normals;
std::string line;
std::ifstream infile( filename.c_str() );
int i = 0;
while (std::getline( infile, line )) {
std::istringstream iss(line);
std::vector<std::string> tokens;
copy(std::istream_iterator<std::string>(iss),
std::istream_iterator<std::string>(),
std::back_inserter(tokens));
if (tokens.size() == 0)
continue;
else if (tokens.at(0) == "v") {
float x, y, z;
std::stringstream datax(tokens.at(1));
datax >> x;
std::stringstream datay(tokens.at(2));
datay >> y;
std::stringstream dataz(tokens.at(3));
dataz >> z;
vertices.push_back(new Vector4D(x, y, z, 1.0));
i++;
}
else if (tokens.at(0) == "f") {
void ThermalCalibrationHelper::calculate()
{
// baro
int count = m_baroSamples.count();
Eigen::VectorXf datax(count);
Eigen::VectorXf datay(1);
Eigen::VectorXf dataz(1);
Eigen::VectorXf datat(count);
for (int x = 0; x < count; x++) {
datax[x] = m_baroSamples[x].Pressure;
datat[x] = m_baroSamples[x].Temperature;
}
m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro,
&m_results.baroInSigma, &m_results.baroOutSigma);
if (m_results.baroCalibrated) {
addInstructions(tr("Barometer is calibrated."));
} else {
qDebug() << "Failed to calibrate baro!";
addInstructions(tr("Failed to calibrate barometer!"), WizardModel::Warn);
}
m_results.baroTempMin = datat.array().minCoeff();
m_results.baroTempMax = datat.array().maxCoeff();
// gyro
count = m_gyroSamples.count();
datax.resize(count);
datay.resize(count);
dataz.resize(count);
datat.resize(count);
for (int x = 0; x < count; x++) {
datax[x] = m_gyroSamples[x].x;
datay[x] = m_gyroSamples[x].y;
dataz[x] = m_gyroSamples[x].z;
datat[x] = m_gyroSamples[x].temperature;
}
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat,
m_results.gyro, m_results.gyroBias,
m_results.gyroInSigma, m_results.gyroOutSigma);
if (m_results.gyroCalibrated) {
addInstructions(tr("Gyro is calibrated."));
} else {
qDebug() << "Failed to calibrate gyro!";
addInstructions(tr("Failed to calibrate gyro!"), WizardModel::Warn);
}
// accel
m_results.accelGyroTempMin = datat.array().minCoeff();
m_results.accelGyroTempMax = datat.array().maxCoeff();
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
/*
count = m_accelSamples.count();
datax.resize(count);
datay.resize(count);
dataz.resize(count);
datat.resize(count);
for(int x = 0; x < count; x++){
datax[x] = m_accelSamples[x].x;
datay[x] = m_accelSamples[x].y;
dataz[x] = m_accelSamples[x].z;
datat[x] = m_accelSamples[x].temperature;
}
m_results.accelCalibrated = ThermalCalibration::AccelerometerCalibration(datax, datay, dataz, datat, m_results.accel);
*/
m_results.accelCalibrated = false;
QString str = QStringLiteral("INFO::Calibration results") + "\n";
str += QStringLiteral("INFO::Baro cal {%1, %2, %3, %4}; initial variance: %5; Calibrated variance %6")
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + "\n";
str += QStringLiteral("INFO::Gyro cal x{%1, %2, %3} y{%4, %5, %6} z{%7, %8, %9}; initial variance: {%10, %11, %12}; Calibrated variance {%13, %14, %15}")
.arg(m_results.gyroBias[0]).arg(m_results.gyro[0]).arg(m_results.gyro[1])
.arg(m_results.gyroBias[1]).arg(m_results.gyro[2]).arg(m_results.gyro[3])
.arg(m_results.gyroBias[2]).arg(m_results.gyro[4]).arg(m_results.gyro[5])
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + "\n";
str += QStringLiteral("INFO::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + "\n";
qDebug() << str;
m_debugStream << str;
emit calculationCompleted();
closeDebugLog();
}
