void Screen::update(const State& state){
SDL_SetRenderDrawColor(renderer_.get(), empty_.r, empty_.g, empty_.b, empty_.a);
SDL_RenderClear(renderer_.get());
// Do the actual rendering of world contents
auto cam_pos = state.camera.get_position();
auto bottom_right = camera_to_world(Position{kStartWidth, kStartHeight},
state.camera);
for(unsigned int col = cam_pos.y_; col < bottom_right.y_; ++col){
for(unsigned int row = cam_pos.x_; row < bottom_right.x_; ++row){
// Get entity position in camera coordinates
auto entity_world_pos = Position{row, col};
auto entity_cam_pos = world_to_camera(entity_world_pos, state.camera);
if(!state.map[entity_world_pos]){ /* there's nothing here */
continue;
}
auto texture = state.map[entity_world_pos]->get_texture();
SDL_Rect rect{(int)entity_cam_pos.x_, (int)entity_cam_pos.y_, kTileSize,
kTileSize};
SDL_RenderCopy(renderer_.get(), texture, nullptr, &rect);
}
}
if(state.is_debug){
auto world_time = state.timer.read_ms();
draw_frame_time(world_time);
stringstream mousestr;
mousestr << "Mouse: (" << state.mouse_loc.x_ << "," << state.mouse_loc.y_ << ")";
print_line_at(mousestr.str(), kStartHeight - kFontHeight, 0);
}
SDL_RenderPresent(renderer_.get());
}
int main(int argc, char **argv)
{
if (argc != 2)
{
printf("usage: ./borg LASERFILE\n");
return 1;
}
const char *laserfile = argv[1];
FILE *f = fopen(laserfile,"r");
if (!f)
{
printf("couldn't open [%s]\n", laserfile);
return 1;
}
FILE *out = fopen("points.txt","w");
int line = 0;
const double encoder_offset = 240;
while (!feof(f))
{
line++;
double laser_ang, row, col;
if (3 != fscanf(f, "%lf %lf %lf\n", &laser_ang, &row, &col))
{
printf("error parsing line %d\n", line);
break;
}
if ((line % 1) != 0)
continue;
extrinsics(tilt, (encoder_offset - laser_ang)*M_PI/180,
0, 135*M_PI/180,
0.48, 0.02, 0.22);
intrinsics(col, row);
intersect();
camera_to_world();
fprintf(out, "%f %f %f\n",
world_point[0],
world_point[1],
world_point[2]);
}
fclose(f);
fclose(out);
return 0;
}
void camera_to_world_build_map(ProbabilisticMapParams map_params, IplImage *dst, IplImage *src, double camera_height, double camera_pitch, int horizon_line, stereo_util stereo_util_instance)
{
for (int y = horizon_line; y < src->height; y++)
{
for (int x = 0; x < src->width; x++)
{
carmen_position_t right_point;
right_point.x = x;
right_point.y = y;
carmen_vector_3D_t p3D = camera_to_world(right_point, camera_height, camera_pitch, stereo_util_instance);
int x_map = map_grid_x(map_params, p3D.x);
int y_map = map_grid_y(map_params, p3D.y);
if (is_map_grid_cell_valid(map_params, x_map, y_map))
cvSet2D(dst, y_map, x_map, cvGet2D(src, y, x));
}
}
}
void opencv_birds_eye_remap(ProbabilisticMapParams map, const IplImage *src_image, IplImage *dst_image, double camera_height, double camera_pitch, stereo_util stereo_util_instance)
{
// coordinates of 4 quadrangle vertices in the source image.
CvPoint2D32f src_pts[4] = { cvPoint2D32f(180, 400), cvPoint2D32f(180, 320), cvPoint2D32f(520, 320), cvPoint2D32f(520, 400) };
// coordinates of the 4 corresponding quadrangle vertices in the destination image.
CvPoint2D32f dst_pts[4];
for (int i = 0; i < 4; i++)
{
carmen_position_t right_point;
right_point.x = src_pts[i].x;
right_point.y = src_pts[i].y;
carmen_vector_3D_t p3D = camera_to_world(right_point, camera_height, camera_pitch, stereo_util_instance);
int x_map = map_grid_x(map, p3D.x);
int y_map = map_grid_y(map, p3D.y);
dst_pts[i] = cvPoint2D32f(x_map, y_map);
}
// FIND THE HOMOGRAPHY
static CvMat *homography_matrix;
if (homography_matrix == NULL)
homography_matrix = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(dst_pts, src_pts, homography_matrix);
float Z = camera_height;
CV_MAT_ELEM(*homography_matrix, float, 2, 2) = Z;
cvWarpPerspective(src_image,
dst_image,
homography_matrix,
CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS,
cvScalarAll(0));
}
