Freespace and Obstacle Detection using the Stereoscopic camera

Chapter 6. Conclusion and Future Work

B. 1 Freespace and Obstacle Detection using the Stereoscopic camera

The C++ software was used to program for the ‘Bumblebee’ stereoscopic camera system in order to create the left and right images. In addition, the stereo disparity map and 3D point map were constructed based on the left and right images.

*** CameraDLL.cpp : Defines the entry point for the DLL application.

#include "stdafx.h"

#include <stdio.h>

#include <stdlib.h>

#include "include/triclops.h"

#include "include/digiclops.h"

#include "include/pnmutils.h"

BOOL APIENTRY DllMain( HANDLE hModule, DWORD ul_reason_for_call,

LPVOID lpReserved ) { return TRUE; }

inline float round2(float x) { return x > 0 ? (float)((int)(x * 100 + 0.5)) / 100 : (float)((int)(x * 100 - 0.5)) / 100; }

// Sort the arrays the x, y, z ranges visible by the camera

void q_sort(float x_arr[], float y_arr[], float z_arr[], int left, int right) {

int l_hold, r_hold, mid;

float x_pivot, y_pivot, z_pivot;

l_hold = left;

r_hold = right;

x_pivot = x_arr[left];

y_pivot = y_arr[left];

z_pivot = z_arr[left];

while (left < right) {

while ((x_arr[right] >= x_pivot) && (left < right)) right--;

if (left != right) { x_arr[left] = x_arr[right];

y_arr[left] = y_arr[right];

z_arr[left] = z_arr[right];

left++;

}

while ((x_arr[left] <= x_pivot) && (left < right)) left++;

if (left != right) { x_arr[right] = x_arr[left];

y_arr[right] = y_arr[left];

z_arr[right] = z_arr[left];

right--;

} }

x_arr[left] = x_pivot;

y_arr[left] = y_pivot;

z_arr[left] = z_pivot;

mid = left;

left = l_hold;

right = r_hold;

if (left < mid)

q_sort(x_arr, y_arr, z_arr, left, mid - 1);

if (right > mid)

q_sort(x_arr, y_arr, z_arr, mid + 1, right);

}

void quickSort(float x_arr[], float y_arr[], float z_arr[], int array_size) {

q_sort(x_arr, y_arr, z_arr, 0, array_size - 1);

}

void __declspec(dllexport) TestDLL2(float *xoutarray, float *zoutarray) {

TriclopsInput stereoData;

TriclopsInput colorData;

TriclopsImage16 depthImage16;

TriclopsColorImage colorImage;

TriclopsContext triclops;

DigiclopsContext digiclops;

TriclopsError te;

DigiclopsError de;

float x, y, z;

int r, g, b;

// FILE* pointFile;

int nPoints = 0;

int pixelinc;

int i, j, k;

unsigned short* row;

unsigned short disparity;

// float xoutarray[2]; float youtarray[2]; float zoutarray[2]; open the Digiclops

de = digiclopsInitialize( digiclops, 0 );

// get the camera module configuration

de = digiclopsGetTriclopsContextFromCamera( digiclops, &triclops );

// set the digiclops to deliver the stereo image and right (color) image

de = digiclopsSetImageTypes( digiclops, STEREO_IMAGE | RIGHT_IMAGE );

// set the Digiclops resolution, especially for color images

// digiclopsSetImageResolution ( digiclops, DIGICLOPS_HALF );

de = digiclopsSetImageResolution( digiclops, DIGICLOPS_FULL );

// start grabbing

de = digiclopsStart( digiclops );

// set up some stereo parameters, set to 320x240 output images te = triclopsSetResolution( triclops, 240, 320 );

// set disparity range

te = triclopsSetDisparity( triclops, 1,64 );

te = triclopsSetStereoMask( triclops, 9 );

te = triclopsSetEdgeCorrelation( triclops, 1 );

te = triclopsSetEdgeMask( triclops, 11 );

// lets turn off all validation except subpixel and surface te = triclopsSetTextureValidation( triclops, 0 );

te = triclopsSetTextureValidationThreshold( triclops, 0 );

te = triclopsSetUniquenessValidation( triclops, 0 );

// turn on sub-pixel interpolation

te = triclopsSetSubpixelInterpolation( triclops, 1 );

// make sure strict subpixel validation is on

te = triclopsSetStrictSubpixelValidation( triclops, 1 );

// turn on surface validation

te = triclopsSetSurfaceValidation( triclops, 100 );

te = triclopsSetSurfaceValidationSize( triclops, 200 );

te = triclopsSetSurfaceValidationDifference( triclops, 0.5 );

// grab the image set

de = digiclopsGrabImage( digiclops );

// grab the stereo data

de = digiclopsExtractTriclopsInput(digiclops, STEREO_IMAGE, &stereoData );

// grab the color image data

de = digiclopsExtractTriclopsInput( digiclops, RIGHT_IMAGE, &colorData );

// preprocessing the images

te = triclopsPreprocess( triclops, &stereoData );

// stereo processing

te = triclopsStereo( triclops );

// retrieve the interpolated depth image from the context

te = triclopsGetImage16( triclops, TriImg16_DISPARITY, riCam_REFERENCE, &depthImage16 );

te = triclopsRectifyColorImage(triclops, TriCam_REFERENCE, &colorData,

&colorImage );

//declare linked arrays of x,y,z

int const ELEMENT_COUNT = 1024;

float *x_cur, *y_cur, *z_cur;

x_cur = new float[ELEMENT_COUNT];;

y_cur = new float[ELEMENT_COUNT];;

z_cur = new float[ELEMENT_COUNT];;

int array_count = 1;

float * x_arrays[100];

float * y_arrays[100];

float * z_arrays[100];

x_arrays[0] = x_cur;

y_arrays[0] = y_cur;

z_arrays[0] = z_cur;

int cur_pos = 0;

// determine the number of pixels spacing per row pixelinc = depthImage16.rowinc/2;

for ( i = 0, k = 0; i < depthImage16.nrows; i++ ) {

row = depthImage16.data + i * pixelinc;

for ( j = 0; j < depthImage16.ncols; j++, k++ )

disparity = row[j];

// filter invalid points

if ( disparity < 0xFF00 ) {

// convert the 16 bit disparity value to floating point x,y,z

triclopsRCD16ToXYZ( triclops, i, j, disparity, &x, &y, &z );

// look at points within a range if (( 0 <= z ) && ( z <= 5.0 )) {

if ((y <= 0.6) && (y >= -0.6)) {

if ((x <= 1.7) && (x >= -1.8)) {

r = (int)colorImage.red[k];

g = (int)colorImage.green[k];

b = (int)colorImage.blue[k];

// round to x, y, z values

x_cur[cur_pos] = round2(x);

y_cur[cur_pos] = round2(y);

z_cur[cur_pos] = round2(z);

cur_pos++;

if (cur_pos == ELEMENT_COUNT) {

float *tmp_arr;

tmp_arr = new float[ELEMENT_COUNT];

//tmp_arr[ELEMENT_COUNT - 1] = NULL;

x_arrays[array_count] = tmp_arr;

x_cur = tmp_arr;

tmp_arr = new float[ELEMENT_COUNT];

//tmp_arr[ELEMENT_COUNT - 1] = NULL;

y_arrays[array_count] = tmp_arr;

y_cur = tmp_arr;

tmp_arr = new float[ELEMENT_COUNT];

//tmp_arr[ELEMENT_COUNT - 1] = NULL;

z_arrays[array_count] = tmp_arr;

z_cur = tmp_arr;

// reset point cur_pos = 0;

array_count ++;

} nPoints++;

} } } } } }

/*printf("%d\n", nPoints);*/

int nCount = ELEMENT_COUNT * (array_count - 1) + cur_pos;

float *x_final = new float [nCount];

float *y_final = new float [nCount];

float *z_final = new float [nCount];

//float x_final[500]; float y_final[500]; float z_final[500];

for (i = 0; i < array_count - 1; i++) {

memcpy(x_final + (ELEMENT_COUNT) * i, x_arrays[i], (ELEMENT_COUNT) * sizeof(float));

delete [](x_arrays[i]);

memcpy(y_final + (ELEMENT_COUNT) * i, y_arrays[i], (ELEMENT_COUNT) * sizeof(float));

delete [](y_arrays[i]);

memcpy(z_final + (ELEMENT_COUNT) * i, z_arrays[i], (ELEMENT_COUNT) * sizeof(float));

delete [](z_arrays[i]);

}

memcpy(x_final + (ELEMENT_COUNT) * i , x_arrays[i], cur_pos * sizeof(float));

delete [](x_arrays[i]);

memcpy(y_final + (ELEMENT_COUNT) * i , y_arrays[i], cur_pos * sizeof(float));

memcpy(z_final + (ELEMENT_COUNT) * i , z_arrays[i], cur_pos * sizeof(float));

delete [](z_arrays[i]);

//fclose( pointFile );

printf( "Points in file: %d\n", nPoints );

//if (nCount>500)nCount=500;

int min_z_idx = 0;

int outputcounter = 0;

for (i = 1; i < nCount; i++) {

if (x_final[i - 1] == x_final[i]) { if (z_final[min_z_idx] > z_final[i]) {

min_z_idx = i;

} }

else {

//fprintf( filePointsX, "%f\n", x_final[min_z_idx]);

xoutarray[outputcounter]=x_final[min_z_idx];

//youtarray[outputcounter]=y_final[min_z_idx];

zoutarray[outputcounter]=z_final[min_z_idx];

min_z_idx = i;

outputcounter++;

} }

//fclose(filePointsX); fclose(filePointsY); fclose(filePointsZ);

delete []x_final;

delete []y_final;

delete []z_final;

/*printf("End printing to files %d\n", GetTickCount());*/

digiclopsStop( digiclops );

digiclopsDestroyContext( digiclops );

triclopsDestroyContext( triclops );

//return 0; }

*** Camera DLL

LIBRARY CameraDLL

DESCRIPTION 'A C++ dll that can be called from VB' EXPORTS

TestDLL2 @1

*** stdafx.cpp : source file that includes just the standard includes // CameraDLL.pch will be the pre-compiled header

// stdafx.obj will contain the pre-compiled type information

#include "stdafx.h"

*** CameraDLL.h

#ifndef CAMERADLL_H

#define CAMERADLL_H

//This function is exported from the CAMERADLL.dll

void __declspec(dllexport) TestDLL2(float *xoutarray, float *zoutarray);

#endif

#ifndef DIGICLOPS_H

#define DIGICLOPS_H

#ifdef WIN32

#ifdef DIGICLOPSLIB_EXPORTS

#define DIGICLOPSLIB_API __declspec( dllexport )

#else

#define DIGICLOPSLIB_API __declspec( dllimport )

#endif

#define DIGICLOPSLIB_API

#endif // PGR Includes

#ifdef __cplusplus extern "C"

{

#endif

// Macro Definitions; Group = Macro Definitions; The version of the library.

#define DIGICLOPS_VERSION 2339

#define TOP_IMAGE 0x1 // 32bpp RGBU packed image from the top camera.

#define LEFT_IMAGE 0x2 // 32bpp RGBU packed image from the left camera.

#define RIGHT_IMAGE 0x4 // 32bpp RGBU packed image from the right camera.

#define STEREO_IMAGE 0x8 // 24bpp unpacked image from all 3 cameras.

#define ALL_IMAGES ( TOP_IMAGE | LEFT_IMAGE | RIGHT_IMAGE | STEREO_IMAGE )

#define DEFAULT_IMAGE_BUFFER NULL

// Type Definitions and Enumerations; Group = Type Definitions typedef void* DigiclopsContext;

// LEFT_IMAGE, RIGHT_IMAGE, and STEREO_IMAGE.

typedef unsigned long DigiclopsImageType;

// Description: The error codes returned by the functions in this library.

typedef enum DigiclopsError {

DIGICLOPS_ok, // Function completed successfully.

DIGICLOPS_ALREADY_INITIALIZED, // Device already initialized.

DIGICLOPS_ALREADY_STARTED, // Grabbing has already been started.

DIGICLOPS_CALLBACK_NOT_REGISTERED, // Callback is not registered DIGICLOPS_CALLBACK_ALREADY_REGISTERED, // Callback is already registered

DIGICLOPS_CAMERACONTROL_PROBLEM, / Problem controlling camera.

DIGICLOPS_COULD_NOT_OPEN_FILE, // Failed to open file.

DIGICLOPS_FAILED, // General failure.

DIGICLOPS_INVALID_ARGUMENT, // Invalid argument passed.

DIGICLOPS_INVALID_CONTEXT, // Invalid context passed.

DIGICLOPS_INVALID_IMAGE_TYPE, // Invalid image type passed.

DIGICLOPS_ISOCH_GRAB_ERROR, // Isoch grab error occured DIGICLOPS_ISOCH_GRAB_NOT_STARTED, // Isoch grab not started DIGICLOPS_ISOCH_GRAB_SHUTDOWN_ERROR, // Error shutting down isoch grab

DIGICLOPS_MEMORY_ALLOC_ERROR, // Memory allocation error DIGICLOPS_NO_IMAGE, // DigiclopsGrabImage() not called.

DIGICLOPS_NO_TRICLOPS_CONTEXT_FOUND, // No Triclops context found on camera.

DIGICLOPS_NOT_IMPLEMENTED, // Function not implemented.

DIGICLOPS_NOT_STARTED, // DigiclopsStart() not called.

DIGICLOPS_MAX_BANDWIDTH_EXCEEDED, // Request would exceed maximum bandwidth.

DIGICLOPS_NON_PGR_CAMERA,// Attempt to use driver on non-PGR camera

DIGICLOPS_INVALID_MODE, // Invalid mode or framerate set or retrieved.

DIGICLOPS_ERROR_UNKNOWN, // Unknown Error.

} DigiclopsError;

// DigiclopsBusNotificationCallback

#define DIGICLOPS_BUS_INVALID 0 // DigiclopsBusNotificationCallback

#define DIGICLOPS_BUS_VALID 1

// FLYCAPTURE_BUS_INVALID or FLYCAPTURE_BUS_VALID.

typedef void DigiclopsBusNotificationCallback( int iBusNotificationMsg );

// The type used to store the serial number uniquely identifying a Digiclops typedef unsigned long DigiclopsSerialNumber;

// An enumeration used to identify the different image resolutions that are // supported by the library.

typedef enum DigiclopsImageResolution {

DIGICLOPS_160x120, // 160 x 120 resolution.

DIGICLOPS_320x240, // 320 x 240 resolution.

DIGICLOPS_640x480, // 640 x 480 resolution.

DIGICLOPS_1024x768, // 1024 x 768 resolution.

} DigiclopsImageResolution;

// An enumeration used to identify the settings for the size of the output // image relative to the camera sensor resolution.

typedef enum DigiclopsOutputImageResolution {

DIGICLOPS_FULL, // full resolution images DIGICLOPS_HALF, // half resolution images } DigiclopsOutputImageResolution;

// An enumeration used to describe the different camera color configurations.

typedef enum DigiclopsCameraType {

DIGICLOPS_BLACK_AND_WHITE, // black and white system DIGICLOPS_COLOR // color system

} DigiclopsCameraType;

// An enumeration used to describe the camera device currently being controlled.

typedef enum DigiclopsCameraDevice {

// Digiclops 3-camera system.

DIGICLOPS_DEVICE_DIGICLOPS, // Bumblebee 2-camera system.

DIGICLOPS_DEVICE_BUMBLEBEE,

// An enumeration used to describe the different color processing method.

typedef enum DigiclopsColorMethod {

DIGICLOPS_DISABLE_COLOUR_PROCESSING, // do no color processing DIGICLOPS_NEAREST_NEIGHBOR, // nearest neighbor de-mosaicing DIGICLOPS_EDGE_SENSING // edge sensing de-mosaicing } DigiclopsColorMethod;

// A record used in querying the Digiclops properties.

typedef struct DigiclopsInfo {

// Camera serial number

DigiclopsSerialNumber SerialNumber;

// CCD resolution

DigiclopsImageResolution ImageSize;

// Type of CCD (color or b&w)

DigiclopsCameraType CameraType;

// Type of device.

DigiclopsCameraDevice CameraDevice;

} DigiclopsInfo;

// The low-level 1394 bus timestamp. Note that this is not an absolute // epoch-based timestamp, it is a relative timestamp that wraps around every // 128 seconds.

typedef struct Digiclops1394Timestamp {

// The current seconds value of the 1394 bus time. (0-127)

unsigned long ulCycleSeconds;

// The current cycle count value of the 1394 bus time. (0-7999) unsigned long ulCycleCount;

} Digiclops1394Timestamp;

// A wrapper for a TriclopsInput that includes a Digiclops1394Timestamp.

typedef struct DigiclopsImage {

// The low-level timestamp value for this DigiclopsImage.

Digiclops1394Timestamp digiclopsTimestamp;

// The TriclopsInput structure contained in this DigiclopsImage.

TriclopsInput triclopsInput;

} DigiclopsImage;

// An enumeration of the different camera properties that can be set via the programming interface.

typedef enum DigiclopsCameraProperty TriclopsBool

ppmWriteFromTriclopsInputWithComment( const char* filename, const char* comment,

TriclopsInput* input );

#ifdef __cplusplus }

#endif

#endif //#ifndef _PNMUTILS_H_