Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

Merge remote-tracking branch 'mattiGH/ME/renkaat'

Conflicts:
	CV.cabal
  • Loading branch information...
commit 25c49c90d5769ea3daeda8908157dea0963ed904 2 parents bea423a + 5958b78
@aleator authored
View
18 CV.cabal
@@ -66,15 +66,21 @@ Library
cbits/cvWrapLEO.h,
cbits/cvWrapCore.h,
cbits/wrapImgProc.h,
- cbits/cvIterators.h
+ cbits/cvIterators.h,
+ cbits/cvProjection.h,
+ cbits/cvFiles.h
c-sources: cbits/cvWrapLEO.c,
cbits/cvWrapCore.c,
cbits/wrapImgProc.c,
- cbits/cvIterators.c
+ cbits/cvIterators.c,
+ cbits/cvProjection.c,
+ cbits/cvFiles.c
install-includes: cbits/cvWrapLEO.h,
cbits/cvWrapCore.h,
cbits/wrapImgProc.h,
- cbits/cvIterators.h
+ cbits/cvIterators.h,
+ cbits/cvProjection.h,
+ cbits/cvFiles.h
if flag(opencv23)
cpp-options: -DOpenCV23
cc-options: -DOpenCV23
@@ -154,6 +160,8 @@ Library
,CV.Tracking
,CV.Transforms
,CV.Video
+ ,CV.Projection
+ ,CV.Files
,Utils.DrawingClass
,Utils.Function
,Utils.GeometryClass
@@ -171,7 +179,9 @@ Library
CV.Bindings.Drawing,
CV.Bindings.Error,
CV.Bindings.Features,
- CV.Bindings.Iterators
+ CV.Bindings.Iterators,
+ CV.Bindings.Projection,
+ CV.Bindings.Files
Extensions: CPP
source-repository head
View
20 CV/Bindings/Files.hsc
@@ -0,0 +1,20 @@
+{-# LANGUAGE ForeignFunctionInterface #-}
+module CV.Bindings.Files where
+
+import Foreign.Ptr
+import Foreign.C.String
+import CV.Bindings.Types
+import CV.Image(BareImage)
+
+#strict_import
+
+#include <bindings.dsl.h>
+#include "cvFiles.h"
+
+#ccall read_from_tcr , CString -> Ptr (Ptr CUInt) -> IO (Ptr BareImage)
+
+#ccall read_from_tcr_rectified , CString -> IO (Ptr BareImage)
+
+#ccall read_from_tcr_mag , CString -> IO (Ptr BareImage)
+
+#ccall calc_derivative_direction , Ptr BareImage -> IO (Ptr BareImage)
View
13 CV/Bindings/Projection.hsc
@@ -0,0 +1,13 @@
+{-# LANGUAGE ForeignFunctionInterface #-}
+module CV.Bindings.Projection where
+
+import Foreign.Ptr
+import CV.Bindings.Types
+import CV.Image(BareImage)
+
+#strict_import
+
+#include <bindings.dsl.h>
+#include "cvProjection.h"
+
+#ccall project_polar , Ptr BareImage -> IO (Ptr BareImage)
View
28 CV/Files.hs
@@ -0,0 +1,28 @@
+module CV.Files where
+
+import CV.Bindings.Files
+import CV.Image
+
+import Foreign.C.String
+import Foreign.Ptr(nullPtr, castPtr)
+import System.IO.Unsafe
+
+readFromTcr :: String -> Image GrayScale D32
+readFromTcr p = unsafePerformIO $ creatingImage $
+ withCString p $ \path ->
+ c'read_from_tcr path nullPtr
+
+readFromTcrRectified :: String -> Image GrayScale D32
+readFromTcrRectified p = unsafePerformIO $ creatingImage $
+ withCString p $ \path ->
+ c'read_from_tcr_rectified path
+
+readFromTcrMag :: String -> Image GrayScale D32
+readFromTcrMag p = unsafePerformIO $ creatingImage $
+ withCString p $ \path ->
+ c'read_from_tcr_mag path
+
+calcDerivativeDirection :: Image GrayScale D32 -> Image GrayScale D32
+calcDerivativeDirection image = unsafePerformIO $ creatingImage $
+ withImage image $ \pimage ->
+ c'calc_derivative_direction (castPtr pimage)
View
15 CV/Operations.hs
@@ -1,3 +1,4 @@
+{-# LANGUAGE ScopedTypeVariables #-}
module CV.Operations
( clear
, set
@@ -6,6 +7,7 @@ module CV.Operations
, unitNormalize
, unitStretch
, logNormalize
+, cartToPolar
) where
import CV.Bindings.Core
@@ -78,3 +80,16 @@ unitNormalize i
unitStretch i = normalize 0 1 NormMinMax i
logNormalize = unitNormalize . IM.log . (1 |+)
+
+cartToPolar :: (Image GrayScale D32, Image GrayScale D32) -> (Image GrayScale D32, Image GrayScale D32)
+cartToPolar (x,y) = unsafePerformIO $ do
+ r::(Image GrayScale D32) <- create (w, h)
+ a::(Image GrayScale D32) <- create (w, h)
+ withImage x $ \px ->
+ withImage y $ \py ->
+ withImage r $ \pr ->
+ withImage a $ \pa -> do
+ c'cvCartToPolar (castPtr px) (castPtr py) (castPtr pr) (castPtr pa) (fromIntegral 0)
+ return (r,a)
+ where
+ (w,h) = getSize x
View
12 CV/Projection.hs
@@ -0,0 +1,12 @@
+module CV.Projection where
+
+import CV.Bindings.Projection
+import CV.Image
+
+import Foreign.Ptr
+import System.IO.Unsafe
+
+projectPolar :: Image c d -> Image GrayScale D32
+projectPolar i = unsafePerformIO $ creatingImage $
+ withImage i $ \i_ptr ->
+ c'project_polar (castPtr i_ptr)
View
1,963 cbits/cvFiles.c
@@ -0,0 +1,1963 @@
+#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*)&timestamp, 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, &timestamps);
+ 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, &timestamps);
+ 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*)&timestamp, 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*)&timestamp, 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*)&timestamp, 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;
+}
View
14 cbits/cvFiles.h
@@ -0,0 +1,14 @@
+#ifndef __CVFILES__
+#define __CVFILES__
+
+#include <opencv2/core/core_c.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+IplImage *read_from_tcr(const char *path, unsigned int **timestamp_array);
+IplImage *read_from_tcr_rectified(const char *path);
+IplImage *read_from_tcr_mag(const char *path);
+
+#endif
View
150 cbits/cvProjection.c
@@ -0,0 +1,150 @@
+#include "cvProjection.h"
+
+#include <stdio.h>
+#include <math.h>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+float calc_sum_9(float *pos, size_t width)
+{
+ float *pos1, *pos2, *pos3, *pos4, *pos5;
+ pos1 = pos - 1 * width - 1;
+ pos2 = pos - 1;
+ pos3 = pos + width - 1;
+ return
+ *pos1 + *(pos1+1) + *(pos1+2) +
+ *pos2 + *(pos2+1) + *(pos2+2) +
+ *pos3 + *(pos3+1) + *(pos3+2);
+}
+
+IplImage *project_polar(IplImage *src)
+{
+ IplImage *dst;
+ unsigned int w, h, x, y, px, py, minx, maxx, miny, maxy, sumx, sumy, count;
+ unsigned int new_width, new_height, src_stride, dst_stride;
+ float value, a, r, rmin, rmax, cx, cy, ratio;
+ float *src_data, *dst_data, *src_pos, *dst_pos;
+
+ CvSize size = cvGetSize(src);
+ src_data = (float*)src->imageData;
+ src_stride = (int)(src->widthStep / sizeof(float));
+ w = size.width;
+ h = size.height;
+
+ minx = 2000000000;
+ maxx = 0;
+ miny = 2000000000;
+ maxy = 0;
+ sumx = 0;
+ sumy = 0;
+ count = 0;
+
+ /* find the extents of object */
+ for (y = 0; y < h; y++) {
+ src_pos = src_data + y * src_stride;
+ for (x = 0; x < w; x++, src_pos++) {
+ value = *src_pos;
+ if (value > 0.0001) {
+ if (x < minx) minx = x;
+ if (x > maxx) maxx = x;
+ if (y < miny) miny = y;
+ if (y > maxy) maxy = y;
+ sumx += x;
+ sumy += y;
+ count += 1;
+ }
+ }
+ }
+
+ cx = (float)sumx / count;
+ cy = (float)sumy / count;
+ ratio = (float)(maxy - miny) / (float)(maxx - minx);
+ printf("w=%d h=%d cx=%f cy=%f ratio=%f\n", w, h, cx, cy, ratio);
+
+ rmin = 2000000000;
+ rmax = 0;
+ for (y = 0; y < h; y++) {
+ src_pos = src_data + y * src_stride;
+ for (x = 0; x < w; x++, src_pos++) {
+ value = *src_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;
+ }
+ }
+ }
+
+ 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 >= w || py >= h) {
+ /*printf(".");*/
+ }
+ else {
+ src_pos = src_data + py * src_stride + px;
+ value = *src_pos;
+ if (value < 0.0001) {
+ if (px > 0 && py > 0 && px < w-1 && py < h-1) {
+ value = calc_sum_9(src_pos, src_stride) / 9;
+ if (value < 0.0001) {
+ value = 0.5;
+ }
+ }
+ else {
+ value = 0.5;
+ }
+ }
+ *dst_pos = value;
+ }
+ }
+ }
+ return dst;
+}
+
+/*
+
+ int w = (int)(2 * M_PI * r);
+ tmp = ensure32F(src);
+ dst = wrapCreateImage32F(w,h,1);
+ if (r < cx && r < cy && h <= r) {
+ float *src_data = (float *)tmp->imageData;
+ float *dst_data = (float *)dst->imageData;
+ int src_stride = (int)(tmp->widthStep / sizeof(float));
+ int dst_stride = (int)(dst->widthStep / sizeof(float));
+ int px, py;
+ double angle;
+ double radius;
+ for (int i = 0; i < w; i++) {
+ angle = ((double)i / (double)w) * (2 * M_PI);
+ for (int j = 0; j < h; j++) {
+ radius = (double)(r-j);
+ px = cx + floor(radius * cos(angle));
+ py = cy + floor(radius * sin(angle));
+ *(dst_data + j * dst_stride + i) = *(src_data + py * src_stride + px);
+ }
+ }
+ }
+ else {
+ printf("image too small\n");
+ }
+*/
+
View
8 cbits/cvProjection.h
@@ -0,0 +1,8 @@
+#ifndef __CVITERATORS__
+#define __CVITERATORS__
+
+#include <opencv2/core/core_c.h>
+
+IplImage *project_polar(IplImage *src);
+
+#endif
Please sign in to comment.
Something went wrong with that request. Please try again.