CV-0.3.7: cbits/cvFiles.c
#include "cvFiles.h"
#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/highgui/highgui_c.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
typedef unsigned int uint_t;
const float epsilon = 0.001;
int *mask_idx;
int mask_size;
float calc_sum(float *pos, uint_t *count)
{
int *p, *e;
float s, v;
uint_t c;
c = 0;
s = 0;
e = &mask_idx[mask_size];
for (p = mask_idx; p < e; p++) {
v = *(pos + *p);
if (v > epsilon) {
s += v;
c++;
}
}
*count = c;
return s;
}
float calc_sum2(float *pos, uint_t *count)
{
int *p, *e;
float s, v;
uint_t c;
c = 0;
s = 0;
e = &mask_idx[mask_size];
for (p = mask_idx; p < e; p++) {
v = *(pos + *p);
if (v > epsilon) {
s += v*v;
c++;
}
}
*count = c;
return s;
}
float calc_sum9(float *pos, size_t stride)
{
float *pos1, *pos2, *pos3, *pos4, *pos5;
pos1 = pos - 1 * stride - 1;
pos2 = pos - 1;
pos3 = pos + stride - 1;
return
*pos1 + *(pos1+1) + *(pos1+2) +
*pos2 + *(pos2+1) + *(pos2+2) +
*pos3 + *(pos3+1) + *(pos3+2);
}
float calc_dx(float *pos)
{
float d, p1, p2, n1, n2;
p1 = *(pos + 1);
p2 = *(pos + 2);
n1 = *(pos - 1);
n2 = *(pos - 2);
d = 0;
if (p1 > epsilon && n1 > epsilon) {
d = p1 - n1;
}
if (p2 > epsilon && n2 > epsilon) {
d += (2*p2 - 2*n2);
}
return d;
}
float calc_dy(float *pos, uint_t stride)
{
float d, p1, p2, n1, n2;
p1 = *(pos + stride);
p2 = *(pos + 2 * stride);
n1 = *(pos - stride);
n2 = *(pos - 2 * stride);
d = 0;
if (p1 > epsilon && n1 > epsilon) {
d = p1 - n1;
}
if (p2 > epsilon && n2 > epsilon) {
d += (2*p2 - 2*n2);
}
return d;
}
#define C_PI 3.14159265358979323846
const float pi = C_PI;
const float dir_diff_small = C_PI / 8;
const float dir_diff_large = C_PI / 4;
float direction_difference(float a, float b)
{
float diff = a - b;
if (diff < 0) diff = -diff;
if (diff > pi) diff = 2*pi - diff;
return diff;
}
/*
bool is_same_direction(float a, float b, float diff)
{
float lower_bound, upper_bound;
lower_bound = a - diff;
upper_bound = b + diff;
if (lower_bound < -pi) {
return ((((lower_bound + 2*pi) < b) || -pi < b) && b < upper_bound)
}
else
if (upper_bound > pi) {
return (lower_bound < b && (b < pi || b < (upper_bound - 2*pi)));
}
else {
return (lower_bound < b && b < upper_bound);
}
return false;
}
*/
float calc_directionality_1(float *pos, uint_t stride)
{
float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;
result = 0;
dir_c = *pos;
if (direction_difference(dir_c, pi) < dir_diff_small || direction_difference(dir_c, 0) < dir_diff_small) {
dir_a = *(pos - stride);
dir_b = *(pos + stride);
value = *(pos - stride - 1);
dir_diff_s1 = direction_difference(dir_c, value);
value = *(pos - 1);
dir_diff_s1 += direction_difference(dir_c, value);
value = (*pos + stride - 1);
dir_diff_s1 += direction_difference(dir_c, value);
dir_diff_s1 /= 3;
value = *(pos - stride + 1);
dir_diff_s2 = direction_difference(dir_c, value);
value = *(pos + 1);
dir_diff_s2 += direction_difference(dir_c, value);
value = *(pos + stride + 1);
dir_diff_s2 += direction_difference(dir_c, value);
dir_diff_s2 /= 3;
if (dir_diff_s1 > dir_diff_s2) {
diff_s = dir_diff_s1;
}
else {
diff_s = dir_diff_s2;
}
if (diff_s > dir_diff_small) {
diff_c = direction_difference(dir_c, dir_a);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
diff_c = direction_difference(dir_c, dir_b);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
}
}
return result;
}
float calc_directionality_2(float *pos, uint_t stride)
{
float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;
result = 0;
dir_c = *pos;
if (direction_difference(dir_c, 3*pi/4) < dir_diff_small || direction_difference(dir_c, -pi/4) < dir_diff_small) {
dir_a = *(pos - stride + 1);
dir_b = *(pos + stride - 1);
value = *(pos - stride - 1);
dir_diff_s1 = direction_difference(dir_c, value);
value = *(pos - stride);
dir_diff_s1 += direction_difference(dir_c, value);
value = (*pos - 1);
dir_diff_s1 += direction_difference(dir_c, value);
dir_diff_s1 /= 3;
value = *(pos + stride + 1);
dir_diff_s2 = direction_difference(dir_c, value);
value = *(pos + stride);
dir_diff_s2 += direction_difference(dir_c, value);
value = *(pos + 1);
dir_diff_s2 += direction_difference(dir_c, value);
dir_diff_s2 /= 3;
if (dir_diff_s1 > dir_diff_s2) {
diff_s = dir_diff_s1;
}
else {
diff_s = dir_diff_s2;
}
if (diff_s > dir_diff_small) {
diff_c = direction_difference(dir_c, dir_a);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
diff_c = direction_difference(dir_c, dir_b);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
}
}
return result;
}
float calc_directionality_3(float *pos, uint_t stride)
{
float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;
result = 0;
dir_c = *pos;
if (direction_difference(dir_c, pi/2) < dir_diff_small || direction_difference(dir_c, -pi/2) < dir_diff_small) {
dir_a = *(pos + 1);
dir_b = *(pos - 1);
value = *(pos - stride - 1);
dir_diff_s1 = direction_difference(dir_c, value);
value = *(pos - stride);
dir_diff_s1 += direction_difference(dir_c, value);
value = (*pos - stride + 1);
dir_diff_s1 += direction_difference(dir_c, value);
dir_diff_s1 /= 3;
value = *(pos + stride - 1);
dir_diff_s2 = direction_difference(dir_c, value);
value = *(pos + stride);
dir_diff_s2 += direction_difference(dir_c, value);
value = *(pos + stride + 1);
dir_diff_s2 += direction_difference(dir_c, value);
dir_diff_s2 /= 3;
if (dir_diff_s1 > dir_diff_s2) {
diff_s = dir_diff_s1;
}
else {
diff_s = dir_diff_s2;
}
if (diff_s > dir_diff_small) {
diff_c = direction_difference(dir_c, dir_a);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
diff_c = direction_difference(dir_c, dir_b);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
}
}
return result;
}
float calc_directionality_4(float *pos, uint_t stride)
{
float result, dir_c, dir_a, dir_b, dir_diff_s1, dir_diff_s2, value, diff_s, diff_c;
result = 0;
dir_c = *pos;
if (direction_difference(dir_c, pi/4) < dir_diff_small || direction_difference(dir_c, -3*pi/4) < dir_diff_small) {
dir_a = *(pos + stride + 1);
dir_b = *(pos - stride - 1);
value = *(pos - stride);
dir_diff_s1 = direction_difference(dir_c, value);
value = *(pos - stride + 1);
dir_diff_s1 += direction_difference(dir_c, value);
value = (*pos + 1);
dir_diff_s1 += direction_difference(dir_c, value);
dir_diff_s1 /= 3;
value = *(pos - 1);
dir_diff_s2 = direction_difference(dir_c, value);
value = *(pos + stride - 1);
dir_diff_s2 += direction_difference(dir_c, value);
value = *(pos + stride);
dir_diff_s2 += direction_difference(dir_c, value);
dir_diff_s2 /= 3;
if (dir_diff_s1 > dir_diff_s2) {
diff_s = dir_diff_s1;
}
else {
diff_s = dir_diff_s2;
}
if (diff_s > dir_diff_small) {
diff_c = direction_difference(dir_c, dir_a);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
diff_c = direction_difference(dir_c, dir_b);
if (diff_c < dir_diff_small) {
result += 1.0;
}
else
if (diff_c < diff_s) {
result += 0.5;
}
}
}
return result;
}
IplImage *read_from_tcr(const char *path, uint_t **timestamp_array)
{
IplImage *dst;
float *dst_data, *dst_pos;
uint_t dst_stride, *timestamps, timestamp;
size_t width, height, file_size, read_size, line_size, timestamp_size;
size_t line_count, timestamp_count;
CvSize size;
FILE *input_file;
width = 2592;
line_size = width * sizeof(float);
timestamp_size = sizeof(uint_t);
dst = NULL;
input_file = fopen(path, "rb");
if (input_file == NULL) {
printf("Error: unable to open file %s\n", path);
return NULL;
}
/* seek file end to determine image size */
fseek(input_file , 0 , SEEK_END);
file_size = ftell(input_file);
rewind(input_file);
height = (size_t)(file_size / (line_size + timestamp_size));
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
if (dst == NULL) {
printf("Error: unable to create image\n");
return NULL;
}
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
if (timestamp_array != NULL) {
timestamps = (uint_t *)malloc(height * timestamp_size);
if (timestamps == NULL) {
printf("Error: unable to allocate timestamp array\n");
return NULL;
}
*timestamp_array = timestamps;
}
else {
timestamps = NULL;
}
line_count = 0;
timestamp_count = 0;
dst_pos = dst_data;
while (1) {
/* read line timestamp to use for rectification */
read_size = fread((void*)×tamp, timestamp_size, 1, input_file);
if (read_size < 1) {
if (line_count < height) {
printf("Error: unexpected end of file after line %d\n", line_count);
return dst;
}
break;
}
else {
if (timestamps != NULL) {
timestamps[timestamp_count++] = timestamp;
}
}
/* read data line */
read_size = fread((void*)dst_pos, sizeof(float), width, input_file);
if (read_size < width) {
printf("Error unexpected end of file after line %d\n", line_count);
return dst;
}
else {
line_count += 1;
dst_pos += dst_stride;
}
}
fclose(input_file);
return dst;
}
IplImage *rectify_tcr(IplImage *src, uint_t *timestamps, uint_t startx, uint_t endx, uint_t starty, uint_t endy)
{
IplImage *dst;
float *src_data, *dst_data, *src_pos, *dst_pos, tratio;
uint_t src_stride, dst_stride, tstart, tend, tdiff, *timestamps_accum;
size_t height, new_width, new_height, row_size;
int i, j, result;
CvSize size;
new_width = endx - startx;
new_height = new_width;
tstart = timestamps[starty];
tend = timestamps[endy];
tdiff = tend - tstart;
tratio = (float)tdiff / (float)new_height;
/* create a lookup table for rectified image time stamps */
timestamps_accum = (uint_t *)malloc(new_height * sizeof(uint_t));
if (timestamps_accum == NULL) {
printf("Error: rectify_tcr: unable to allocate cumulative timestamp array\n");
return NULL;
}
timestamps_accum[0] = tstart;
for (i = 1; i < new_height; i++) {
timestamps_accum[i] = tstart + (uint_t)(tratio * i);
}
size.width = new_width;
size.height = new_height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
if (dst == NULL) {
printf("Error: rectify_tcr: unable to create image\n");
return NULL;
}
height = src->height;
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
src_pos = src_data + starty * src_stride + startx;
j = starty;
row_size = new_width * sizeof(float);
dst_pos = dst_data;
for (i = 0; i < new_height; i++, dst_pos += dst_stride) {
if (timestamps[j] < timestamps_accum[i]) {
j++;
if (j == height) {
printf("Error: rectify_tcr: row overflow\n");
break;
}
src_pos += src_stride;
}
memcpy(dst_pos, src_pos, row_size);
}
free(timestamps_accum);
return dst;
}
IplImage *derivative_direction(IplImage *src, float ignore_val, float ignore_eps)
{
IplImage *dst, *tr, *ta;
float *src_data, *src_pos, *dst_data, *dst_pos, *tr_data, *tr_pos, *ta_data, *ta_pos;
float dx, dy, r, a, asum, amean;
uint_t src_stride, dst_stride, tr_stride, ta_stride, width, height;
CvSize size;
int x, y;
width = src->width;
height = src->height;
size.width = width;
size.height = height;
tr = cvCreateImage(size, IPL_DEPTH_32F, 1);
ta = cvCreateImage(size, IPL_DEPTH_32F, 1);
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(tr, cvScalarAll(0), NULL);
cvSet(ta, cvScalarAll(0), NULL);
cvSet(dst, cvScalarAll(0), NULL);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
tr_data = (float*)tr->imageData;
tr_stride = (uint_t)(tr->widthStep / sizeof(float));
ta_data = (float*)ta->imageData;
ta_stride = (uint_t)(ta->widthStep / sizeof(float));
for (y = 2; y < height - 2; y++) {
src_pos = src_data + y * src_stride + 2;
tr_pos = tr_data + y * tr_stride + 2;
ta_pos = ta_data + y * ta_stride + 2;
for (x = 2; x < width - 2; x++, src_pos++, tr_pos++, ta_pos++) {
dx = calc_dx(src_pos);
dy = calc_dy(src_pos, src_stride);
r = sqrt(dx*dx+dy*dy);
if (dx == 0 && dy == 0) {
a = 0;
}
else {
a = atan2(dy,dx);
}
*tr_pos = r;
*ta_pos = a;
}
}
for (y = 2; y < height - 2; y++) {
dst_pos = dst_data + y * dst_stride + 2;
tr_pos = tr_data + y * tr_stride + 2;
ta_pos = ta_data + y * ta_stride + 2;
for (x = 2; x < width - 2; x++, dst_pos++, tr_pos++, ta_pos++) {
a = calc_directionality_1(ta_pos, ta_stride) +
0.5 * calc_directionality_2(ta_pos, ta_stride) +
calc_directionality_3(ta_pos, ta_stride) +
0.5 * calc_directionality_4(ta_pos, ta_stride);
/*
asum = calc_sum9(ta_pos, ta_stride);
amean = asum / 9;
*/
/*r = sqrt(dx*dx+dy*dy);*/
/*
if (abs(a - amean) > 0.3) {
*dst_pos = a;
}
else {
*dst_pos = 0;
}
*/
*dst_pos = a;
}
}
cvReleaseImage(&tr);
cvReleaseImage(&ta);
return dst;
}
IplImage *mean_fill_holes(IplImage *src, int r, float ignore_val, float ignore_eps)
{
IplImage *dst;
float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
uint_t src_stride, dst_stride, width, height, sum_count, min_count;
CvSize size;
int x, y, p, d;/*, mask_size, *mask_idx;*/
width = src->width;
height = src->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(dst, cvScalarAll(ignore_val), NULL);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
d = (2*r+1);
/* create the mask offset table for the averaging filter mask */
mask_size = d*d;
mask_idx = malloc(mask_size * sizeof(int));
for (y = -r, p = 0; y <= r; y++) {
for (x = -r; x <= r; x++) {
mask_idx[p++] = y * src_stride + x;
}
}
min_count = (uint_t)(mask_size / 2);
for (y = r; y < height - r; y++) {
src_pos = src_data + y * src_stride + r;
dst_pos = dst_data + y * dst_stride + r;
for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
value = *src_pos;
if ((ignore_val - ignore_eps) < value && value < (ignore_val + ignore_eps)) {
sum = calc_sum(src_pos, &sum_count);
if (sum_count > min_count) {
value = (sum/sum_count);
}
}
*dst_pos = value;
}
}
free(mask_idx);
mask_idx = NULL;
return dst;
}
IplImage *filter_mean(IplImage *src, int r, float ignore_val, float ignore_eps)
{
IplImage *dst;
float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
uint_t src_stride, dst_stride, width, height, sum_count, min_count;
CvSize size;
int x, y, p, d;/*, mask_size, *mask_idx;*/
width = src->width;
height = src->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(dst, cvScalarAll(ignore_val), NULL);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
d = (2*r+1);
/* create the mask offset table for the averaging filter mask */
mask_size = d*d;
mask_idx = malloc(mask_size * sizeof(int));
for (y = -r, p = 0; y <= r; y++) {
for (x = -r; x <= r; x++) {
mask_idx[p++] = y * src_stride + x;
}
}
min_count = (uint_t)(mask_size / 2);
for (y = r; y < height - r; y++) {
src_pos = src_data + y * src_stride + r;
dst_pos = dst_data + y * dst_stride + r;
for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
value = *src_pos;
sum = calc_sum(src_pos, &sum_count);
if (sum_count > min_count) {
*dst_pos = (sum / sum_count);
}
else {
*dst_pos = ignore_val;
}
}
}
free(mask_idx);
mask_idx = NULL;
return dst;
}
IplImage *image_diff(IplImage *src1, IplImage *src2, float ignore_val, float ignore_eps)
{
IplImage *dst;
float *src1_data, *src2_data, *dst_data, *src1_pos, *src2_pos, *dst_pos, value, diff;
uint_t src1_stride, src2_stride, dst_stride, width, height;
CvSize size;
int x, y;
width = src1->width;
height = src1->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(dst, cvScalarAll(ignore_val), NULL);
src1_data = (float*)src1->imageData;
src1_stride = (uint_t)(src1->widthStep / sizeof(float));
src2_data = (float*)src2->imageData;
src2_stride = (uint_t)(src2->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
for (y = 0; y < height; y++) {
src1_pos = src1_data + y * src1_stride;
src2_pos = src2_data + y * src2_stride;
dst_pos = dst_data + y * dst_stride;
for (x = 0; x < width; x++, src1_pos++, src2_pos++, dst_pos++) {
value = *src1_pos;
if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
diff = abs(value - *src2_pos);
*dst_pos = diff;
}
else {
*dst_pos = 0;
}
}
}
return dst;
}
IplImage *abs_diff_mean(IplImage *src, float ignore_val, float ignore_eps)
{
IplImage *dst;
float *src_data, *dst_data, *src_pos, *dst_pos;
float value, sum, count, avg;
uint_t src_stride, dst_stride, width, height;
CvSize size;
int x, y;
width = src->width;
height = src->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(dst, cvScalarAll(0), NULL);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
sum = 0;
count = 0;
for (y = 0; y < height; y++) {
src_pos = src_data + y * src_stride;
for (x = 0; x < width; x++, src_pos++) {
value = *src_pos;
if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
sum += value;
count += 1;
}
}
}
avg = sum / count;
for (y = 0; y < height; y++) {
src_pos = src_data + y * src_stride;
dst_pos = dst_data + y * dst_stride;
for (x = 0; x < width; x++, src_pos++, dst_pos++) {
value = *src_pos;
if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
*dst_pos = abs(value - avg);
}
}
}
return dst;
}
IplImage *box_mean_abs_diff(IplImage *src, int r, float ignore_val, float ignore_eps)
{
IplImage *dst;
float *src_data, *src_pos, *dst_data, *dst_pos, value, sum;
uint_t src_stride, dst_stride, width, height, sum_count, min_count;
CvSize size;
int x, y, p, d;/*, mask_size, *mask_idx;*/
width = src->width;
height = src->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
cvSet(dst, cvScalarAll(ignore_val), NULL);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (float*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(float));
d = (2*r+1);
/* create the mask offset table for the averaging filter mask */
mask_size = d*d;
mask_idx = malloc(mask_size * sizeof(int));
for (y = -r, p = 0; y <= r; y++) {
for (x = -r; x <= r; x++) {
mask_idx[p++] = y * src_stride + x;
}
}
min_count = (uint_t)(mask_size / 2);
for (y = r; y < height - r; y++) {
src_pos = src_data + y * src_stride + r;
dst_pos = dst_data + y * dst_stride + r;
for (x = r; x < width - r; x++, src_pos++, dst_pos++) {
value = *src_pos;
sum = calc_sum(src_pos, &sum_count);
if (sum_count > min_count) {
if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
value = abs((sum/sum_count) - value);
}
else {
value = ignore_val;
}
*dst_pos = value;
}
else {
*dst_pos = ignore_val;
}
}
}
free(mask_idx);
mask_idx = NULL;
return dst;
}
void stretch_histogram_avg_sdv(IplImage *img, float ignore_val, float ignore_eps)
{
float *img_data, *img_pos;
uint_t img_stride, x, y, width, height;
float value, min, max, sum1, sum2, count, avg, sdv;
float bound_1, bound_2, stretch_1, stretch_2, stretch_3, stretch_4;
width = img->width;
height = img->height;
img_data = (float*)img->imageData;
img_stride = (uint_t)(img->widthStep / sizeof(float));
min = 2000000000;
max = 0;
count = 0;
sum1 = 0;
sum2 = 0;
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
if (value < min) min = value; else if (value > max) max = value;
sum1 += value;
sum2 += value*value;
count += 1;
}
}
}
avg = sum1 / count;
sdv = sqrt(sum2 / count - avg*avg);
printf("min=%f max=%f avg=%f sdv=%f\n", min, max, avg, sdv);
if ((avg - 2 * sdv) > min) {
min = avg - 2 * sdv;
}
if ((avg + 2 * sdv) < max) {
max = avg + 2 * sdv;
}
if (avg - sdv < min) {
bound_1 = min + 0.32 * (avg-min);
}
else {
bound_1 = avg - sdv;
}
if (avg + sdv > max) {
bound_2 = max - 0.32 * (max-avg);
}
else {
bound_2 = avg + sdv;
}
stretch_1 = 0.25 / (bound_1 - min);
stretch_2 = 0.25 / (avg - bound_1);
stretch_3 = 0.25 / (bound_2 - avg);
stretch_4 = 0.25 / (max - bound_2);
/* normalize values and create the histogram */
printf("b1=%f b2=%f b3=%f b4=%f b5=%f s1=%f s2=%f s3=%f s4=%f\n", min, bound_1, avg, bound_2, max, stretch_1, stretch_2, stretch_3, stretch_4);
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if ((ignore_val - ignore_eps) < value && value < (ignore_val + ignore_eps)) {
value = 0;
}
else
if (value < min) {
value = 0;
}
else
if (value < bound_1) {
value = stretch_1 * (value - min);
}
else
if (value < avg) {
value = 0.25 + stretch_2 * (value - bound_1);
}
else
if (value < bound_2) {
value = 0.5 + stretch_3 * (value - avg);
}
else
if (value < max) {
value = 0.75 + stretch_4 * (value - bound_2);
}
else {
value = 1;
}
*img_pos = value;
}
}
}
void equalize_histogram(IplImage *img, float ignore_val, float ignore_eps)
{
float *img_data, *img_pos;
uint_t img_stride, i, x, y, width, height, hidx;
float value, count, min, max, stretch;
float hist[256];
width = img->width;
height = img->height;
img_data = (float*)img->imageData;
img_stride = (uint_t)(img->widthStep / sizeof(float));
min = 2000000000;
max = 0;
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
if (value < min) min = value; else if (value > max) max = value;
}
}
}
stretch = 1.0 / (max - min);
printf("min=%f max=%f stretch=%f\n", min, max, stretch);
/* normalize values and create the histogram */
for (i = 0; i < 256; i++) {
hist[i] = 0;
}
count = 0;
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if (value > ignore_val + ignore_eps || value < ignore_val - ignore_eps) {
value = stretch * (value - min);
if (value < 0) value = 0; else if (value > 1) value = 1;
hidx = (int)(255 * value);
if (hidx < 0) hidx = 0; else if (hidx > 255) hidx = 255;
hist[hidx] += 1;
count += 1;
}
else {
value = 0;
}
*img_pos = value;
}
}
/* transform the histogram into a cumulative distribution */
printf("%f ", hist[0]);
hist[0] /= count;
for (i = 1; i < 256; i++) {
/*printf("(%.2f,", hist[i]);*/
hist[i] = (hist[i] / count) + hist[i-1];
/*printf("%.2f) ", hist[i]);*/
}
/*printf("\n");*/
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
hidx = (int)(255 * value);
if (hidx < 0) hidx = 0; else if (hidx > 255) hidx = 255;
value = (hist[hidx] * value);
if (value < 0) value = 0; else if (value > 1) value = 1;
*img_pos = value;
}
}
}
void stretch_histogram(IplImage *img, float ignore_val, float ignore_eps)
{
float *img_data, *img_pos;
uint_t img_stride, x, y, width, height, count;
float value, min, max, stretch;
width = img->width;
height = img->height;
img_data = (float*)img->imageData;
img_stride = (uint_t)(img->widthStep / sizeof(float));
min = 2000000000;
max = 0;
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
if (value < min) min = value; else if (value > max) max = value;
}
}
}
stretch = 1.0 / (max - min);
printf("min=%f max=%f stretch=%f\n", min, max, stretch);
for (y = 0; y < height; y++) {
img_pos = img_data + y * img_stride;
for (x = 0; x < width; x++, img_pos++) {
value = *img_pos;
if (value > (ignore_val + ignore_eps) || value < (ignore_val - ignore_eps)) {
value = stretch * (value - min);
if (value < 0) value = 0; else if (value > 1) value = 1;
}
else {
value = 0;
}
*img_pos = value;
}
}
}
IplImage *to_8bit(IplImage *src)
{
IplImage *dst;
float *src_data, *src_pos, value;
unsigned char *dst_data, *dst_pos;
uint_t x, y, width, height, src_stride, dst_stride;
int temp;
CvSize size;
width = src->width;
height = src->height;
size.width = width;
size.height = height;
dst = cvCreateImage(size, IPL_DEPTH_8U, 1);
src_data = (float*)src->imageData;
src_stride = (uint_t)(src->widthStep / sizeof(float));
dst_data = (unsigned char*)dst->imageData;
dst_stride = (uint_t)(dst->widthStep / sizeof(unsigned char));
for (y = 0; y < height; y++) {
src_pos = src_data + y * src_stride;
dst_pos = dst_data + y * dst_stride;
for (x = 0; x < width; x++, src_pos++, dst_pos++) {
value = *src_pos;
temp = (int)(255 * value);
if (temp < 0) temp = 0;
else if (temp > 255) temp = 255;
*dst_pos = (unsigned char)temp;
}
}
return dst;
}
IplImage *read_from_tcr_mag(const char *path)
{
IplImage *src, *tmp1, *tmp2, *dst;
uint_t *timestamps;
printf("read\n");
src = read_from_tcr(path, ×tamps);
printf("filter\n");
tmp1 = filter_mean(src, 3, 0, 0.001);
cvReleaseImage(&src);
printf("derivate\n");
tmp2 = derivative_direction(tmp1, 0, 0.001);
cvReleaseImage(&tmp1);
printf("equalize\n");
equalize_histogram(tmp2, 0, 0.001);
printf("convert\n");
dst = to_8bit(tmp2);
cvReleaseImage(&tmp2);
cvSaveImage("tcr_mag.png", dst, 0);
return dst;
}
IplImage *calc_derivative_direction(IplImage *src)
{
IplImage *dst;
dst = derivative_direction(src, 0, 0.001);
equalize_histogram(dst, 0, 0.001);
return dst;
}
IplImage *read_from_tcr_rectified(const char *path)
{
IplImage *src, *tmp1, *tmp2, *dst;
float *tmp_data, *tmp_pos, value;
uint_t *timestamps, tmp_stride, width, height, x, y, r;
uint_t minx, maxx, startx, endx, miny, maxy, starty, endy;
src = read_from_tcr(path, ×tamps);
if (src == NULL || timestamps == NULL) {
return NULL;
}
/*src = mean_fill_holes(tmp, 3, 0, 0.001);*/
/*cvReleaseImage(&tmp);*/
/*tmp1 = filter_mean(src, 5, 0, 0.001);*/
/*cvSaveImage("tcr_mean.png", tmp, 0);*/
/*tmp2 = image_diff(src, tmp1, 0, 0.001);*/
/*tmp2 = abs_diff_mean(tmp1, 0, 0.001);*/
/*cvSaveImage("tcr_diff.png", dst, 0);*/
tmp1 = box_mean_abs_diff(src, 5, 0, 0.001);
cvReleaseImage(&src);
/*cvReleaseImage(&tmp1);*/
stretch_histogram_avg_sdv(tmp1, 0, 0.001);
/*equalize_histogram(tmp2, 0, 0.001);*/
/*stretch_histogram(tmp2, 0, 0.001);*/
width = tmp1->width;
height = tmp1->height;
tmp_data = (float*)tmp1->imageData;
tmp_stride = (uint_t)(tmp1->widthStep / sizeof(float));
minx = 2000000000;
maxx = 0;
miny = 2000000000;
maxy = 0;
for (y = 0; y < height; y++) {
tmp_pos = tmp_data + y * tmp_stride;
for (x = 0; x < width; x++, tmp_pos++) {
value = *tmp_pos;
if (value > 0.001) {
if (x < minx) minx = x; else if (x > maxx) maxx = x;
if (y < miny) miny = y; else if (y > maxy) maxy = y;
}
}
}
printf("minx=%d maxx=%d miny=%d maxy=%d\n", minx, maxx, miny, maxy);
startx = 0;
if (minx > 2) startx = minx - 2;
endx = width - 1;
if (maxx < endx - 2) endx = maxx + 2;
starty = 0;
if (miny > 2) starty = miny - 2;
endy = height - 1;
if (maxy < endy - 2) endy = maxy + 2;
tmp2 = rectify_tcr(tmp1, timestamps, startx, endx, starty, endy);
cvReleaseImage(&tmp1);
dst = to_8bit(tmp2);
cvReleaseImage(&tmp2);
cvSaveImage("tcr_eq.png", dst, 0);
free(timestamps);
return dst;
}
IplImage *read_from_tcr_rectified_old(const char *path)
{
IplImage *src, *tmp, *tmp2, *tmp3, *tmp4, *dst;
float *src_data, *tmp_data, *dst_data, *src_pos, *tmp_pos, *dst_pos;
int src_stride, tmp_stride, dst_stride;
unsigned int *timestamps, *timestamps_accum;
unsigned int timestamp, timestamp_count, tstart, tend, tdiff;
float tratio;
CvSize size;
size_t width, height, read_size;
int line_width, timestamp_width, line_count, file_size;
FILE *input_file;
/* histogram for grayscale equalization */
float hist[256];
width = 2592;
line_width = width * sizeof(float);
timestamp_width = sizeof(unsigned int);
src = NULL;
tmp = NULL;
dst = NULL;
input_file = fopen(path, "rb");
if (input_file != NULL) {
/* seek file end to determine image size */
fseek (input_file , 0 , SEEK_END);
file_size = ftell(input_file);
rewind (input_file);
height = (size_t)(file_size / (line_width + timestamp_width));
/*printf("File size: %d, rows: %d\n", file_size, height);*/
size.width = width;
size.height = height;
src = cvCreateImage(size, IPL_DEPTH_32F, 1);
tmp = cvCreateImage(size, IPL_DEPTH_32F, 1);
tmp2 = cvCreateImage(size, IPL_DEPTH_32F, 1);
tmp3 = cvCreateImage(size, IPL_DEPTH_32F, 1);
tmp4 = cvCreateImage(size, IPL_DEPTH_32F, 1);
src_data = (float*)src->imageData;
tmp_data = (float*)tmp->imageData;
src_stride = (int)(src->widthStep / sizeof(float));
tmp_stride = (int)(tmp->widthStep / sizeof(float));
cvSet(tmp, cvScalarAll(0), NULL);
uint_t r = 2;
uint_t d = (2*r+1);
/* create the mask offset table for the averaging filter mask */
mask_size = d*d;
mask_idx = malloc(mask_size * sizeof(int));
for (int i = -r, pos = 0; i <= r; i++) {
for (int j = -r; j <= r; j++) {
mask_idx[pos++] = i * src_stride + j;
/*printf("%d ", mask_idx[pos-1]);*/
}
}
/*printf("\n");*/
timestamps = (unsigned int *)malloc(height * timestamp_width);
if (src_data != NULL) {
line_count = 0;
timestamp_count = 0;
src_pos = src_data;
while (1) {
/* read line timestamp to use for rectification */
read_size = fread((void*)×tamp, timestamp_width, 1, input_file);
if (read_size < 1) {
break;
}
else {
timestamps[timestamp_count++] = timestamp;
}
/* read data line */
read_size = fread((void*)src_pos, sizeof(float), width, input_file);
if (read_size < width) {
break;
}
else {
line_count += 1;
src_pos += src_stride;
}
}
fclose(input_file);
/*cvSaveImage("tcr_raw.png", src, 0);*/
/*
cvSmooth(src,tmp,CV_GAUSSIAN,5,5,0,0);
cvLaplace(tmp,tmp2,5);
*/
/*
cvSmooth(src,tmp,CV_BLUR,5,5,0,0);
cvSmooth(src,tmp2,CV_GAUSSIAN,3,3,0,0);
cvAbsDiff(tmp,src,tmp3);
cvSaveImage("tcr_diff.png", tmp3, 0);
cvSaveImage("tcr_smooth2.png", tmp2, 0);
*/
/*
cvSaveImage("tcr_laplacian5.png", tmp2, 0);
*/
{
unsigned int x, y, minx, maxx, miny, maxy, count;
unsigned int new_width, new_height, startx, endx, starty, endy;
float value, sum, sum1, sum2, min, max, ratio, stretch, avg, sdv;
float sum_count, min_count, stretch_1, stretch_2;
min = 2000000000;
max = 0;
minx = 2000000000;
maxx = 0;
miny = 2000000000;
maxy = 0;
count = 0;
sum1 = 0;
sum2 = 0;
min_count = mask_size / 2.0;
for (y = r; y < height - r; y++) {
src_pos = src_data + y * src_stride + r;
tmp_pos = tmp_data + y * tmp_stride + r;
for (x = r; x < width - r; x++, src_pos++, tmp_pos++) {
value = *src_pos;
sum = calc_sum(src_pos, &sum_count);
if (sum_count > min_count) {
if (value > epsilon) {
value = abs((sum/sum_count) - value);
if (value < min) min = value; else if (value > max) max = value;
if (x < minx) minx = x; else if (x > maxx) maxx = x;
if (y < miny) miny = y; else if (y > maxy) maxy = y;
sum1 += value;
sum2 += value*value;
count += 1;
}
else {
value = 0;//sum/count;
}
*tmp_pos = value;
}
else {
*tmp_pos = 0;
}
}
}
printf("\n");
avg = sum1 / count;
sdv = sqrt(sum2 / count - avg*avg);
printf("min=%f max=%f avg=%f sdv=%f minx=%d maxx=%d miny=%d maxy=%d\n", min, max, avg, sdv, minx, maxx, miny, maxy);
/*cvSaveImage("tcr_filtered.png", tmp, 0);*/
startx = r;
if (minx > startx + 2) startx = minx - 2;
endx = width - r - 1;
if (maxx < endx - 2) endx = maxx + 2;
starty = r;
if (miny > starty + 2) starty = miny - 2;
endy = height - r - 1;
if (maxy < endy - 2) endy = maxy + 2;
new_width = endx - startx;
/*new_height = endy - starty;*/
new_height = new_width;
tstart = timestamps[starty];
tend = timestamps[endy];
tdiff = tend - tstart;
ratio = (float)tdiff / (float)new_height;
printf("start=%d end=%d diff=%d ratio=%f\n", tstart, tend, tdiff, ratio);
/* create a lookup table for rectified image time stamps */
timestamps_accum = (unsigned int *)malloc(new_height * timestamp_width);
timestamps_accum[0] = tstart;
for (int i = 1; i < new_height; i++) {
timestamps_accum[i] = tstart + (unsigned int)(ratio * i);
}
size.width = new_width;
size.height = new_height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
dst_data = (float*)dst->imageData;
dst_stride = (int)(dst->widthStep / sizeof(float));
dst_pos = dst_data;
int j = starty;
int row_size = new_width * sizeof(float);
tmp_pos = tmp_data + starty * width + startx;
for (int i = 0; i < new_height; i++, dst_pos += dst_stride) {
if (timestamps[j] < timestamps_accum[i]) {
j += 1;
if (j == height) {
printf("row overflow\n");
break;
}
tmp_pos += tmp_stride;
}
memcpy(dst_pos, tmp_pos, row_size);
}
free(timestamps_accum);
/*cvSaveImage("tcr_rectified.png", dst, 0);*/
int hidx;
/* create a cumulative gaussian distribution table for normalizing the histogram */
float norm[256];
float nsum = 0;
for (int i = 0; i < 256; i++) {
norm[i] = exp(-(pow(((float)i/255.0) - 0.5, 2.0) / (2*0.25*0.25)));
nsum += norm[i];
}
norm[0] /= nsum;
for (int i = 1; i < 256; i++) {
norm[i] = (norm[i] / nsum) + norm[i-1];
/*printf("%f ", norm[i]);*/
}
/*printf("\n");*/
for (int i = 0; i < 256; i++) {
hist[i] = 0;
}
stretch = 1.0 / ((avg+2*sdv) - 0);
stretch_1 = 0.5 / (avg - 0);
stretch_2 = 0.5 / (max - avg);
/* normalize values and create the histogram */
printf("stretch_1=%f stretch_2=%f\n", stretch_1, stretch_2);
count = 0;
for (y = 0; y < new_height; y++) {
dst_pos = dst_data + y * dst_stride;
for (x = 0; x < new_width; x++, dst_pos++) {
value = *dst_pos;
if (value > epsilon) {
if (value < (avg+epsilon)) {
value = stretch_1 * (value - 0);
}
else {
value = 0.5 + stretch_2 * (value - avg);
}
/*value = stretch * (value - 0);*/
hidx = (int)(255.0 * value);
if (hidx < 1) hidx = 1;
else if (hidx > 255) hidx = 255;
hist[hidx] += 1;
count += 1;
*dst_pos = value;
}
else {
*dst_pos = 0;
}
}
}
/*cvSaveImage("tcr_normalized.png", dst, 0);*/
for (int i = 1; i < 256; i++) {
printf("%.3f ", hist[i]);
}
printf("\n");
/* transform the histogram into a cumulative distribution */
hist[0] = 0;
for (int i = 1; i < 256; i++) {
hist[i] = (hist[i] / count) + hist[i-1];
printf("%.3f ", hist[i]);
}
printf("\n");
for (y = 0; y < new_height; y++) {
dst_pos = dst_data + y * dst_stride;
for (x = 0; x < new_width; x++, dst_pos++) {
value = *dst_pos;
if (value < epsilon) {
value = 0;
}
else {
hidx = (int)(255.0 * value);
if (hidx < 1) hidx = 1;
else if (hidx > 255) hidx = 255;
value = (hist[hidx] * value); /*norm[hidx] * */
}
*dst_pos = value;
}
}
/*cvSaveImage("tcr_stretched.png", dst, 0);*/
}
}
else {
printf("Failed to allocate memory for image");
}
free(mask_idx);
free(timestamps);
cvReleaseImage(&tmp);
cvReleaseImage(&tmp2);
cvReleaseImage(&tmp3);
cvReleaseImage(&tmp4);
cvReleaseImage(&src);
}
else {
printf("Failed to open file %s\n", path);
}
return dst;
}
IplImage *read_from_tcr_projected(const char *path)
{
}
IplImage *read_from_tcr_old(const char *path)
{
IplImage *tmp, *dst;
CvSize size;
float *tmp_data, *dst_data, *tmp_pos, *dst_pos;
/*unsigned char *b_data, *b_pos;*/
float hist[256];
size_t width = 2592;
size_t height;
int tmp_stride, dst_stride;/*, b_stride;*/
int line_width = width * sizeof(float);
int timestamp_width = sizeof(unsigned int);
tmp = NULL;
dst = NULL;
FILE *input_file = fopen(path, "rb");
size_t readsize;
int line_count;
int file_size;
if (input_file != NULL) {
unsigned int *timestamps, *timestamps_accum;
unsigned int timestamp, prev_timestamp, timestamp_diff;
unsigned int total_timestamp_diff;
unsigned int total_timestamp_diff_2;
unsigned int timestamp_count;
float timestamp_avg, timestamp_sdv;
fseek (input_file , 0 , SEEK_END);
file_size = ftell (input_file);
rewind (input_file);
height = (size_t)(file_size / (line_width + timestamp_width));
printf("File size: %d, rows: %d\n", file_size, height);
size.width = width;
size.height = height;
tmp = cvCreateImage(size, IPL_DEPTH_32F, 1);
tmp_data = (float*)tmp->imageData;
tmp_stride = (int)(tmp->widthStep / sizeof(float));
timestamps = (unsigned int *)malloc(height * timestamp_width);
timestamps_accum = (unsigned int *)malloc(height * timestamp_width);
if (tmp_data != NULL) {
line_count = 0;
prev_timestamp = 0;
total_timestamp_diff = 0;
total_timestamp_diff_2 = 0;
timestamp_count = 1;
timestamps[0] = 0;
tmp_pos = tmp_data;
while (1) {
readsize = fread((void*)×tamp, timestamp_width, 1, input_file);
/*printf("Read %d elements for timestamp\n", readsize);*/
if (readsize < 1) {
break;
}
else {
if (prev_timestamp > 0) {
timestamp_diff = (timestamp - prev_timestamp);
/*printf("%d ", timestamp_diff);*/
total_timestamp_diff += timestamp_diff;
total_timestamp_diff_2 += (timestamp_diff * timestamp_diff);
timestamps[timestamp_count] = timestamp_diff;
timestamp_count += 1;
}
prev_timestamp = timestamp;
}
readsize = fread((void*)tmp_pos, sizeof(float), width, input_file);
/*printf("Read %d elements for line\n", readsize);*/
if (readsize < width) {
break;
}
else {
line_count += 1;
tmp_pos += tmp_stride;
}
}
fclose(input_file);
timestamp_avg = total_timestamp_diff / timestamp_count;
timestamp_sdv = sqrt(total_timestamp_diff_2 / timestamp_count - timestamp_avg*timestamp_avg);
printf("Read %d lines, avg diff %f, diff sdv %f\n", line_count, timestamp_avg, timestamp_sdv);
timestamps_accum[0] = 0;
for (int i = i; i < height; i++) {
timestamps_accum[i] = timestamps[i] / total_timestamp_diff + timestamps_accum[i-1];
}
{
unsigned int pos, x, y, px, py, minx, maxx, miny, maxy, sumx, sumy, count;
unsigned int new_width, new_height;
float value, min, max, sum, sum2, avg, sdv, ratio, stretch, stretch_1, stretch_2;
float a, r, rmin, rmax, cx, cy;
unsigned char bval;
min = 2000000000;
max = 0;
sum = 0;
sum2 = 0;
minx = 2000000000;
maxx = 0;
miny = 2000000000;
maxy = 0;
sumx = 0;
sumy = 0;
count = 0;
for (y = 0; y < height; y++) {
tmp_pos = tmp_data + y * tmp_stride;
for (x = 0; x < width; x++, tmp_pos++) {
value = *tmp_pos;
if (value > 0.0001) {
if (value < min) min = value;
if (value > max) max = value;
if (x < minx) minx = x;
if (x > maxx) maxx = x;
if (y < miny) miny = y;
if (y > maxy) maxy = y;
sum += value;
sum2 += (value * value);
sumx += x;
sumy += y;
count += 1;
}
}
}
for (int i = 0; i < 256; i++) {
hist[i] = 0;
}
int hidx;
avg = sum / count;
sdv = sqrt((sum2 / count) - (avg * avg));
cx = (float)sumx / count;
cy = (float)sumy / count;
ratio = (float)(maxy - miny) / (float)(maxx - minx);
printf("amin=%f amax=%f aavg=%f asdv=%f\n", min, max, avg, sdv);
printf("cx=%f cy=%f ratio=%f\n", cx, cy, ratio);
stretch = 1.0 / (max - min);
stretch_1 = 0.2 / (avg - min);
stretch_2 = 0.8 / (max - avg);
rmin = 2000000000;
rmax = 0;
count = 0;
for (y = 0; y < height; y++) {
tmp_pos = tmp_data + y * tmp_stride;
for (x = 0; x < width; x++, tmp_pos++) {
value = *tmp_pos;
if (value > 0.0001) {
px = floor(x - cx);
py = floor((y / ratio) - (cy / ratio));
r = sqrt(py*py + px*px);
if (r < rmin) rmin = r;
if (r > rmax) rmax = r;
hidx = (int)(255 * stretch * (value - min));
if (hidx < 1) hidx = 1;
else if (hidx > 255) hidx = 255;
hist[hidx] += 1;
count += 1;
}
}
}
hist[0] = 0;
for (int i = 1; i < 256; i++) {
hist[i] = (hist[i] / count) + hist[i-1];
printf("%f ", hist[i]);
}
new_height = ceil(rmax - rmin);
new_width = ceil(2 * M_PI * rmax);
printf("w=%d h=%d rmin=%f rmax=%f\n", new_width, new_height, rmin, rmax);
size.width = new_width;
size.height = new_height;
dst = cvCreateImage(size, IPL_DEPTH_32F, 1);
dst_data = (float*)dst->imageData;
dst_stride = (int)(dst->widthStep / sizeof(float));
for (y = 0; y < new_height; y++) {
dst_pos = dst_data + y * dst_stride;
r = (float)(rmax - y);
for (x = 0; x < new_width; x++, dst_pos++) {
a = ((float)x / (float)new_width) * 2 * M_PI;
px = floor(cx + (r * cos(a)));
py = floor(((cy / ratio) + (r * sin(a))) * ratio);
if (px < 0 || py < 0 || px >= width || py >= height) {
/*printf("x=%d y=%d\n", px, py);*/
}
else {
value = *(tmp_data + py * tmp_stride + px);
if (value < 0.0001) {
value = 0;
}
if (value < avg) {
value = stretch_1 * (value - min);
hidx = (int)(255 * value);
if (hidx < 1) hidx = 1;
else if (hidx > 255) hidx = 255;
value = (hist[hidx] * value);
}
else {
value = 0.2 + stretch_2 * (value - avg);
hidx = (int)(255 * value);
if (hidx < 1) hidx = 1;
else if (hidx > 255) hidx = 255;
value = (hist[hidx] * value);
}
*dst_pos = value;
}
}
}
}
}
else {
printf("Failed to allocate memory for image");
}
cvReleaseImage(&tmp);
}
else {
printf("Failed to open file %s\n", path);
}
return dst;
}
IplImage *read_from_tcr_sdv(const char *path)
{
IplImage *dst;
CvSize size;
float *data, *data2, *data_pos, *data2_pos;
size_t width = 2592;
size_t height;
int line_width = width * sizeof(float);
int timestamp_width = sizeof(unsigned int);
dst = NULL;
FILE *input_file = fopen(path, "rb");
size_t readsize;
int line_count;
int file_size;
if (input_file != NULL) {
unsigned int timestamp, prev_timestamp, timestamp_diff;
unsigned int total_timestamp_diff;
unsigned int total_timestamp_diff_2;
unsigned int timestamp_count;
fseek (input_file , 0 , SEEK_END);
file_size = ftell (input_file);
rewind (input_file);
height = (size_t)(file_size / (line_width + timestamp_width));
printf("File size: %d, rows: %d\n", file_size, height);
data = (float *)malloc(height * width * sizeof(float));
data2 = (float *)malloc(height * width * sizeof(float));
memset(data2, 0, height * width * sizeof(float));
if (data != NULL) {
line_count = 0;
prev_timestamp = 0;
total_timestamp_diff = 0;
total_timestamp_diff_2 = 0;
timestamp_count = 0;
data_pos = data;
while (1) {
readsize = fread((void*)×tamp, timestamp_width, 1, input_file);
/*printf("Read %d elements for timestamp\n", readsize);*/
if (readsize < 1) {
break;
}
else {
if (prev_timestamp > 0) {
timestamp_diff = (timestamp - prev_timestamp);
total_timestamp_diff += timestamp_diff;
total_timestamp_diff_2 += (timestamp_diff * timestamp_diff);
timestamp_count += 1;
}
prev_timestamp = timestamp;
}
readsize = fread((void*)data_pos, sizeof(float), width, input_file);
/*printf("Read %d elements for line\n", readsize);*/
if (readsize < width) {
break;
}
else {
line_count += 1;
data_pos += width;
}
}
size.width = width;
size.height = line_count;
dst = cvCreateImageHeader(size, IPL_DEPTH_32F, 1);
cvSetData(dst, (void*)data2, line_width);
fclose(input_file);
printf("Read %d lines, average diff %d\n", line_count, (int)(total_timestamp_diff / timestamp_count));
{
int size, pos, x, y, count, c, c2;
float amin, amax, smin, smax, asum, asum2, ssum, ssum2, aavg, asdv, savg, ssdv;
float value,v1,v2,v3,v4,v5,v6,v7,v8,v9,vsum,vsum2,vavg,vavg2,vsdv,vsdv2;
float stretch_1, stretch_2;
amin = 2000000000;
smin = 2000000000;
amax = 0;
smax = 0;
asum = 0;
asum2 = 0;
ssum = 0;
ssum2 = 0;
count = 0;
vsum = 0;
vsum2 = 0;
vavg = 0;
vavg2 = 0;
vsdv = 0;
vsdv2 = 0;
for (y = 2; y < height - 2; y++) {
for (x = 2; x < width - 2; x++) {
data_pos = &data[y * width + x];
if (*data_pos > 0.0001) {
/*
v1 = *(data_pos - width - 1);
v2 = *(data_pos - width);
v3 = *(data_pos - width + 1);
v4 = *(data_pos - 1);
v5 = *data_pos;
v6 = *(data_pos + 1);
v7 = *(data_pos + width - 1);
v8 = *(data_pos + width);
v9 = *(data_pos + width + 1);
*/
vsum = calc_sum(data_pos, &c);
/*v1+v2+v3+v4+v5+v6+v7+v8+v9;*/
vsum2 = calc_sum2(data_pos, &c2);
/*(v1*v1)+(v2*v2)+(v3*v3)+(v4*v4)+(v5*v5)+(v6*v6)+(v7*v7)+(v8*v8)+(v9*v9);*/
vavg = vsum / c;
vavg2 = vavg * vavg;
if (vavg < amin) amin = vavg;
else if (vavg > amax) amax = vavg;
asum += vavg;
asum2 += vavg2;
vsum2 /= c2;
if (vsum2 < vavg2) {
vsdv = 0;
}
else {
vsdv = sqrt(vsum2 - vavg2);
}
if (vsdv < smin) smin = vsdv;
else if (vsdv > smax) smax = vsdv;
ssum += vsdv;
ssum2 += (vsdv*vsdv);
count += 1;
}
}
}
/*
for (pos = 0; pos < size; pos++) {
value = data[pos];
v1 = data[pos-
if (value < 0.0001) {
count_0 += 1;
}
else {
if (value < min) min = value;
if (value > max) max = value;
sum_b += value;
sum2_b += (value * value);
count_b += 1;
}
}
*/
aavg = asum / count;
asdv = sqrt((asum2 / count) - (aavg * aavg));
savg = ssum / count;
ssdv = sqrt((ssum2 / count) - (savg * savg));
/*
if (min < (avg - 2 * sdv)) min = (avg - 2 * sdv);
if (max > (avg + 2 * sdv)) max = (avg + 2 * sdv);
*/
/*
stretch_1 = 0.1 / (avg - min);
stretch_2 = 0.9 / (max - avg);
*/
printf("amin=%f amax=%f aavg=%f asdv=%f\n", amin, amax, aavg, asdv);
printf("smin=%f smax=%f savg=%f ssdv=%f\n", smin, smax, savg, ssdv);
smin = 2000000000;
smax = 0;
ssum = 0;
ssum2 = 0;
count = 0;
vsum = 0;
vsum2 = 0;
for (y = 2; y < height-2; y++) {
for (x = 2; x < width-2; x++) {
data_pos = &data[y * width + x];
data2_pos = &data2[y * width + x];
if (*data_pos > 0.0001) {
/*
v1 = *(data_pos - width - 1);
v2 = *(data_pos - width);
v3 = *(data_pos - width + 1);
v4 = *(data_pos - 1);
v5 = *data_pos;
v6 = *(data_pos + 1);
v7 = *(data_pos + width - 1);
v8 = *(data_pos + width);
v9 = *(data_pos + width + 1);
*/
vsum = calc_sum(data_pos, &c);
/*v1+v2+v3+v4+v5+v6+v7+v8+v9;*/
vsum2 = calc_sum2(data_pos, &c2);
/*(v1*v1)+(v2*v2)+(v3*v3)+(v4*v4)+(v5*v5)+(v6*v6)+(v7*v7)+(v8*v8)+(v9*v9);*/
vavg = vsum/c;
vavg2 = vavg*vavg;
vsum2 /= c2;
if (vsum2 < vavg2) {
vsdv = 0;
}
else {
vsdv = sqrt(vsum2 - vavg2);
}
/*if (vsdv > savg) {*/
value = vsdv / ssdv; /* - savg */
*data2_pos = value;
if (value > 1) value = 1;
if (value < smin) smin = value;
if (value > smax) smax = value;
ssum += value;
ssum2 += value*value;
count += 1;
/*}*/
}
}
}
savg = ssum / count;
ssdv = sqrt((ssum2 / count) - (savg*savg));
printf("vmin=%f vmax=%f vavg=%f vsdv=%f\n", smin, smax, savg, ssdv);
size = width * height;
stretch_1 = 1.0 / (smax - smin);
for (pos = 0; pos < size; pos++) {
value = data2[pos];
if (value < 0.0001) {
value = 0.5;
}
else {
value = stretch_1 * (value - smin);
}
data2[pos] = value;
}
}
}
else {
printf("Failed to allocate memory for image");
}
free(data);
}
else {
printf("Failed to open file %s\n", path);
}
return dst;
}