Approximation Approximator::find (
const NullaryFunction & fun)
{
if (Ob val = fun.find()) {
return Approximation(val, m_less);
}
return unknown();
}
Approximation Approximator::find(const SymmetricFunction& fun,
const Approximation& lhs,
const Approximation& rhs) {
if (Ob ob = (lhs.ob and rhs.ob) ? fun.find(lhs.ob, rhs.ob) : 0) {
return Approximation(ob, m_less);
} else {
Approximation val = unknown();
DenseSet temp_set(m_item_dim);
map(fun, lhs.upper, rhs.upper, val.upper, temp_set);
map(fun, lhs.lower, rhs.lower, val.lower, temp_set);
close(val, temp_set);
return val;
}
}
Approximation Approximator::find(const InjectiveFunction& fun,
const Approximation& key) {
if (Ob ob = key.ob ? fun.find(key.ob) : 0) {
return Approximation(ob, m_less);
} else if (&fun == m_quote) {
// QUOTE is not monotone
return unknown();
} else {
Approximation val = unknown();
DenseSet temp_set(m_item_dim);
map(fun, key.upper, val.upper, temp_set);
map(fun, key.lower, val.lower, temp_set);
close(val, temp_set);
return val;
}
}
void DriftModel::Train() {
// We will solve A * x = b
vector<vector<double> > A;
vector<double> b;
vector<double> x;
if (!straight_ready_ && raw_points_straight_.size() >= 2) {
for (int i = (int) raw_points_straight_.size() - 1; i >= 0; --i) {
double angle = raw_points_straight_[i][0];
double previous_angle = raw_points_straight_[i][1];
double velocity = raw_points_straight_[i][2];
double radius = raw_points_straight_[i][3];
double direction = raw_points_straight_[i][4];
double next_angle = raw_points_straight_[i][5];
A.push_back({velocity * angle});
b.push_back(next_angle - (x_[0] * angle + x_[1] * previous_angle));
if (b.size() > 2) break;
}
double error = Approximation(A, b, x);
if (error < 1e-9) {
x_[2] = x[0];
straight_ready_ = true;
if (FLAGS_log_drift_model) std::cout << "INFO: Drift model, straight part ready (x[2] = " << x_[2] << ")" << std::endl;
}
A.clear();
b.clear();
x.clear();
}
for (int i = (int) raw_points_turn_.size() - 1; i >= 0; --i) {
double angle = raw_points_turn_[i][0];
double previous_angle = raw_points_turn_[i][1];
double velocity = raw_points_turn_[i][2];
double radius = raw_points_turn_[i][3];
double direction = raw_points_turn_[i][4];
double next_angle = raw_points_turn_[i][5];
// Ignore points if velocity is too small or we are on straigh piece (the
// first x0, x1, x2 are enough to compute next_angle). We need points were
// we can assume that we can drop the "max" factor in the model.
if (straight_ready_ && fabs(next_angle - (x_[0] * angle + x_[1] * previous_angle + x_[2] * angle * velocity)) < 1e-9) {
continue;
}
A.push_back({
velocity * angle,
-direction * velocity * velocity * sqrt(InvRadius(radius)),
direction * velocity});
b.push_back(next_angle - (x_[0] * angle + x_[1] * previous_angle));
if (!ready_ && b.size() == 4) break;
if (b.size() >= 10) break;
}
// We use 4 points instead of 3 just to make sure we do not learn model
// on incorrect ones.
if (b.size() >= 4) {
double error = Approximation(A, b, x);
// If chosen points are incorrect, then do not learn model. Wait for better
// points.
if (error < 1e-9) {
// Check if we were able to train x_[3] and x_[4]
for (int i = 0; i < A.size(); ++i) {
if (fabs(A[i][0] * x[0] - b[i]) < 1e-5) {
if (FLAGS_log_drift_model) std::cout << "WARNING: We were driving too slow on some turns. Ignore this model and wait for better one." << std::endl;
return;
}
}
if (!ready_) std::cout << "INFO: Model ready" << std::endl;
ready_ = true;
straight_ready_ = true;
x_ = {x_[0], x_[1], x[0], x[1], x[2]};
} else {
if (FLAGS_log_drift_model) std::cout << "WARNING: Learnt drift model has big error. Ignore this model and wait for better one." << std::endl;
return;
}
if (b.size() >= 10) {
std::cout << "INFO: Model very ready" << std::endl;
very_ready_ = true;
}
}
}
