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; } } }