void resize( size_t rows, size_t cols ) {
flut_error_if( rows < row_size() || cols < column_size(), "tables cannot be shrinked" );
// reorganize existing data
data_.resize( row_size() * cols );
for ( index_t ri = rows; ri-- > 0; )
for ( index_t ci = cols; ci-- > 0; )
data_[ cols * ri + ci ] = ( ri < row_size() && ci < column_size() ) ? data_[ column_size() * ri + ci ] : T();
col_labels_.resize( cols );
row_labels_.resize( rows );
}
// The constructor uses the appropriate settings in the config file to
// properly set up the sensor model for the specified "phase" of the LGMD
// input signal.
SensorModel::SensorModel(const std::string& lgmd_phase)
: m_sigma(0.0f), m_name(lgmd_phase)
{
const range<float> lgmd_range =
get_conf(locust_model(), "spike_range", make_range(0.0f, 800.0f)) ;
// Get the LGMD ranges for the columns of the sensor model
m_lgmd_ranges = string_to_deque<float>(
conf<std::string>(lgmd_phase + "_lgmd_ranges", "0 800")) ;
if (m_lgmd_ranges.size() < 2) { // crappy configuration!
m_lgmd_ranges.clear() ;
m_lgmd_ranges.push_back(lgmd_range.min()) ;
m_lgmd_ranges.push_back(lgmd_range.max()) ;
}
sort(m_lgmd_ranges.begin(), m_lgmd_ranges.end()) ;
if (m_lgmd_ranges.front() > lgmd_range.min())
m_lgmd_ranges.push_front(lgmd_range.min()) ;
if (m_lgmd_ranges.back() < lgmd_range.max())
m_lgmd_ranges.push_back(lgmd_range.max()) ;
// Figure out how many rows and columns the sensor model's probability
// table has and allocate space for the required number of elements.
// Initialize the probability table using a uniform distribution.
const int C = m_lgmd_ranges.size() - 1 ;
const int R = column_size() ;
const int N = R * C ;
m_prob.reserve(N) ;
std::fill_n(std::back_inserter(m_prob), N, 1.0f/N) ;
// Apply Gabbiani model to obtain causal probabilities and Gaussian
// blur neighbouring bins in each row.
update(clamp(conf(lgmd_phase + "_sigma", 1.0f),
0.1f, static_cast<float>(row_size()))) ;
}
// Copy column vector specified by given LGMD value
std::vector<float> SensorModel::column_vector(float lgmd) const
{
AutoMutex M(m_mutex) ;
const int N = column_size() ;
Table::const_iterator begin = m_prob.begin() + col_index(lgmd) * N ;
return std::vector<float>(begin, begin + N) ;
}
// This function increments the bin "pointed" to by the given [TTI, LGMD]
// pair. It also increments the other bins in the row "pointed" to by the
// TTI using a Gaussian weighting formula to ensure that no bin in that
// row has weird likelihood values that can screw up the Bayesian TTI
// estimation. Finally, it normalizes the row to ensure that each row
// vector is a valid probability distribution.
void SensorModel::update_row(float tti, float lgmd, float sigma)
{
const int N = row_size() ;
const int C = column_size() ;
const int I = col_index(lgmd) ;
const float S = 1/(2 * sqr(sigma)) ;
Table::iterator begin = m_prob.begin() + row_index(tti) ;
float normalizer = 0 ;
Table::iterator it = begin ;
for (int i = 0; i < N; ++i, it += C) {
*it += exp(-sqr(i - I) * S) ;
//*it = exp(-sqr(i - I) * S) ;
normalizer += *it ;
}
it = begin ;
for (int i = 0; i < N; ++i, it += C)
*it /= normalizer ;
}
T& operator()( index_t row, index_t col ) { flut_assert( row < row_size() && col < column_size() ); return data_[ row * column_size() + col ]; }
index_t add_column( const L& label, const T& default_value = T() ) {
resize( row_size(), column_size() + 1 );
return col_labels_.set( column_size() - 1, label );
}
index_t add_row( const L& label, const T& default_value = T() ) {
row_labels_.add( label );
data_.resize( row_size() * column_size(), default_value );
return row_size() - 1;
}
