Skip to content

Commit

Permalink
Merged Jim's lens correction code into Fiducials.
Browse files Browse the repository at this point in the history
  • Loading branch information
@waynegramlich
waynegramlich committed Oct 6, 2013
1 parent a580bc5 commit 5a25410
Show file tree
Hide file tree
Showing 82 changed files with 153 additions and 49 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@ Map_Test
Tags
Video_Capture
calibrate
undistort
html
latex
ozcam
6 changes: 5 additions & 1 deletion Arc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,11 @@ Arc Arc__read(File in_file, Map map) {
void Arc__svg_write(Arc arc, SVG svg) {
Tag from_tag = arc->from_tag;
Tag to_tag = arc->to_tag;
SVG__line(svg, from_tag->x, from_tag->y, to_tag->x, to_tag->y, "black");
String color = "green";
if (arc->in_tree) {
color = "red";
}
SVG__line(svg, from_tag->x, from_tag->y, to_tag->x, to_tag->y, color);
}

/// @brief Updates the contenst of *arc*.
Expand Down
89 changes: 70 additions & 19 deletions CV.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -273,12 +273,6 @@ void CV__release_image(CV_Image image) {
Integer CV__inter_linear = CV_INTER_LINEAR;
Integer CV__warp_fill_outliers = CV_WARP_FILL_OUTLIERS;

void CV__remap(CV_Image source_image, CV_Image destination_image,
CV_Matrix mapx, CV_Matrix mapy, Integer flags, CV_Scalar fill_value) {
cvRemap(source_image,
destination_image, mapx, mapy, flags, *fill_value);
}

void CV__rodrigues2(
CV_Matrix rotation_vector, CV_Matrix rotation_matrix, CV_Matrix jacobian) {
cvRodrigues2(rotation_vector, rotation_matrix, jacobian);
Expand All @@ -301,6 +295,57 @@ Integer CV__round(Double value) {
return cvRound(value);
}

// Read the calibration file and generate the undistortion maps
// in:
// calibrate_file_name - camera calibration file
// w h - width and height of images to undistort
// out:
// mapx, mapy, - undistortion maps

Integer CV__undistortion_setup(String calibrate_file_name,
Integer width, Integer height, CV_Image *mapx, CV_Image *mapy) {
Double fcx, fcy, ccx, ccy;
Double kc[4];

// Open *calibrate_file_name*:
File file = File__open(calibrate_file_name, "r");
if (file == (File)0) {
File__format(stderr, "Could not open \"%s\"\n", calibrate_file_name);
return -1;
}

// Scan in the calibration values:
// format is fc - focal length, cc, principal point, kc distortion vector
int x = fscanf(file, "fc %lf %lf cc %lf %lf kc %lf %lf %lf %lf",
&fcx, &fcy, &ccx, &ccy, &kc[0], &kc[1], &kc[2], &kc[3]);
if (x != 8) {
File__format(stderr, "Expected 8 parameters got %d\n", x);
return -1;
}
File__close(file);

// Create *intrisic* matrix:
double intrinsic_vector[9] = {
fcx, 0, ccx,
0, fcy, ccy,
0, 0, 1
};
CvMat intrinsic = cvMat(3, 3, CV_64FC1, intrinsic_vector);
//printf("intrinsic matrix\n");
//dumpMat(&intrinsic);

// Create *distortion* matrix*:
CvMat distortion = cvMat(1, 4, CV_64FC1, kc);
//printf("distortion matrix\n");
//dumpMat(&distortion);

*mapx = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
*mapy = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
cvInitUndistortMap(&intrinsic, &distortion, *mapx, *mapy);

return 0;
}

// *CV_Image* routines:

void CV_Image__adaptive_threshold(CV_Image source_image,
Expand Down Expand Up @@ -489,6 +534,12 @@ void CV_Image__pnm_write(CV_Image image, String file_name) {
cvSaveImage(file_name, image, (Integer *)0);
}

void CV_Image__remap(CV_Image source_image, CV_Image destination_image,
CV_Image map_x, CV_Image map_y, Integer flags, CV_Scalar fill_value) {
cvRemap(source_image,
destination_image, map_x, map_y, flags, *fill_value);
}

void CV_Image__smooth(CV_Image source_image, CV_Image destination_image,
Integer smooth_type, Integer parameter1, Integer parameter2,
Double parameter3, Double parameter4) {
Expand All @@ -502,7 +553,18 @@ void CV_Image__store3(
pointer[channel] = (unsigned char)value;
}

CV_Image CV_Image__xtga_read(CV_Image image, String tga_file_name) {
/// @brief Read in a .tga file.
/// @param image to read into (or null).
/// @param tga_file_name is the file name of the .tga file.
/// @returns image from .tga file.
///
/// *CV__tga_read will read the contents of {tga_file_name} into
/// {image}. If the sizes do not match, {image} is released
/// and new {CV_Image} object of the right size is allocated,
/// filled and returned. In either case, the returned {CV_Image}
/// object containing the read in image data is returned.

CV_Image CV_Image__tga_read(CV_Image image, String tga_file_name) {
// Open *tga_in_file*:
File tga_in_file = File__open(tga_file_name, "rb");
if (tga_in_file == (File)0) {
Expand Down Expand Up @@ -611,7 +673,7 @@ CV_Image CV_Image__xtga_read(CV_Image image, String tga_file_name) {
/// *CV_Image__tga_write*() will write *image* out to *file_name* in
/// .tga format.

void CV_Image__xtga_write(CV_Image image, String file_name) {
void CV_Image__tga_write(CV_Image image, String file_name) {
Unsigned channels = (Unsigned)image->nChannels;
Unsigned depth = (Unsigned)image->depth;
Unsigned height = (Unsigned)image->height;
Expand Down Expand Up @@ -892,14 +954,3 @@ CV_Term_Criteria CV_Term_Criteria__create(
return term_criteria;
}

/// @brief Read in a .tga file.
/// @param image to read into (or null).
/// @param tga_file_name is the file name of the .tga file.
/// @returns image from .tga file.
///
/// *CV__tga_read will read the contents of {tga_file_name} into
/// {image}. If the sizes do not match, {image} is released
/// and new {CV_Image} object of the right size is allocated,
/// filled and returned. In either case, the returned {CV_Image}
/// object containing the read in image data is returned.

4 changes: 4 additions & 0 deletions CV.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@ extern Integer CV__thresh_binary;
extern Integer CV__window_auto_size;

extern Integer CV__round(Double value);
extern Integer CV__undistortion_setup(String calibrate_file_name,
Integer width, Integer height, CV_Image *mapx, CV_Image *mapy);

extern void CV_Image__adaptive_threshold(CV_Image source_image,
CV_Image destination_image, Double maximum_value, Integer adaptive_method,
Expand Down Expand Up @@ -72,6 +74,8 @@ extern Integer CV_Image__points_maximum(CV_Image image,
extern Integer CV_Image__points_minimum(CV_Image image,
CV_Point2D32F_Vector points, Unsigned start_index, Unsigned end_index);
extern Integer CV_Image__point_sample(CV_Image image, CV_Point2D32F point);
extern void CV_Image__remap(CV_Image source_image, CV_Image destination_image,
CV_Image map_x, CV_Image map_y, Integer flags, CV_Scalar fill_value);
extern Integer CV_Image__save(
CV_Image image, String file_name, Integer *parameters);
extern void CV_Image__smooth(CV_Image source_image, CV_Image destination_image,
Expand Down
24 changes: 17 additions & 7 deletions Demo.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#include "String.h"
#include "Unsigned.h"

Integer main(Unsigned argc, String argv[]) {
Integer main(Unsigned arguments_size, String arguments[]) {
struct timeval start_time_value_struct;
struct timeval end_time_value_struct;
struct timeval difference_time_value_struct;
Expand All @@ -27,12 +27,21 @@ Integer main(Unsigned argc, String argv[]) {
assert (gettimeofday(start_time_value, (struct timezone *)0) == 0);

List /* <String> */ image_file_names = List__new();
File__format(stdout, "Hello\n");
if (argc <= 1) {
File__format(stderr, "Usage: Demo *.pnm\n");
String lens_calibrate_file_name = (String)0;
//File__format(stdout, "Hello\n");
if (arguments_size <= 1) {
File__format(stderr, "Usage: Demo lens.txt *.pnm\n");
} else {
for (Unsigned index = 1; index < argc; index++) {
List__append(image_file_names, argv[index]);
for (Unsigned index = 1; index < arguments_size; index++) {
String argument = arguments[index];
Unsigned size = String__size(argument);
if (size > 4 && String__equal(argument + size - 4, ".txt")) {
lens_calibrate_file_name = argument;
} else if (size > 4 && String__equal(argument + size - 4, ".pnm")) {
List__append(image_file_names, argument);
} else {
File__format(stderr, "Unrecoginized file '%s'\n", argument);
}
}
}

Expand All @@ -42,7 +51,8 @@ Integer main(Unsigned argc, String argv[]) {
CV_Image image = (CV_Image)0;
image = CV_Image__pnm_read(image_file_name0);
assert (image != (CV_Image)0);
Fiducials fiducials = Fiducials__create(image);
Fiducials fiducials =
Fiducials__create(image, lens_calibrate_file_name);

for (Unsigned index = 0; index < size; index++) {
String image_file_name =
Expand Down
54 changes: 40 additions & 14 deletions Fiducials.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,8 @@ void Fiducials__image_show(Fiducials fiducials, Logical show) {
}
}

Fiducials Fiducials__create(CV_Image original_image) {
Fiducials Fiducials__create(
CV_Image original_image, String lens_calibrate_file_name) {
// Create *image_size*:
Unsigned width = CV_Image__width_get(original_image);
Unsigned height = CV_Image__height_get(original_image);
Expand Down Expand Up @@ -308,6 +309,13 @@ Fiducials Fiducials__create(CV_Image original_image) {
// File__format(stderr, "mappings[%d]=0x%x\n", index, mappings[index]);
//}

CV_Image map_x = (CV_Image)0;
CV_Image map_y = (CV_Image)0;
if (lens_calibrate_file_name != (String)0) {
assert (CV__undistortion_setup(
lens_calibrate_file_name, width, height, &map_x, &map_y) == 0);
}

// Create and load *fiducials*:
Fiducials fiducials = Memory__new(Fiducials);
fiducials->blue = CV_Scalar__rgb(0.0, 0.0, 1.0);
Expand All @@ -323,6 +331,8 @@ Fiducials Fiducials__create(CV_Image original_image) {
fiducials->gray_image = CV_Image__create(image_size, CV__depth_8u, 1);
fiducials->green = CV_Scalar__rgb(0.0, 255.0, 0.0);
fiducials->map = Map__new();
fiducials->map_x = map_x;
fiducials->map_y = map_y;
fiducials->mappings = &mappings[0];
fiducials->origin = CV_Point__create(0, 0);
fiducials->original_image = original_image;
Expand All @@ -333,6 +343,8 @@ Fiducials Fiducials__create(CV_Image original_image) {
fiducials->size_5x5 = CV_Size__create(5, 5);
fiducials->size_m1xm1 = CV_Size__create(-1, -1);
fiducials->storage = storage;
fiducials->temporary_gray_image =
CV_Image__create(image_size, CV__depth_8u, 1);
fiducials->term_criteria =
CV_Term_Criteria__create(term_criteria_type, 5, 0.2);
fiducials->y_flip = (Logical)0;
Expand All @@ -350,6 +362,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
Unsigned debug_index = fiducials->debug_index;
CV_Image edge_image = fiducials->edge_image;
CV_Image gray_image = fiducials->gray_image;
CV_Image temporary_gray_image = fiducials->temporary_gray_image;
CV_Image original_image = fiducials->original_image;

// For *debug_level* 0, we show the original image in color:
Expand Down Expand Up @@ -388,13 +401,26 @@ Unsigned Fiducials__process(Fiducials fiducials) {
CV_Image__convert_color(gray_image, debug_image, CV__gray_to_rgb);
}

// Preform undistort if available:
if (fiducials->map_x != (CV_Image)0) {
Integer flags = CV_INTER_NN | CV_WARP_FILL_OUTLIERS;
CV_Image__copy(gray_image, temporary_gray_image, (CV_Image)0);
CV_Image__remap(temporary_gray_image, gray_image,
fiducials->map_x, fiducials->map_y, flags, fiducials->black);
}

// Show results of undistort:
if (debug_index == 2) {
CV_Image__convert_color(gray_image, debug_image, CV__gray_to_rgb);
}

// Perform Gaussian blur if requested:
if (fiducials->blur) {
CV_Image__smooth(gray_image, gray_image, CV__gaussian, 3, 0, 0.0, 0.0);
}

// Show results of Gaussian blur for *debug_index* 2:
if (debug_index == 2) {
if (debug_index == 3) {
CV_Image__convert_color(gray_image, debug_image, CV__gray_to_rgb);
}

Expand All @@ -403,7 +429,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
CV__adaptive_thresh_gaussian_c, CV__thresh_binary, 45, 5.0);

// Show results of adaptive threshold for *debug_index* 3:
if (debug_index == 3) {
if (debug_index == 4) {
CV_Image__convert_color(edge_image, debug_image, CV__gray_to_rgb);
}

Expand All @@ -417,7 +443,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
}

// For *debug_index* 4, show the *edge_image* *contours*:
if (debug_index == 4) {
if (debug_index == 5) {
//File__format(stderr, "Draw red contours\n");
CV_Scalar red = fiducials->red;
CV_Image__convert_color(gray_image, debug_image, CV__gray_to_rgb);
Expand Down Expand Up @@ -450,7 +476,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
CV_Sequence polygon_contour =
CV_Sequence__approximate_polygon(contour,
header_size, storage, CV__poly_approx_dp, arc_length, 0.0);
if (debug_index == 5) {
if (debug_index == 6) {
//File__format(stderr, "Draw green contours\n");
CV_Scalar green = fiducials->green;
CV_Image__draw_contours(debug_image,
Expand All @@ -467,7 +493,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
//File__format(stderr, "Have 4 sides > 500i\n");

// Just show the fiducial outlines for *debug_index* of 6:
if (debug_index == 6) {
if (debug_index == 7) {
CV_Scalar red = fiducials->red;
CV_Image__draw_contours(debug_image,
polygon_contour, red, red, 2, 2, 1, origin);
Expand All @@ -482,7 +508,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
CV_Sequence__point_fetch1(polygon_contour, index);
CV_Point2D32F__point_set(corner, point);

if (debug_index == 6) {
if (debug_index == 7) {
//File__format(stderr,
// "point[%d] x:%f y:%f\n", index, point->x, point->y);
}
Expand All @@ -498,7 +524,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {

// For debugging show the 4 corners of the possible tag where
//corner0=red, corner1=green, corner2=blue, corner3=purple:
if (debug_index == 7) {
if (debug_index == 8) {
for (Unsigned index = 0; index < 4; index++) {
CV_Point point =
CV_Sequence__point_fetch1(polygon_contour, index);
Expand Down Expand Up @@ -551,7 +577,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
// For debugging, show the 8 points that are sampled around the
// the tag periphery to even decide whether to do further testing.
// Show "black" as green crosses, and "white" as green crosses:
if (debug_index == 8) {
if (debug_index == 9) {
CV_Scalar red = fiducials->red;
CV_Scalar green = fiducials->green;
for (Unsigned index = 0; index < 8; index++) {
Expand Down Expand Up @@ -592,7 +618,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
tag_bits[index] = bit;

// For debugging:
if (debug_index == 9) {
if (debug_index == 10) {
CV_Scalar red = fiducials->red;
CV_Scalar green = fiducials->green;
CV_Scalar cyan = fiducials->cyan;
Expand Down Expand Up @@ -656,7 +682,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
}
tag_bytes[i] = byte;
}
if (debug_index == 10) {
if (debug_index == 11) {
File__format(stderr, "dir=%d Tag[0]=0x%x Tag[1]=0x%x\n",
direction_index, tag_bytes[0], tag_bytes[1]);
}
Expand All @@ -665,7 +691,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
FEC fec = fiducials->fec;
if (FEC__correct(fec, tag_bytes, 8)) {
// We passed FEC:
if (debug_index == 10) {
if (debug_index == 11) {
File__format(stderr, "FEC correct\n");
}

Expand All @@ -679,7 +705,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {
Unsigned tag_id =
(tag_bytes[1] << 8) | tag_bytes[0];

if (debug_index == 10) {
if (debug_index == 11) {
File__format(stderr,
"CRC correct, Tag=%d\n", tag_id);
}
Expand All @@ -699,7 +725,7 @@ Unsigned Fiducials__process(Fiducials fiducials) {

// Load up *camera_tag* to get center, twist, etc.:
Tag tag = Map__tag_lookup(map, tag_id);
if (debug_index == 10) {
if (debug_index == 11) {
Camera_Tag__initialize(camera_tag, tag,
direction_index, corners, debug_image);
} else {
Expand Down
Loading

0 comments on commit 5a25410

Please sign in to comment.