//Update the position
Ball BallFilter::get_updated_ball_estimate(float new_dist, float dist_variance, float new_dir, float dir_variance)
{ //dt seconds
float new_x = new_dist * cos(new_dir);
float new_y = new_dist * sin(new_dir);
KMath::Kalman1D<float>::Xbar x_dist = x_filter.update(new_x, dist_variance);
KMath::Kalman1D<float>::Xbar y_dist = y_filter.update(new_y, dist_variance);
// Kalman1D<float>::Xbar dist = dist_filter.update(new_dist, dist_variance, dt);
// Kalman1D<float>::Xbar dir = dir_filter.update(new_dir, dir_variance, dt);
// filtered_ball.set_relativex(dist(0) * cos(dir(0)));
// filtered_ball.set_relativey(dist(0) * sin(dir(0)));
//
// filtered_ball.set_relativexspeed((dist(0) + dist(1)) * cos(dir(0) + dir(1)) - filtered_ball.relativex());
// filtered_ball.set_relativeyspeed((dist(0) + dist(1)) * sin(dir(0) + dir(1)) - filtered_ball.relativey());
filtered_ball.set_relativex(x_dist(0));
filtered_ball.set_relativey(y_dist(0));
filtered_ball.set_relativexspeed(x_dist(1));
filtered_ball.set_relativeyspeed(y_dist(1));
return filtered_ball;
}
void training_imagenet_raw_to_structured_data_transformer::transform(
unsigned int sample_id,
const std::vector<unsigned char>& raw_data,
float * structured_data)
{
cv::Mat3b original_image = cv::imdecode(raw_data, CV_LOAD_IMAGE_COLOR);
// Defaults to center crop
unsigned int source_crop_image_width = std::min(original_image.rows, original_image.cols);
unsigned int source_crop_image_height = source_crop_image_width;
unsigned int x = (original_image.cols - source_crop_image_width) / 2;
unsigned int y = (original_image.rows - source_crop_image_height) / 2;
{
std::lock_guard<std::mutex> lock(gen_mutex);
for(int attempt = 0; attempt < 10; ++attempt)
{
float local_area = static_cast<float>(original_image.rows * original_image.cols);
float relative_target_area = dist_relative_target_area.min();
if (dist_relative_target_area.max() > dist_relative_target_area.min())
relative_target_area = dist_relative_target_area(gen);
float target_area = local_area * relative_target_area;
float aspect_ratio = expf(dist_log_aspect_ratio(gen));
unsigned int new_source_crop_image_width = std::max(static_cast<unsigned int>(sqrtf(target_area * aspect_ratio) + 0.5F), 1U);
unsigned int new_source_crop_image_height = std::max(static_cast<unsigned int>(sqrtf(target_area / aspect_ratio) + 0.5F), 1U);
if ((new_source_crop_image_width < static_cast<unsigned int>(original_image.cols)) && (new_source_crop_image_height < static_cast<unsigned int>(original_image.rows)))
{
source_crop_image_width = new_source_crop_image_width;
source_crop_image_height = new_source_crop_image_height;
std::uniform_int_distribution<unsigned int> x_dist(0, original_image.cols - source_crop_image_width);
std::uniform_int_distribution<unsigned int> y_dist(0, original_image.rows - source_crop_image_height);
x = x_dist.min();
if (x_dist.max() > x_dist.min())
x = x_dist(gen);
y = y_dist.min();
if (y_dist.max() > y_dist.min())
y = y_dist(gen);
break;
}
}
}
cv::Mat3b source_image_crop = original_image.rowRange(y, y + source_crop_image_height).colRange(x, x + source_crop_image_width);
cv::Mat3b target_image(target_image_height, target_image_width);
cv::resize(source_image_crop, target_image, target_image.size(), 0.0, 0.0, cv::INTER_CUBIC);
float * r_dst_it = structured_data;
float * g_dst_it = structured_data + (target_image_width * target_image_height);
float * b_dst_it = structured_data + (target_image_width * target_image_height * 2);
for(cv::Mat3b::const_iterator it = target_image.begin(); it != target_image.end(); ++it, ++r_dst_it, ++g_dst_it, ++b_dst_it)
{
*r_dst_it = static_cast<float>((*it)[2]) * (1.0F / 255.0F);
*g_dst_it = static_cast<float>((*it)[1]) * (1.0F / 255.0F);
*b_dst_it = static_cast<float>((*it)[0]) * (1.0F / 255.0F);
}
}
//Predict the position without observations and robots Motion Model
Ball BallFilter::get_predicted_ball_estimate(float dt, KLocalization::KMotionModel const & MM)
{
double tmpDist, tmpDir, tmpRot, speedX, speedY, newx, newy, newx2, newy2;
tmpDist = MM.Distance.val * (MM.Distance.ratiomean);
tmpDist/=1000;
tmpDir = MM.Direction.val + MM.Direction.Emean;
tmpRot = - (MM.Rotation.val + MM.Rotation.Emean); //We turn on the other way the robot turns
KMath::Kalman1D<float>::Xbar x_dist = x_filter.read();
KMath::Kalman1D<float>::Xbar y_dist = y_filter.read();
//Translation
newx = x_dist(0) - cos(tmpDir)*tmpDist;
newy = y_dist(0) - sin(tmpDir)*tmpDist;
//Rotate point
newx2 = newx*cos(tmpRot) - newy*sin(tmpRot);
newy2 = newx*sin(tmpRot) + newy*cos(tmpRot);
//Rotate speed vector
speedX = x_dist(1)*cos(tmpRot) - y_dist(1)*sin(tmpRot);
speedY = x_dist(1)*sin(tmpRot) + y_dist(1)*cos(tmpRot);
if(fabs(speedX)<0.02) speedX=0;//2cm per second is the threshold according to RAUL ROJAS, MARK SIMON
if(fabs(speedY)<0.02) speedY=0;//2cm per second is the threshold according to RAUL ROJAS, MARK SIMON
x_filter.set(newx2, speedX);
y_filter.set(newy2, speedY);
//Again -0.35m/s is the deceleration found by RAUL ROJAS, MARK SIMON
x_dist = x_filter.predict_with_decel(dt,0.35);
y_dist = y_filter.predict_with_decel(dt,0.35);
// Kalman1D<float>::Xbar dist = dist_filter.predict(dt);
// Kalman1D<float>::Xbar dir = dir_filter.predict(dt);
//
// filtered_ball.set_relativex(dist(0) * cos(dir(0)));
// filtered_ball.set_relativey(dist(0) * sin(dir(0)));
//
// filtered_ball.set_relativexspeed((dist(0) + dist(1)) * cos(dir(0) + dir(1)) - filtered_ball.relativex());
// filtered_ball.set_relativeyspeed((dist(0) + dist(1)) * sin(dir(0) + dir(1)) - filtered_ball.relativey());
filtered_ball.set_relativex(x_dist(0));
filtered_ball.set_relativey(y_dist(0));
filtered_ball.set_relativexspeed(x_dist(1));
filtered_ball.set_relativeyspeed(y_dist(1));
return filtered_ball;
}
std::vector<needle> scatter_needles(double height, std::size_t n) {
static std::mt19937 gen((std::random_device())());
std::uniform_real_distribution<> x_dist(0.0, height);
std::uniform_real_distribution<> phi_dist(0.0, 2 * M_PI);
std::vector<needle> ret;
ret.reserve(n);
for (std::size_t i = 0; i != n; ++i) {
ret.emplace_back(x_dist(gen), phi_dist(gen));
}
return ret;
}
/**
* Move snake to the next state
*/
bool Snake::update() {
int nodes = (int)xs.size();
if (is_closed()) {
double x = (xs[0] + xs[nodes - 1]) / 2;
double y = (ys[0] + ys[nodes - 1]) / 2;
xs[0] = xs[nodes - 1] = x;
ys[0] = ys[nodes - 1] = y;
}
cv::Mat x_dist(nodes, 1, CV_64F), y_dist(nodes, 1, CV_64F);
for (int k = 0; k < nodes; ++k) {
double x_force =
grad.first.at<double>(ys[k], xs[k]) *
(edge_weight * hess.at<double>(ys[k], xs[k]) - line_weight);
double y_force =
grad.second.at<double>(ys[k], xs[k]) *
(edge_weight * hess.at<double>(ys[k], xs[k]) - line_weight);
x_dist.at<double>(k, 0) = tick * x_force;
y_dist.at<double>(k, 0) = tick * y_force;
}
cv::Mat forced_xs(xs), forced_ys(ys);
forced_xs += x_dist;
forced_ys += y_dist;
// std::vector<double> prev_xs(xs), prev_ys(ys);
xs = (cv::Mat)(pentamat * forced_xs);
ys = (cv::Mat)(pentamat * forced_ys);
// double acc_x = 0, acc_y = 0;
// double eps = ???;
//
// for (int k = 0; k < nodes; ++k)
// {
// acc_x += xs[k] - prev_xs[k];
// acc_y += ys[k] - prev_ys[k];
// }
// acc_x += acc_y; acc_x /= tick;
//
// if (std::abs(acc_x) < eps)
// {
// return true;
// }
return false;
}
void generateData() {
for (int32_t i = 0; i < num_aps; i++) {
auto pair = std::make_tuple(x_dist(rng), y_dist(rng), geom_dist(rng) + 1);
ap_locations.push_back(pair);
}
for (int32_t i = 0; i < num_examples; i++) {
int32_t x_coord = x_dist(rng);
int32_t y_coord = y_dist(rng);
std::vector<int32_t> rssi;
for (int32_t k = 0; k < num_aps; k++) {
double l2_norm =
std::sqrt(std::pow(x_coord - std::get<0>(ap_locations[k]), 2) +
std::pow(y_coord - std::get<1>(ap_locations[k]), 2));
double strength = 1 / std::pow(l2_norm, 2) + norm(rng);
std::cout << strength << " ";
double scaled = strength / (std::get<2>(ap_locations[k]) / 1000.);
double db = 10 * std::log(scaled);
rssi.push_back(db);
}
examples.push_back(std::make_tuple(x_coord, y_coord, rssi));
}
}
int main(int argc, char* argv[])
{
// スコープ長いけれど……
std::string const output_filename = "report_movement";
if(argc != 2 || argc != 4)\
if(argc != 2 && argc != 4)
{
std::cout << "Usage: " << argv[0] << " 試行回数 [分割数タテ 分割数ヨコ]" << std::endl;
//std::quick_exit(-1);
return -1;
}
bool const is_fixed = argc == 4;
int const try_num = std::atoi(argv[1]);
auto const split_num = is_fixed ? std::make_pair(std::atoi(argv[3]), std::atoi(argv[2])) : std::make_pair(-1, -1);
std::mt19937 mt;
std::uniform_int_distribution<int> select_dist(1, 20); // 選択可能回数生成器
std::uniform_int_distribution<int> size_dist (2, 16); // サイズ生成器
std::uniform_int_distribution<int> cost_dist (1, 50); // コスト生成器
for(int n=0; n<try_num; ++n)
{
int const problem_id = 0; // 仮
std::string const player_id = "0"; //仮
std::pair<int,int> const size = is_fixed ? split_num : std::make_pair(size_dist(mt), size_dist(mt));
int const selectable = select_dist(mt);
int const cost_select = cost_dist(mt);
int const cost_change = cost_dist(mt);
std::vector<std::vector<point_type>> block(
size.second,
std::vector<point_type>(size.first, {-1, -1})
);
std::uniform_int_distribution<int> x_dist(0, size.first - 1);
std::uniform_int_distribution<int> y_dist(0, size.second - 1);
for(int i=0; i<size.second; ++i)
{
for(int j=0; j<size.first; ++j)
{
int x, y;
do
{
x = x_dist(mt);
y = y_dist(mt);
}
while(!(block[y][x] == point_type{-1, -1}));
block[y][x] = point_type{j, i};
}
}
// テストデータ発行
question_data question{
problem_id,
player_id,
size,
selectable,
cost_select,
cost_change,
block
};
// 実行から評価まで
algorithm algo;
algo.reset(question); // テストデータのセット
// エミュレータ起動
test_tool::emulator emu(question);
// 評価保存先ファイルを開き，問題データを書き込む
std::ofstream ofs(output_filename + std::to_string(n) + ".csv");
ofs << "[Question]\n";
for(auto const& line : block)
{
for(auto const& elem : line)
{
ofs << R"(")" << elem.x << "," << elem.y << R"(", )";
}
ofs << "\n";
}
ofs << "\n";
ofs << "[Answer]\n";
ofs << "回数,間違い位置数,コスト\n";
int counter = 0;
while(auto first_result = algo.get()) // 解答が受け取れる間ループ
{
// 評価
auto const evaluate = emu.start(first_result.get());
// 書出
ofs << counter+1 << "," <<evaluate.wrong << "," << evaluate.cost << "\n";
++counter; // テストなので解答回数をカウントする必要あるかなと
}
}
return 0;
}
