inline const Matrix&
PiecewiseConstantCorrelation::correlation(Size i) const {
const std::vector<Matrix>& results = correlations();
QL_REQUIRE(i<results.size(),
"index (" << i <<
") must be less than correlations vector size (" <<
results.size() << ")");
return results[i];
}
// Load a correlation table from file
arma::Mat<float> load_correlation_file(std::string &filename) {
// Used to store strings from file prior to matrix construction and other variables
std::string line;
std::string ele;
std::stringstream line_stream;
std::vector<float> correlations_vector;
bool id;
// Open file stream
std::ifstream cor_file;
cor_file.open(filename);
// Skip header, order SHOULD be the same as input OTU table
std::getline(cor_file, line);
line_stream.str(line);
// Count OTUs in header if needed
int otu_number = std::count(line.begin(), line.end(), '\t');
// Reserving space memory
correlations_vector.reserve(otu_number * otu_number);
// Process correlation elements
while(std::getline(cor_file, line)) {
// (Re)sets variables for loop
id = true;
line_stream.clear();
// Add current line to line stream and then split by tabs
line_stream.str(line);
while (std::getline(line_stream, ele, '\t')) {
// Skip the OTU ID column
if (id) {
id = false;
continue;
}
// Add current element to correlation mat after converting to float
correlations_vector.push_back(std::stof(ele));
}
}
// Construct matrix and return it
arma::Mat<float> correlations(correlations_vector);
correlations.reshape(otu_number, otu_number);
return correlations;
}
Disposable<Matrix> exponentialCorrelations(
const std::vector<Time>& rateTimes,
Real longTermCorr,
Real beta,
Real gamma,
Time time) {
// preliminary checks
checkIncreasingTimes(rateTimes);
QL_REQUIRE(longTermCorr<=1.0 && longTermCorr>=0.0,
"Long term correlation (" << longTermCorr <<
") outside [0;1] interval");
QL_REQUIRE(beta>=0.0,
"beta (" << beta <<
") must be greater than zero");
QL_REQUIRE(gamma<=1.0 && gamma>=0.0,
"gamma (" << gamma <<
") outside [0;1] interval");
// Calculate correlation matrix
Size nbRows = rateTimes.size()-1;
Matrix correlations(nbRows, nbRows, 0.0);
for (Size i=0; i<nbRows; ++i) {
// correlation is defined only between
// (alive) stochastic rates...
if (time<=rateTimes[i]) {
correlations[i][i] = 1.0;
for (Size j=0; j<i; ++j) {
if (time<=rateTimes[j]) {
correlations[i][j] = correlations[j][i] =
longTermCorr + (1.0-longTermCorr) *
std::exp(-beta*std::fabs(
std::pow(rateTimes[i]-time, gamma) -
std::pow(rateTimes[j]-time, gamma)
)
);
}
}
}
}
return correlations;
}
Matrix<double> InputsSelectionAlgorithm::calculate_logistic_correlations(void) const
{
// Control sentence (if debug)
#ifdef __OPENNN_DEBUG__
std::ostringstream buffer;
if(!training_strategy_pointer)
{
buffer << "OpenNN Exception: InputsSelectionAlgorithm class.\n"
<< "void check(void) const method.\n"
<< "Pointer to training strategy is NULL.\n";
throw std::logic_error(buffer.str());
}
// Performance functional stuff
if(!training_strategy_pointer->has_performance_functional())
{
buffer << "OpenNN Exception: InputsSelectionAlgorithm class.\n"
<< "void check(void) const method.\n"
<< "Pointer to performance functional is NULL.\n";
throw std::logic_error(buffer.str());
}
if(!training_strategy_pointer->get_performance_functional_pointer()->has_data_set())
{
buffer << "OpenNN Exception: InputsSelectionAlgorithm class.\n"
<< "void check(void) const method.\n"
<< "Pointer to data set is NULL.\n";
throw std::logic_error(buffer.str());
}
#endif
// Problem stuff
const PerformanceFunctional* performance_functional_pointer = training_strategy_pointer->get_performance_functional_pointer();
const DataSet* data_set_pointer = performance_functional_pointer->get_data_set_pointer();
const Variables& variables = data_set_pointer->get_variables();
const size_t inputs_number = variables.count_inputs_number();
const size_t targets_number = variables.count_targets_number();
const Vector<size_t> input_indices = variables.arrange_inputs_indices();
const Vector<size_t> target_indices = variables.arrange_targets_indices();
Matrix<double> correlations(inputs_number, targets_number);
for(size_t i = 0; i < inputs_number; i++)
{
const Vector<double> inputs = data_set_pointer->get_variable(input_indices[i]);
for(size_t j = 0; j < targets_number; j++)
{
const Vector<double> targets = data_set_pointer->get_variable(target_indices[j]);
Matrix<double> data(inputs.size(), 2);
data.set_column(0, inputs);
data.set_column(1, targets);
DataSet data_set(data);
data_set.scale_inputs("MinimumMaximum");
Instances* instances_pointer = data_set.get_instances_pointer();
instances_pointer->set_training();
NeuralNetwork neural_network(1, 1);
MultilayerPerceptron* multilayer_perceptron_pointer = neural_network.get_multilayer_perceptron_pointer();
multilayer_perceptron_pointer->set_layer_activation_function(0, Perceptron::Logistic);
PerformanceFunctional performance_functional(&neural_network, &data_set);
performance_functional.set_objective_type(PerformanceFunctional::MEAN_SQUARED_ERROR_OBJECTIVE);
TrainingStrategy training_strategy(&performance_functional);
training_strategy.set_main_type(TrainingStrategy::LEVENBERG_MARQUARDT_ALGORITHM);
training_strategy.get_Levenberg_Marquardt_algorithm_pointer()->set_display(false);
training_strategy.get_Levenberg_Marquardt_algorithm_pointer()->set_performance_goal(0.0);
training_strategy.get_Levenberg_Marquardt_algorithm_pointer()->set_gradient_norm_goal(0.0);
training_strategy.get_Levenberg_Marquardt_algorithm_pointer()->set_minimum_performance_increase(0.0);
training_strategy.perform_training();
const Vector<double> outputs = neural_network.calculate_output_data(inputs.to_column_matrix()).to_vector();
correlations(i,j) = targets.calculate_linear_correlation(outputs);
}
}
return(correlations);
}
vector<SubstitutionalCorrelation*> correlators_from_cuns(ChemicalUnitNode* node1,ChemicalUnitNode* node2,vector<double> corr) {
int s1 = node1->chemical_units.size();
int s2 = node2->chemical_units.size();
vector<double> correlations(s1*s2);
//check there are enough numbers in the matrix
//fill the correlations
//if full version, check the last column-row
bool full_version;
if(corr.size()==(s1-1)*(s2-1)) {
full_version = false;
//fill upper-right corner of correlation matrix
for(int i=0; i<s1-1; i++)
for(int j=0; j<s2-1; j++)
correlations[i+s1*j] = corr[i+j*(s1-1)];
} else if(corr.size()==s1*s2) {
full_version = true;
correlations = corr;
} else {
REPORT(ERROR) << "Wrong number of joint probabilities, expected a " << (s1-1)<< "x" << (s2-1) << " or a " << s1 << "x" << s2 << " matrix. Found " << corr.size() << " values.\n";
throw "fail";
}
//fill last row
for(int j=0; j<s2-1; j++){
correlations[s1*j+s1-1]=node2->chemical_units[j].get_occupancy();
for(int i=0; i<s1-1; i++)
correlations[s1*j+s1-1]-=correlations[i+s1*j];
}
//fill last coloumn
for(int i=0; i<s1; i++) {
correlations[(s2-1)*s1+i]=node1->chemical_units[i].get_occupancy();
for(int j=0; j<s2-1; j++)
correlations[(s2-1)*s1+i] -= correlations[i+s1*j];
}
if(full_version)
for(int i=0; i<correlations.size(); ++i)
if(!(almost_equal(corr[i],correlations[i]))){
REPORT(ERROR) << "The joint probabilities are incompatible with the average structure.\n";
throw "fail";
}
//create correlations
vector<SubstitutionalCorrelation*> res;
for(int j=0; j<s2; j++)
{
for(int i=0; i<s1; i++)
{
res.push_back( new SubstitutionalCorrelation(& node1->chemical_units[i],& node2->chemical_units[j], correlations[i+s1*j]));
}
}
return res;
}
void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
{
res = boost::none;
height_res = boost::none;
if (!p.is_open()) // Open serial port if closed
{
open();
return;
}
ByteVec ensemble;
ensemble.resize(4);
// Header ID
if (!read_byte(ensemble[0]))
return;
if (ensemble[0] != 0x7F)
return;
ros::Time stamp = ros::Time::now();
// Data Source ID
if (!read_byte(ensemble[1]))
return;
if (ensemble[1] != 0x7F)
return;
// Size low
if (!read_byte(ensemble[2]))
return;
// Size high
if (!read_byte(ensemble[3]))
return;
uint16_t ensemble_size = getu16le(ensemble.data() + 2);
ensemble.resize(ensemble_size);
for (int i = 4; i < ensemble_size; i++)
{
if (!read_byte(ensemble[i]))
return;
}
uint16_t checksum = 0;
BOOST_FOREACH (uint16_t b, ensemble)
checksum += b;
uint16_t received_checksum;
if (!read_short(received_checksum))
return;
if (received_checksum != checksum)
{
ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
checksum, ensemble_size);
return;
}
if (ensemble.size() < 6)
return;
for (int dt = 0; dt < ensemble[5]; dt++)
{
int offset = getu16le(ensemble.data() + 6 + 2 * dt);
if (ensemble.size() - offset < 2)
continue;
// Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
// Bottom Track, Bottom Track Range
uint16_t section_id = getu16le(ensemble.data() + offset);
std::vector<double> correlations(4, nan(""));
if (section_id == 0x5803) // Bottom Track High Resolution Velocity
{
if (ensemble.size() - offset < 2 + 4 * 4)
continue;
res = boost::make_optional(mil_msgs::VelocityMeasurements());
res->header.stamp = stamp;
std::vector<geometry_msgs::Vector3> dirs;
{
double tilt = 30 * boost::math::constants::pi<double>() / 180;
double x = sin(tilt);
double z = cos(tilt);
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
}
// Keep track of which beams didn't return for logging
std::vector<size_t> invalid_beams;
invalid_beams.reserve(4);
for (int i = 0; i < 4; i++)
{
mil_msgs::VelocityMeasurement m;
m.direction = dirs[i];
int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
m.velocity = -vel * .01e-3;
if (vel == -3276801) // -3276801 indicates no data
{
invalid_beams.push_back(i + 1);
m.velocity = nan("");
}
res->velocity_measurements.push_back(m);
}
// Report a list of invalid beams
if (invalid_beams.size() > 0)
{
std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
for (auto beam : invalid_beams)
to_log += std::to_string(beam) + " ";
ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
}
}
else if (section_id == 0x0600) // Bottom Track
{
for (int i = 0; i < 4; i++)
correlations[i] = *(ensemble.data() + offset + 32 + i);
}
else if (section_id == 0x5804) // Bottom Track Range
{
if (ensemble.size() - offset < 2 + 4 * 3)
continue;
if (gets32le(ensemble.data() + offset + 10) <= 0)
{
ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
continue;
}
height_res = boost::make_optional(mil_msgs::RangeStamped());
height_res->header.stamp = stamp;
height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
}
if (res)
{
for (int i = 0; i < 4; i++)
{
res->velocity_measurements[i].correlation = correlations[i];
}
}
}
}
