img_t *UndistortImage(img_t *img,
const fisheye_params_t &fisheye_params) {
double R[9];
matrix_ident(3, R);
int w = img->w;
int h = img->h;
img_t *img_out = img_new(w, h);
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
double xn = x - 0.5 * w;
double yn = y - 0.5 * h;
double x_new = 0.0, y_new = 0.0;
DistortPoint(xn, yn, fisheye_params, R, x_new, y_new);
x_new += 0.5 * w;
y_new += 0.5 * h;
if (x_new < 0 || x_new >= w || y_new < 0 || y_new >= h)
continue;
fcolor_t c = pixel_lerp(img, x_new, y_new);
img_set_pixel(img_out, x, y,
iround(c.r), iround(c.g), iround(c.b));
}
}
return img_out;
}
img_t *img_fix_radial_distortion(img_t *img, double k1, double k2, double f) {
int x, y, w = img->w, h = img->h;
img_t *Timg = img_new(w, h);
/* Map each pixel to
* x = x' / (1 + k1 * r^2 + k2 * r^4)
* y = y' / (1 + k1 * r^2 + k2 * r^4) */
for (y = 0; y < h; y++) {
for (x = 0; x < w; x++) {
double xf = ((double) x - 0.5 * img->w) / f;
double yf = ((double) y - 0.5 * img->h) / f;
double rsq = xf * xf + yf * yf;
double xt = xf * (1.0 + rsq * (k1 + rsq * k2));
double yt = yf * (1.0 + rsq * (k1 + rsq * k2));
fcolor_t c;
xt = f * xt + 0.5 * img->w;
yt = f * yt + 0.5 * img->h;
if (xt < 0.0 || yt < 0.0 || xt > w - 1 || yt > h - 1) {
img_set_pixel(Timg, x, y,
background_color.r, background_color.g, background_color.b);
} else {
c = pixel_lerp(img, xt, yt);
img_set_pixel(Timg, x, y, iround(c.r), iround(c.g), iround(c.b));
}
}
}
return Timg;
}
/* Produces an image of the chamfer metric between two images */
img_t *img_dmap_render(img_dmap_t *dmap) {
int w = dmap->w, h = dmap->h;
double *dists = dmap->dists;
img_t *img_out = img_new(w, h);
int idx, num_pixels = w * h;
double max_dist = 0.0;
double min_dist = DBL_MAX;
for (idx = 0; idx < num_pixels; idx++) {
if (dists[idx] == DBL_MAX)
continue;
if (dists[idx] > max_dist)
max_dist = dists[idx];
if (dists[idx] < min_dist)
min_dist = dists[idx];
}
/* Fill in the pixels */
for (idx = 0; idx < num_pixels; idx++) {
if (dists[idx] == DBL_MAX) {
img_out->pixels[idx].r = img_out->pixels[idx].g = 0;
img_out->pixels[idx].b = 255;
} else {
int c = (int) (256 * (dists[idx] - min_dist) / (max_dist - min_dist));
img_out->pixels[idx].r = img_out->pixels[idx].g = img_out->pixels[idx].b = c;
}
}
return img_out;
}
/* Produces an image of the flow map */
img_t *img_dmap_render_flow(img_dmap_t *dmap) {
int w = dmap->w, h = dmap->h;
double *dists = dmap->dists;
v2_t *nns = dmap->nns;
img_t *img_out = img_new(w, h);
int idx, x, y;
double max_dist = 0.0;
/* Fill in the pixels */
for (y = 0; y < h; y++) {
for (x = 0; x < w; x++) {
idx = y * dmap->w + x;
v2_t nn = nns[idx];
#define FACTOR 6.0
double dx = CLAMP(FACTOR * (Vx(nn) - x) + 127.0, 0.0, 255.0);
double dy = CLAMP(FACTOR * (Vy(nn) - y) + 127.0, 0.0, 255.0);
#undef FACTOR
if (dists[idx] == DBL_MAX) {
img_out->pixels[idx].r = img_out->pixels[idx].g = 0;
img_out->pixels[idx].b = 0;
} else {
img_out->pixels[idx].r = (int) rint(dx);
img_out->pixels[idx].g = (int) rint(dy);
img_out->pixels[idx].b = 0.0;
}
}
}
return img_out;
}
int write_data(const char* paths, const char* fname, unsigned char type) {
FILE* in = fopen(paths, "r");
FILE* out = fopen(fname, "wb");
char buff[FILENAME_MAX];
unsigned int num_paths;
unsigned int path_len;
unsigned int type_size;
void* val;
void (*func) (img_t*, void*);
img_t* img;
int i;
fscanf(in, "%u", &num_paths);
fscanf(in, "%u", &path_len);
path_len += 1;
fwrite(&num_paths, sizeof(unsigned int), 1, out);
fwrite(&type, sizeof(unsigned int), 1, out);
fwrite(&path_len, sizeof(unsigned int), 1, out);
switch (type) {
case VAL_AVG:
type_size = 1;
func = img_avg;
break;
case VAL_UINT_RGB:
type_size = 4;
func = img_uint;
case VAL_AVG_COL_4:
type_size = 12;
func = img_avg_col_4;
}
val = malloc(type_size);
/* Read the trailing newlinew after path len */
fgets(buff, FILENAME_MAX, in);
for (i = 0; i < num_paths; ++i) {
fgets(buff, path_len + 2, in);
buff[strlen(buff)-1] = 0;
buff[path_len] = 0;
printf("%s", buff);
fwrite(buff, sizeof(char), path_len, out);
img = img_new(buff);
func(img, val);
img_free(img);
fwrite(val, type_size, 1, out);
}
fclose(in);
fclose(out);
return 0;
}
void UndistortImage(const std::string &in,
const camera_params_t &camera,
const std::string &out)
{
printf("Undistorting image %s\n", in.c_str());
fflush(stdout);
img_t *img = LoadJPEG(in.c_str());
int w = img->w;
int h = img->h;
img_t *img_out = img_new(w, h);
double f2_inv = 1.0 / (camera.f * camera.f);
for (int y = 0; y < h; y++) {
for (int x = 0; x < w; x++) {
double x_c = x - 0.5 * w;
double y_c = y - 0.5 * h;
double r2 = (x_c * x_c + y_c * y_c) * f2_inv;
double factor = 1.0 + camera.k[0] * r2 + camera.k[1] * r2 * r2;
x_c *= factor;
y_c *= factor;
x_c += 0.5 * w;
y_c += 0.5 * h;
fcolor_t c;
if (x_c >= 0 && x_c < w - 1 && y_c >= 0 && y_c < h - 1) {
c = pixel_lerp(img, x_c, y_c);
} else {
c = fcolor_new(0.0, 0.0, 0.0);
}
img_set_pixel(img_out, x, y,
iround(c.r), iround(c.g), iround(c.b));
}
}
// img_write_bmp_file(img_out, (char *) out.c_str());
WriteJPEG(img_out, (char *) out.c_str());
img_free(img);
img_free(img_out);
}
static img_t *img_read_pgm(FILE *fp) {
int char1, char2, w, h, max, c1, c2, c3, x, y;
img_t *img;
char1 = fgetc(fp);
char2 = fgetc(fp);
skip_comments(fp);
c1 = fscanf(fp, "%d", &w);
skip_comments(fp);
c2 = fscanf(fp, "%d", &h);
skip_comments(fp);
c3 = fscanf(fp, "%d", &max);
printf("[pgm.c] (%d, %d, %d)\n", w, h, max);
if (char1 != 'P' || char2 != '5' ||
c1 != 1 || c2 != 1 || c3 != 1 || max > 255) {
printf("Input is not a standard raw 8-bit PGM file.\n"
"Use xv or pnmdepth to convert file to 8-bit PGM format.\n");
return NULL;
}
fgetc(fp); /* Discard exactly one byte after header. */
/* Create floating point image with pixels in range [0,1]. */
img = img_new(w, h);
for (y = h - 1; y > 0; y--) {
color_t *c = img->pixels + y * w;
for (x = 0; x < w; x++) {
int v = (int) fgetc(fp);
// img_set_pixel(img, x, y, v, v, v);
c->r = c->g = c->b = v;
img_set_valid_pixel(img, x, y);
c++;
}
}
return img;
}
image read_ppm(char *fn)
{
FILE *fp = fopen(fn, "rb");
int w, h, maxval;
image im = 0;
if (!fp) return 0;
if (fgetc(fp) != 'P' || fgetc(fp) != '6' || !isspace(fgetc(fp)))
goto bail;
w = read_num(fp);
h = read_num(fp);
maxval = read_num(fp);
if (!w || !h || !maxval) goto bail;
im = img_new(w, h);
fread(im->pix, 1, 3 * w * h, fp);
bail:
if (fp) fclose(fp);
return im;
}
/* Create a new image by applying transformation T to img and
* resampling */
img_t *img_resample(img_t *img, trans2D_t *T) {
int w = img->w, h = img->h;
int x, y;
trans2D_t *Tinv = transform_invert(T);
img_t *Timg = img_new(w, h);
for (y = 0; y < h; y++) {
for (x = 0; x < w; x++) {
double Tp[2];
fcolor_t c;
/* Invert the point (x, y) */
transform_point(Tinv, x, y, &Tp[0], &Tp[1]);
#if 1
/* Check if the result is in range */
if (Tp[0] < 0.0 || Tp[1] < 0.0 || Tp[0] > w - 1 || Tp[1] > h - 1) {
img_set_pixel(Timg, x, y,
background_color.r, background_color.g, background_color.b);
/* img_nullify_pixel(Timg, x, y); */
} else {
/* Apply bilinear interpolation */
c = pixel_lerp(img, Tp[0], Tp[1]);
img_set_pixel(Timg, x, y, iround(c.r), iround(c.g), iround(c.b));
}
#else
if (Tp[0] < 0.0) Tp[0] = 0.0;
else if (Tp[0] > w - 1) Tp[0] = w - 1;
if (Tp[1] < 0.0) Tp[1] = 0.0;
else if (Tp[1] > h - 1) Tp[1] = h - 1;
c = pixel_lerp(img, Tp[0], Tp[1]);
img_set_pixel(Timg, x, y, (int) rint(c.r), (int) rint(c.g), (int) rint(c.b));
#endif
}
}
return Timg;
}
int main(int argc, char** argv) {
if (argc < 5) {
printf("Usage: %s <image> <database> <x> <y>\n", argv[0]);
return 1;
}
int bounds[4];
unsigned int x, y;
x = atoi(argv[3]);
y = atoi(argv[4]);
img_t* img = img_new(argv[1]);
map_t* map = map_data(argv[2]);
img_t* edge = img_edge(img, 3*6*6);
img_fill_gaps(edge, 3);
img_find_bounds(edge, x, y, 50, 50, bounds);
img_t* sub = img_sub(img, bounds);
//img_flood_fill(edge, x, y);
img_to_bmp(edge, "/tmp/edge");
img_to_bmp(sub, "/tmp/sub");
return 0;
}
/* Create a new image by applying transformation T to img and
* resampling. Resize the image so that the whole thing fits when
* transformed. */
img_t *img_resample_bbox(img_t *img, trans2D_t *T) {
int w = img->w, h = img->h;
int x, y;
trans2D_t *Tinv = transform_invert(T);
int w_new, h_new, i;
// double x_min = DBL_MAX, x_max = -DBL_MAX, y_min = DBL_MAX, y_max = -DBL_MAX;
v2_t min = v2_new(DBL_MAX, DBL_MAX);
v2_t max = v2_new(-DBL_MAX, -DBL_MAX);
v2_t origin;
img_t *Timg;
/* Find the new dimensions of the window */
v2_t crs[4]; /* Four corners of the original image */
crs[0] = v2_new(0, 0);
crs[1] = v2_new(0, h - 1);
crs[2] = v2_new(w - 1, 0);
crs[3] = v2_new(w - 1, h - 1);
for (i = 0; i < 4; i++) {
crs[i] = v2_add(crs[i], img->origin);
crs[i] = transform_vector(T, crs[i]);
min = v2_minimum(min, crs[i]);
max = v2_maximum(max, crs[i]);
}
Vx(min) = floor(Vx(min));
Vy(min) = floor(Vy(min));
w_new = iround(floor(Vx(max) - Vx(min) + 1));
h_new = iround(floor(Vy(max) - Vy(min) + 1));
origin = min;
Timg = img_new(w_new, h_new);
Timg->origin = origin;
for (y = 0; y < h_new; y++) {
for (x = 0; x < w_new; x++) {
double Tp[2];
fcolor_t c;
/* Invert the point (x, y) - trans */
transform_point(Tinv, x + Vx(origin), y + Vy(origin), &Tp[0], &Tp[1]);
#if 1
/* Check if the result is in range */
if (Tp[0] < Vx(img->origin) || Tp[1] < Vy(img->origin) ||
Tp[0] > Vx(img->origin) + w - 1 || Tp[1] > Vy(img->origin) + h - 1) {
/* pass */
} else {
/* Check if the result is valid */
int x_f = (int) (Tp[0] - Vx(img->origin));
int x_c = x_f + 1;
int y_f = (int) (Tp[1] - Vy(img->origin));
int y_c = y_f + 1;
if (img_pixel_is_valid(img, x_f, y_f) ||
img_pixel_is_valid(img, x_c, y_f) ||
img_pixel_is_valid(img, x_f, y_c) ||
img_pixel_is_valid(img, x_c, y_c)) {
/* Apply bilinear interpolation */
c = pixel_lerp(img, Tp[0] - Vx(img->origin), Tp[1] - Vy(img->origin));
img_set_pixel(Timg, x, y, iround(c.r), iround(c.g), iround(c.b));
} else {
// img_nullify_pixel(Timg, x, y);
}
}
#else
if (Tp[0] < 0.0) Tp[0] = 0.0;
else if (Tp[0] > w - 1) Tp[0] = w - 1;
if (Tp[1] < 0.0) Tp[1] = 0.0;
else if (Tp[1] > h - 1) Tp[1] = h - 1;
c = pixel_lerp(img, Tp[0], Tp[1]);
img_set_pixel(Timg, x, y, c.r, c.g, c.b);
#endif
}
}
/* Add the old origin to the image */
// Timg->origin = v2_add(origin, img->origin);
transform_free(Tinv);
return Timg;
}
gboolean pipe_event(GIOChannel chan, GIOCondition cond, gpointer data) {
static char *path;
char name[TMPNAMELEN];
ssize_t rr;
int nimgs = 0;
if (!path)
path = xmalloc(strlen(tmpdir) + 64);
/* We are sent messages of size TMPNAMELEN containing a null-terminated
* file name. */
while (nimgs < 4 && (rr = xread(dpychld_fd, name, sizeof name)) == sizeof name) {
int saveimg = 0;
struct stat st;
++nimgs;
sprintf(path, "%s/%s", tmpdir, name);
if (stat(path, &st) == -1)
continue;
if (verbose)
fprintf(stderr, PROGNAME": received image %s of size %d\n", name, (int)st.st_size);
/* Check to see whether this looks like an image we're interested in. */
if (st.st_size > 100) {
/* Small images are probably bollocks. */
img i = img_new();
if (!img_load_file(i, path, header, unknown))
fprintf(stderr, PROGNAME": %s: bogus image (err = %d)\n", name, i->err);
else {
if (i->width > 8 && i->height > 8) {
if (img_load(i, full, i->type)) {
/* slot in the new image at some plausible place. */
int w, h;
if (i->width > width - 2 * BORDER) w = width - 2 * BORDER;
else w = i->width;
if (i->height > height - 2 * BORDER) h = height - 2 * BORDER;
else h = i->height;
/* is there space on this row? */
if (width - wrx < w) {
/* no */
scroll_backing_image(h + BORDER);
wrx = BORDER;
rowheight = h + BORDER;
}
if (rowheight < h + BORDER) {
scroll_backing_image(h + BORDER - rowheight);
rowheight = h + BORDER;
}
img_simple_blt(backing_image, wrx, wry - h, i, 0, 0, w, h);
add_image_rectangle(path, wrx, wry - h, w, h);
saveimg = 1;
if (beep)
write(1, "\a", 1);
update_window();
wrx += w + BORDER;
} else fprintf(stderr, PROGNAME": %s: bogus image (err = %d)\n", name, i->err);
} else if (verbose) fprintf(stderr, PROGNAME": %s: image dimensions (%d x %d) too small to bother with\n", name, i->width, i->height);
}
img_delete(i);
} else if (verbose) fprintf(stderr, PROGNAME": image data too small (%d bytes) to bother with\n", (int)st.st_size);
if (!saveimg)
unlink(name);
}
if (rr == -1 && errno != EINTR && errno != EAGAIN) {
perror(PROGNAME": read");
gtk_main_quit();
} else if (rr == 0) {
/* pipe closed, exit. */
gtk_main_quit();
}
return TRUE;
}
/* Produces an image of the disparity between two images */
img_t *img_dmap_render_disparity(img_dmap_t *dmap) {
int w = dmap->w, h = dmap->h;
double *dists = dmap->dists;
int x, y;
img_t *img_out = img_new(w, h);
int idx; // , num_pixels = w * h;
double max_dist = 0.0;
double min_dist = DBL_MAX;
idx = 0;
for (y = 0; y < dmap->h; y++) {
for (x = 0; x < dmap->w; x++) {
double dist;
double dx, dy;
v2_t nn;
if (dists[idx] == DBL_MAX) {
idx++;
continue;
}
nn = dmap->nns[idx];
dx = Vx(nn) - x;
dy = Vy(nn) - y;
// dist = sqrt(dx * dx + dy * dy);
dist = dx;
if (dist < min_dist)
min_dist = dist;
if (dist > max_dist)
max_dist = dist;
idx++;
}
}
printf("mindist = %0.3f\n", min_dist);
printf("maxdist = %0.3f\n", max_dist);
/* Fill in the pixels */
idx = 0;
for (y = 0; y < dmap->h; y++) {
for (x = 0; x < dmap->w; x++) {
if (dists[idx] == DBL_MAX) {
img_out->pixels[idx].r = img_out->pixels[idx].g = 0;
img_out->pixels[idx].b = 255;
} else {
v2_t nn = dmap->nns[idx];
double dx = Vx(nn) - x;
double dy = Vy(nn) - y;
// double dist = sqrt(dx * dx + dy * dy);
double dist = dx;
// int c = (int) (256 * sqrt(dist / max_dist));
int c = CLAMP((int) rint(1 * (-dist + 15)), 0, 255);
img_out->pixels[idx].r = img_out->pixels[idx].g = img_out->pixels[idx].b = c;
}
idx++;
}
}
return img_out;
}
