HEADmaster

1 files changed, 1244 insertions, 0 deletions

diff --git a/ARMFCaptureD3D/ARAnalyse.cpp b/ARMFCaptureD3D/ARAnalyse.cpp
new file mode 100644
index 0000000..71b22b8
--- /dev/null
+++ b/ARMFCaptureD3D/ARAnalyse.cpp
@@ -0,0 +1,1244 @@
+

+

+#include "debug.h"

+#include <algorithm>

+

+#include "ConfigFile\ConfigFile.h"

+

+#include "ARAnalyse.h"

+extern char* narrow( const std::wstring& str );

+extern ConfigFile *g_config;

+extern std::wstring g_camera_name;

+extern int g_camera_instance;

+extern int g_ThresholdMin;

+extern int g_ThresholdMax;

+extern bool g_read_calibrationfile;

+

+// Filtrage regexp IVY !!! a mettre en concordance avec les SendMsg effectué !!!

+const char *ivy_filters[] = {"MarkerAdd", "MarkerMove", "MarkerMoveCalibrated", "MarkerRemove" };

+

+//-------------------------------------------------------------------

+//  constructor

+//-------------------------------------------------------------------

+

+ARAnalyse::ARAnalyse() :

+	m_calibration(NULL),

+	m_tracker(NULL)

+{

+	MarkerMoveThreshold = 0.001;

+	MarkerAbsenceCount = 10;

+	MarkerWidth = 20;

+	m_calibrated = false;

+	m_calibration_enabled = false;

+	FrameCount = 0;

+	bus = NULL;

+	m_gray = NULL;

+	m_blur0 = NULL;

+	m_blur1 = NULL;

+	DownSample = 8;

+	BlurPass = 2;

+	Display = ARAnalyse::Diff;

+	filter = ARAnalyse::FMoyen;

+

+	MarkerWidth = g_config->read<double>("MarkerWidth");

+	MarkerMoveThreshold = g_config->read<double>("MarkerMoveThreshold");

+	MarkerAbsenceCount = g_config->read<double>("MarkerAbsenceCount");

+	MarkerSeekingThreshold = g_config->read<double>("MarkerSeekingThreshold");

+	CameraParameterFile = g_config->read<string>("CameraParameterFile","data\\no_distortion.cal");

+	MarkerSetConfigFile = g_config->read<string>("MarkerSetConfigFile", "data\\One.cfg");

+	CalibrationFile = g_config->read<string>("CalibrationFile", "data/savedCalibration.dat");

+	MarkerBorderPercentage = g_config->read<float>("MarkerBorderPercentage",0.125f);

+    MarkerSeekingThreshold = g_config->read<int>("MarkerSeekingThreshold",160);

+	MarkerType =  (ARToolKitPlus::MARKER_MODE) g_config->read<int>("MarkerTraditionalOrSimpleOrBCH",ARToolKitPlus::MARKER_ID_BCH);

+	

+	m_calibration = new Calibration(1.0, 1.0);

+	// intialize Calibration

+	int id;

+	id = g_config->read<int>("CalibrationID_TopLeft");

+    m_IdToCorner[id] = Calibration::TopLeft;

+	m_calibration_ids.insert( id );

+	id = g_config->read<int>("CalibrationID_TopRight");

+    m_IdToCorner[id] = Calibration::TopRight;

+	m_calibration_ids.insert( id );

+	id = g_config->read<int>("CalibrationID_BottomLeft");

+    m_IdToCorner[id] = Calibration::BottomLeft;

+	m_calibration_ids.insert( id );

+	id = g_config->read<int>("CalibrationID_BottomRight");

+    m_IdToCorner[id] = Calibration::BottomRight;

+	m_calibration_ids.insert( id );

+	if ( g_read_calibrationfile )

+	{

+		m_calibrated = m_calibration->RestoreCalibration( CalibrationFile );

+	}

+}

+

+//-------------------------------------------------------------------

+//  destructor

+//-------------------------------------------------------------------

+

+ARAnalyse::~ARAnalyse()

+{

+	if ( m_calibration ) delete m_calibration;

+	if ( bus )

+	{

+		bus->stop();

+		delete bus;

+	}

+}

+void ARAnalyse::IvyInit(const char *domain)

+{

+	// start Ivy

+	bus = new Ivy( "ARIvy", NULL, NULL );

+	bus->SetFilter( sizeof( ivy_filters ) /sizeof( char *), ivy_filters );

+	bus->start(domain);

+}

+bool ARAnalyse::Calibrate( )

+{

+	if ( m_calibration )

+		m_calibrated = m_calibration->Calibrate();

+	return m_calibrated;

+}

+void ARAnalyse::SaveCalibration( )

+{

+	if ( m_calibration )

+		m_calibration->SaveCalibration(CalibrationFile);

+}

+void ARAnalyse::RestoreCalibration( )

+{

+	if ( m_calibration )

+		m_calibrated = m_calibration->RestoreCalibration(CalibrationFile);

+}

+bool ARAnalyse::ToggleAutoThreshold()

+{

+	if ( m_tracker->isAutoThresholdActivated() )

+	{

+		// try to set manually 

+		if ( MarkerSeekingThreshold > 0 )

+		{

+			m_tracker->activateAutoThreshold( false );

+			m_tracker->setThreshold( MarkerSeekingThreshold );

+		}

+	}

+	else

+	{

+		m_tracker->activateAutoThreshold( true );

+	}

+	return m_tracker->isAutoThresholdActivated();

+}

+

+bool ARAnalyse::IsCalibrationID( int ident )

+{

+	return m_calibration_ids.find( ident ) != m_calibration_ids.end();

+}

+	

+bool ARAnalyse::InitialiseTracker( int width, int height, int sample_size)

+{

+	first_calibration_done = false;

+	camera = narrow( g_camera_name );

+	int pixel_size = sample_size / width /height;

+	m_width = width;

+	m_height = height;

+	nb_pixel = width * height;

+	m_gray = new pixel_type[nb_pixel];

+	// on alloue au max pur eviter de se prendre la tete avec le malloc free , et le changement de DownSample

+	m_blur0 = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];

+	m_blur1 = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];

+	m_filter_h = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];

+	m_filter_v = new pixel_type[nb_pixel /*width /DownSample * height /DownSample*/];

+

+	nb_images = 5;

+	m_sum = new float[nb_pixel];

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		m_sum[i] = 0.0;

+	}

+	m_images = new float*[nb_images];

+	for ( int i = 0 ; i < nb_images; i++ )

+		m_images[i] = new float[nb_pixel];

+	m_first_image = 0;

+	m_last_image = 0;

+

+	

+	if ( m_tracker ) delete m_tracker;

+    // create a tracker that does:

+    //  - 6x6 sized marker images (required for binary markers)

+    //  - samples at a maximum of 6x6

+    //  - works with luminance (gray) images

+    //  - can load a maximum of 0 non-binary pattern

+    //  - can detect a maximum of 8 patterns in one image

+    m_tracker = new TrackerMultiMarker(width, height, 4096, 6, 6, 6, 0);

+	// poor man pixel format chooser

+	switch ( pixel_size )

+	{

+	case 1:

+	    m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_LUM);

+		break;

+	case 2:

+		m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_YUY2);

+		break;

+	case 3:

+		m_tracker->setPixelFormat(ARToolKitPlus::PIXEL_FORMAT_RGB);

+		break;

+	}

+    // load a camera file.

+	if (!m_tracker->init(CameraParameterFile.c_str(),

+						MarkerSetConfigFile.c_str(),

+						0.0f, 3000.0f)) 

+		{

+        DBGMSG(TEXT("ERROR: InitialiseTracker() failed\n"));

+		delete m_tracker;

+		m_tracker = NULL;

+		return false;

+		}

+

+    //m_tracker->getCamera()->printSettings();

+	m_tracker->setNumAutoThresholdRetries(2);

+	//if ( MarkerSeekingThreshold > 0 )

+	//{

+	//	m_tracker->activateAutoThreshold( false );

+	//	m_tracker->setThreshold( MarkerSeekingThreshold );

+	//}

+	//else

+	//{

+		m_tracker->activateAutoThreshold( true );

+	//}

+    m_tracker->setImageProcessingMode(ARToolKitPlus::IMAGE_FULL_RES);

+    m_tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);

+	//m_tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT);

+    // the marker in the BCH test image has a thiner border...

+    m_tracker->setBorderWidth(MarkerBorderPercentage);

+

+    // set a threshold. we could also activate automatic thresholding

+    //m_tracker->setThreshold(MarkerSeekingThreshold);

+

+    // let's use lookup-table undistortion for high-speed

+    // note: LUT only works with images up to 1024x1024

+    //m_tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);

+	m_tracker->setUndistortionMode(ARToolKitPlus::UNDIST_NONE);

+	m_tracker->setUseDetectLite(false);

+    // switch to simple ID based markers

+    // use the tool in tools/IdPatGen to generate markers

+    m_tracker->setMarkerMode(MarkerType);

+	// HULL mode

+    m_tracker->setHullMode(ARToolKitPlus::HULL_FULL);

+            

+    // do the OpenGL camera setup

+    //glMatrixMode(GL_PROJECTION)

+    //glLoadMatrixf(m_tracker->getProjectionMatrix());

+

+    return true;

+}

+

+void ARAnalyse::Analyse( double SampleTime, const unsigned char * cameraBuffer, int size )

+{

+	static double StartTime = -1;

+	static double LastFrameTime = 0;

+	

+

+	extern HWND g_hTrack;  // Quick & Dirty !!!!

+

+	double delay = 0;

+	if ( StartTime < 0 )

+	{

+		StartTime = SampleTime;

+		LastFrameTime = SampleTime;

+	}

+	else

+	{

+	delay = SampleTime - LastFrameTime;

+	}

+	FrameCount++;	

+	if ( FrameCount > 30 ) // skip first frames in calc avg Wait first Seconds video

+	{

+		frame_per_seconds.SetValue( FrameCount / ( SampleTime - StartTime ) );

+		frame_delay.SetValue( delay );

+	}

+	LastFrameTime = SampleTime;

+	

+	if ( !m_tracker) return;

+	// here we go, just one call to find the camera pose

+    int numDetected = m_tracker->calc(cameraBuffer);

+

+	//DBGMSG(TEXT("\n tracker Threshold %d \n"), m_tracker->getThreshold());

+	if ( m_tracker->isAutoThresholdActivated() && FrameCount %10 == 0  )

+	{

+	SendMessage(g_hTrack, TBM_SETPOS, 

+        (WPARAM) TRUE,                   // redraw flag 

+        (LPARAM) m_tracker->getThreshold());

+	}

+    // use the result of calc() to setup the OpenGL transformation

+    //glMatrixMode(GL_MODELVIEW)

+    //glLoadMatrixf(m_tracker->getModelViewMatrix());

+	//if ( numDetected )

+	//    DBGMSG(TEXT("\n%d good Markers found and used for pose estimation.\n"), numDetected);

+#ifdef DUMP_MATRIX

+	if ( numDetected )

+	{

+	DBGMSG(TEXT("Pose-Matrix:\n  "));

+	const ARFloat*matrix = m_tracker->getModelViewMatrix();

+	if ( matrix )

+    for (int i = 0; i < 16; i++)

+        DBGMSG(TEXT("%.2f %s"), matrix[i], (i % 4 == 3) ? TEXT("\n  ") : TEXT(" "));

+	}

+#endif

+	            std::set<int> new_ids;

+                confidence.Raz(); 

+	

+                for (int i = 0; i < numDetected; i++) {

+                    ARToolKitPlus::ARMarkerInfo markerInfo = m_tracker->getDetectedMarker(i);

+                   

+                    if (new_ids.find(markerInfo.id) != new_ids.end()) {

+                        DBGMSG(TEXT("One id for one marker please!!\n"));

+                        continue;

+                    }

+                    // calculate min max confidence

+					confidence.SetValue( markerInfo.cf );

+					

+					new_ids.insert(markerInfo.id);

+

+                    if (markers.find(markerInfo.id) == markers.end() )

+                    {

+                        // Adding new marker

+                        markers[markerInfo.id].id = markerInfo.id;

+                        DBGMSG(TEXT("MarkerAdd cam=%s_%d id=%d\n"), g_camera_name.c_str(), g_camera_instance, markerInfo.id);

+						// filtrage des ID de calibration

+                        if ( bus && !IsCalibrationID(markerInfo.id) ) bus->SendMsg("MarkerAdd cam=%s_%d id=%d", camera, g_camera_instance, markerInfo.id);

+                    }

+                    MyMakerInfo & info = markers[markerInfo.id];

+					info.id = markerInfo.id;

+                    info.absent = 0;

+                    info.confidence = markerInfo.cf;

+                    info.area = markerInfo.area;

+                    info.dir = markerInfo.dir;

+					

+                    info.time = SampleTime - StartTime;

+                    ARFloat center[2] ;

+                    ARFloat  matrix[3][4];

+                    center[0] = 0.0f;

+                    center[1] = 0.0f;

+                    //ARFloat retTransMat = m_tracker->arGetTransMat(tracker, markerInfoPtr, center, (ARFloat)Properties.Settings.Default.MarkerWidth, matrix);

+                    ARFloat retTransMat =  m_tracker->arGetTransMatCont( &markerInfo, info.prev_matrix, center, (ARFloat)MarkerWidth, matrix);

+                    //ARFloat retTransMat = m_tracker->rppGetTransMat(tracker, markerInfoPtr, center, (ARFloat)Properties.Settings.Default.MarkerWidth, matrix);

+                    if (retTransMat < 0)

+                    {

+                        DBGMSG(TEXT("Marker Skip id={0} bad transfrm{1}\n"), info.id, retTransMat);

+                        continue;

+                    }

+                    memcpy(info.prev_matrix, matrix, sizeof(matrix));

+                    double dx = info.x  - markerInfo.pos[0];

+					double dy = info.y  - markerInfo.pos[1];

+					double dist = dx*dx + dy*dy; // Square Length distance

+					

+                    info.x = markerInfo.pos[0];

+					info.y = markerInfo.pos[1];

+					if ( dist >= MarkerMoveThreshold)

+                    {

+                        DBGMSG(TEXT("MarkerMove cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f distance=%.03f \n"),

+                            g_camera_name.c_str(), g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, info.x, info.y, dist);

+                        if ( bus && !IsCalibrationID(markerInfo.id)) 

+							bus->SendMsg("MarkerMove cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f distance=%.03f",

+								camera, g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, info.x, info.y, dist);

+                        if (m_calibrated)

+                        {

+							double cal_x, cal_y;

+                            m_calibration->translateCoords(info.x, info.y, &cal_x, &cal_y);

+                            DBGMSG(TEXT("MarkerMoveCalibrated cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f\n"), g_camera_name.c_str(), g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, cal_x, cal_y);

+                            if ( bus && !IsCalibrationID(markerInfo.id)) 

+								bus->SendMsg("MarkerMoveCalibrated cam=%s_%d id=%d time=%.03f confidence=%.03f area=%d dir=%d x=%.03f y=%.03f", camera, g_camera_instance, info.id, info.time, info.confidence, info.area, info.dir, cal_x, cal_y);

+                        

+                        }

+                    }

+                    

+                }

+                // remove OLD markers

+                std::set<int> old_ids;

+				for (MarkersMap::iterator iter = markers.begin(); iter != markers.end();  iter++)

+				{

+					int id = iter->first;

+					if ( new_ids.find( id ) == new_ids.end() )

+						old_ids.insert( id );

+				}

+				for (std::set<int>::iterator iter = old_ids.begin(); iter != old_ids.end();  iter++)

+                {

+					int id = *iter;

+                    markers[id].absent += 1;

+                    if ( markers[id].absent > MarkerAbsenceCount)

+                    {

+                        markers.erase(id);

+                        DBGMSG(TEXT("Marker Remove cam=%s_%d id=%d\n"), g_camera_name.c_str(), g_camera_instance, id);

+                        if ( bus && !IsCalibrationID(id)) bus->SendMsg("MarkerRemove cam=%s_%d id=%d", camera, g_camera_instance, id);

+                    }

+                }

+                /*if (titleText.Equals("")) {

+                    titleText ="Markers with ID \""

+                        +String.Join(", ", new_ids.Select(id => id.ToString("000")).ToArray())

+                        +"\" are found!!";

+                }*/

+ 

+                

+                    

+                // calibration point

+                m_calibration_enabled = false;

+                if (includes(new_ids.begin(), new_ids.end(), m_calibration_ids.begin(), m_calibration_ids.end() ))

+				{

+                    for (std::set<int>::iterator iter = m_calibration_ids.begin(); iter != m_calibration_ids.end() ; iter++)

+                    {

+						int id = *iter;

+                        MyMakerInfo marker = markers[id];

+                        m_calibration->SetCalibrationPoint(m_IdToCorner[id], marker.x, marker.y);

+                    }

+                    //all calibration points OK

+					m_calibration_enabled = true;

+					if ( !first_calibration_done )

+					{

+						first_calibration_done = Calibrate();

+						m_calibrated = first_calibration_done;

+					}

+                    

+                }

+					

+           

+    bool showConfig = false;

+

+    if (showConfig) {

+        const ARToolKitPlus::ARMultiMarkerInfoT *artkpConfig = m_tracker->getMultiMarkerConfig();

+        DBGMSG(TEXT("%d markers defined in multi marker cfg\n"), artkpConfig->marker_num);

+

+        printf("marker matrices:\n");

+        for (int multiMarkerCounter = 0; multiMarkerCounter < artkpConfig->marker_num; multiMarkerCounter++) {

+            DBGMSG(TEXT("marker %d, id %d:\n"), multiMarkerCounter, artkpConfig->marker[multiMarkerCounter].patt_id);

+            for (int row = 0; row < 3; row++) {

+                for (int column = 0; column < 4; column++)

+                    DBGMSG(TEXT("%.2f  "), artkpConfig->marker[multiMarkerCounter].trans[row][column]);

+                DBGMSG(TEXT("\n"));

+            }

+        }

+    }

+}

+

+#define MAX(a,b) ((a) > (b) ? (a) : (b))

+#define MIN(a,b) ((a) < (b) ? (a) : (b))

+static void RGB2HSL(unsigned char * rgb,  double& h, double& s,  double& l)

+        {

+

+            double r = (double)rgb[0] / 255.0;

+            double g = (double)rgb[1] / 255.0;

+            double b = (double)rgb[2] / 255.0;

+            double v;

+            double m;

+            double vm;

+            double r2, g2, b2;

+

+            h = 0; // default to black

+            s = 0;

+            l = 0;

+

+            v = MAX(r, g);

+            v = MAX(v, b);

+            m = MIN(r, g);

+            m = MIN(m, b);

+            l = (m + v) / 2.0;

+            if (l <= 0.0)

+            {

+                return;

+            }

+

+            vm = v - m;

+            s = vm;

+            if (s > 0.0)

+            {

+                s /= (l <= 0.5) ? (v + m) : (2.0 - v - m);

+            }

+            else

+            {

+                return;

+            }

+

+            r2 = (v - r) / vm;

+            g2 = (v - g) / vm;

+            b2 = (v - b) / vm;

+            if (r == v)

+            {

+                h = (g == m ? 5.0 + b2 : 1.0 - g2);

+            }

+

+            else if (g == v)

+            {

+                h = (b == m ? 1.0 + r2 : 3.0 - b2);

+            }

+            else

+            {

+                h = (r == m ? 3.0 + g2 : 5.0 - r2);

+            }

+            h /= 6.0;

+        }

+int CircularDistance(int index1, int index2, int steps)

+        {            //circular distance

+            int dist = abs(index1 - index2);

+            return MIN(dist, abs(dist - steps));

+        }

+#ifdef TRANSFORM_COLOR

+ void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)

+        {

+			float min = FLT_MAX;

+			float max = FLT_MIN;

+			pixel_type *dst = m_blur0;

+			// Gray Image

+			{

+				for ( int i = 0; i < nb_pixel ; i++ )

+				{

+

+					unsigned char  * pixel = &cameraBuffer[i*3];

+					value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;

+					m_gray[i] = val;

+					{

+						if ( val > max ) 

+							max = val;

+						else if ( val < min ) 

+							min = val;

+					}

+					if ( !moyen ) 

+						m_sum[i] = 0;

+				}

+				float a = 1.0;

+				float b = 0.0;

+				float c =  (max - min);

+				if ( c != 0.0)

+				{

+					a = 1.0 / c;

+					b = - min  / c;

+				}

+				a *= 255.0;

+				b *= 255.0;

+				#pragma omp parallel for

+				for ( int i = 0; i < nb_pixel ; i++ )

+				{

+					m_gray[i] = m_gray[i] * a + b;

+				}

+			// Image moyenne

+				if ( moyen )

+					CalcMoyen();

+			}

+

+			const int steps = 10;

+			const int chromaHistoLength = steps +1;

+            

+            int chromaHisto[chromaHistoLength];

+            double hue[1920];  

+            

+            // chroma histogram 10 steps

+            // find the 2 max

+            //if the 2 max> 1 the threashold = min (2 max) + (max2 - max1)/2

+

+            //Horizontal line 

+            for (int y = 0; y < m_height; y++)

+            {

+                //Find min max mean median

+				for (int i = 0; i < chromaHistoLength; i++)

+					chromaHisto[i] = 0;

+                for (int x = 0; x < m_width; x++)

+                {

+					long index = (x + y * m_width);

+                    unsigned char  * pixel = &cameraBuffer[index*3];

+					double h,s,l;

+					RGB2HSL( pixel, h,s,l );

+					hue[x] = h;

+                    int histIndexChroma = (int)(h * steps);

+                    chromaHisto[histIndexChroma]++;

+                }

+

+                //Find the two max

+                int max1 = -1;

+                int chroma1 = -1;

+                int max2 = -1;

+                int chroma2 = -1;

+        

+                //Find max1

+                for (int i = 0; i < chromaHistoLength; i++)                   

+                {

+                    if ((chromaHisto[i] > max1) && (chromaHisto[i] != 0))

+                    {

+                        max1 = chromaHisto[i];

+                        chroma1 = i;

+                    }

+                }

+               //find max2

+                for (int i = 0; i < chromaHistoLength; i++)                   

+                {

+                    if (CircularDistance(i, chroma1, steps) > (steps / 3))

+                        if ((chromaHisto[i] > max2)  && (chromaHisto[i] != 0))

+                        {

+                            max2 = chromaHisto[i];

+                            chroma2 = i;

+                        }

+                }

+

+                int peak = chroma1;

+                if ((max1 != -1) && (max2 != -1) &&  (chromaHisto[chroma2] > chromaHisto[chroma1])) peak = chroma2;

+                double delta = 3;

+

+				//binaries

+				if ((max1 != -1) && (max2 != -1))  //found a max1 and max 2 ?

+				{

+					for (int x = 0; x < m_width; x++)

+					{

+						int index = (x + y * m_width);

+						int histIndexChroma = (int)(hue[x] * steps);

+						if (  CircularDistance(histIndexChroma, peak, steps) < delta)

+							dst[index] = 255;  //White

+						else

+							dst[index] = 0;  //Black   

+					}

+

+				}

+				else

+					for (int x = 0; x < m_width; x++)

+					{

+						int index = (x + y * m_width);

+						dst[index] = 127.0f;  //no relevant histogram

+					}

+            }

+

+			switch ( Display )

+			{

+			case ARAnalyse::GrayScale:

+				DisplayGrayScale(cameraBuffer, m_gray );

+				break;

+			case ARAnalyse::Blur:

+				DisplayBlur(cameraBuffer, dst );

+				break;

+			case ARAnalyse::Diff:

+				DisplayDiff(cameraBuffer, dst );

+				break;

+			case ARAnalyse::Threshold:

+				DisplayThreshold(cameraBuffer, dst );

+				break;

+			case ARAnalyse::Moyen:

+				DisplayMoyen(cameraBuffer, NULL );

+				break;

+			}

+         

+        }

+

+ 

+#endif

+#ifdef TRANSFORM_HUE

+void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)

+{

+	pixel_type *src;

+	pixel_type *dst;

+	float min = FLT_MAX;

+	float max = FLT_MIN;

+	//#pragma omp parallel //default(none) shared (cameraBuffer,pixel_size, max, min )

+	{

+	//#pragma omp parallel for

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		unsigned char  * pixel = &cameraBuffer[i*3];

+		value_type  rgb_min = MIN(pixel[0], pixel[1], pixel[2]);

+		value_type  rgb_max = MAX(pixel[0], pixel[1], pixel[2]);

+

+		value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;

+		//

+		//if ( pixel[0] > pixel[1]  && pixel[0] > pixel[2] )

+		//	val = 255;

+		//if ( pixel[2] > pixel[0] && pixel[2] > pixel[1]  )

+		//	val = 0;

+		//m_gray[i] = val;

+		

+		/* Compute hue */

+		//value_type hue;

+		//if (rgb_max == pixel[0]) {

+		//	hue = 0 + 43*(pixel[1] - pixel[2])/(rgb_max - rgb_min);

+		//} else if (rgb_max == pixel[1]) {

+		//	hue = 85 + 43*(pixel[2] - pixel[0])/(rgb_max - rgb_min);

+		//} else /* rgb_max == rgb.b */ {

+		//	hue = 171 + 43*(pixel[0] - pixel[1])/(rgb_max - rgb_min);

+		//}

+		double h,s,l;

+		RGB2HSL( pixel, h,s,l );

+		m_gray[i] = h * 255.0;

+		//#pragma omp critical

+		{

+			if ( val > max ) 

+				max = val;

+			else 

+				if ( val < min ) 

+					min = val;

+		}

+		if ( !moyen )

+			m_sum[i] = 0;

+	}

+

+	float a = 1.0;

+	float b = 0.0;

+	float c =  (max - min);

+	if ( c != 0.0)

+	{

+		a = 1.0 / c;

+		b = - min  / c;

+	}

+	a *= 255.0;

+	b *= 255.0;

+	#pragma omp parallel for

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		m_gray[i] = m_gray[i] * a + b;

+	}

+	}

+	if ( moyen )

+		CalcMoyen();

+	// Down sample

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	

+	#pragma omp parallel //default(none) shared (width,height)

+	{

+	#pragma omp parallel for

+	for ( int y = 0; y < height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < width ; x++ )

+			{

+				long yy = y * width + x;

+				value_type val = 0;

+				for ( int i = 0 ; i < DownSample ; i++ )

+					for ( int j = 0; j < DownSample ; j++ )

+					{

+						int index = (y*DownSample+i) * m_width + x*DownSample+j;

+						val += m_gray[index];

+					}

+				val /= DownSample*DownSample;

+				//assert( val <= 255 );

+				m_blur0[yy] = val;

+				m_blur1[yy] = val;

+			}

+	}

+	}

+	// blur

+	dst = m_blur0;

+#define BLUR

+#ifdef BLUR		

+	#pragma omp parallel  //default(none) shared (width,height,src,dst)

+	{

+		

+		static const float moyen[] = {

+			1/9.0,1/9.0,1/9.0,

+			1/9.0,1/9.0,1/9.0,

+			1/9.0,1/9.0,1/9.0

+		};

+		static const float laplacien4[] = {

+			0,-1,0,

+			-1,4,-1,

+			0,-1,0

+		};

+		

+		static const float laplacien8[] = {

+			-1,-1,-1,

+			-1,8,-1,

+			-1,-1,-1

+		};

+		

+		static const float laplaciend[] = {

+			1,-2,1,

+			-2,4,-2,

+			1,-2,1

+		};

+		

+		static const float sobelv[] = {

+			1,0,-1,

+			2,0,-2,

+			1,0,-1

+		};

+		static const float sobelh[] = {

+			1,2,1,

+			0,0,0,

+			-1,2,-1

+		};

+		

+		static const float prewittv[] = {

+			1,0,-1,

+			1,0,-1,

+			1,0,-1

+		};

+		static const float prewitth[] = {

+			1,1,1,

+			0,0,0,

+			-1,-1,-1

+		};

+		//5x5 Gaussian filter

+		static const float gaussian[] = {

+			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0,

+			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,

+			5/159.0,	12/159.0,	15/159.0,	12/159.0,	5/159.0,

+			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,

+			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0	

+		};

+	for ( int pass = 0 ; pass < BlurPass ; pass++ )

+	{

+		if ( (pass & 1) ==0 ) 

+		{

+		src = m_blur0;

+		dst = m_blur1;

+		}

+		else

+		{

+		src = m_blur1;

+		dst = m_blur0;

+		}

+		switch ( filter )

+		{

+		case FMoyen:

+			Filter(  moyen,3, src, dst );

+			break; 

+		case FLaplacien4:

+			Filter( laplacien4,3, src, dst );

+			break;

+		case FLaplacien8:

+			Filter( laplacien8, 3, src, dst );

+			break;

+		case FLaplaciend:

+			Filter( laplaciend, 3, src, dst );

+			break;

+		case FSobel:

+			Filter( sobelv, 3, src, m_filter_v );

+			Filter( sobelh, 3, src, m_filter_h );

+			CombineFilter( m_filter_v, m_filter_h, dst );

+			break;

+		case FPrewitt:

+			Filter( prewittv, 3, src, m_filter_v );

+			Filter( prewitth, 3, src, m_filter_h );

+			CombineFilter( m_filter_v, m_filter_h, dst );

+		case FGaussian:

+			Filter( gaussian, 5, src, dst );

+			break;

+		}

+		

+	}

+	}

+#endif	

+	switch ( Display )

+	{

+	case ARAnalyse::GrayScale:

+		DisplayGrayScale(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Blur:

+		DisplayBlur(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Diff:

+		DisplayDiff(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Threshold:

+		DisplayThreshold(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Moyen:

+		DisplayMoyen(cameraBuffer, dst );

+		break;

+	}

+		

+}

+#endif

+#define GRAY

+#ifdef GRAY 

+// Gray scale & threshold

+void ARAnalyse::Transform( unsigned char * cameraBuffer, int size, bool moyen)

+{

+	pixel_type *src;

+	pixel_type *dst;

+	float min = FLT_MAX;

+	float max = FLT_MIN;

+	//#pragma omp parallel //default(none) shared (cameraBuffer,pixel_size, max, min )

+	{

+	//#pragma omp parallel for

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		unsigned char  * pixel = &cameraBuffer[i*3];

+		value_type val = pixel[0] * 0.3 + pixel[1] * 0.59 + pixel[2] * 0.11;

+		m_gray[i] = val ;

+		//#pragma omp critical

+		{

+			if ( val > max ) 

+				max = val;

+			else 

+				if ( val < min ) 

+					min = val;

+		}

+		if ( !moyen )

+			m_sum[i] = 0;

+	}

+

+	float a = 1.0;

+	float b = 0.0;

+	float c =  (max - min);

+	if ( c != 0.0)

+	{

+		a = 1.0 / c;

+		b = - min  / c;

+	}

+	a *= 255.0;

+	b *= 255.0;

+	#pragma omp parallel for

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		m_gray[i] = m_gray[i] * a + b;

+	}

+	}

+	if ( moyen )

+		CalcMoyen();

+	// Down sample

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	

+	#pragma omp parallel //default(none) shared (width,height)

+	{

+	#pragma omp parallel for

+	for ( int y = 0; y < height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < width ; x++ )

+			{

+				long yy = y * width + x;

+				value_type val = 0;

+				for ( int i = 0 ; i < DownSample ; i++ )

+					for ( int j = 0; j < DownSample ; j++ )

+					{

+						int index = (y*DownSample+i) * m_width + x*DownSample+j;

+						val += m_gray[index];

+					}

+				val /= DownSample*DownSample;

+				//assert( val <= 255 );

+				m_blur0[yy] = val;

+				m_blur1[yy] = val;

+			}

+	}

+	}

+	// blur

+	dst = m_blur0;

+#define BLUR

+#ifdef BLUR		

+	#pragma omp parallel  //default(none) shared (width,height,src,dst)

+	{

+		

+		static const float moyen[] = {

+			1/9.0,1/9.0,1/9.0,

+			1/9.0,1/9.0,1/9.0,

+			1/9.0,1/9.0,1/9.0

+		};

+		static const float laplacien4[] = {

+			0,-1,0,

+			-1,4,-1,

+			0,-1,0

+		};

+		

+		static const float laplacien8[] = {

+			-1,-1,-1,

+			-1,8,-1,

+			-1,-1,-1

+		};

+		

+		static const float laplaciend[] = {

+			1,-2,1,

+			-2,4,-2,

+			1,-2,1

+		};

+		

+		static const float sobelv[] = {

+			1,0,-1,

+			2,0,-2,

+			1,0,-1

+		};

+		static const float sobelh[] = {

+			1,2,1,

+			0,0,0,

+			-1,2,-1

+		};

+		

+		static const float prewittv[] = {

+			1,0,-1,

+			1,0,-1,

+			1,0,-1

+		};

+		static const float prewitth[] = {

+			1,1,1,

+			0,0,0,

+			-1,-1,-1

+		};

+		//5x5 Gaussian filter

+		static const float gaussian[] = {

+			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0,

+			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,

+			5/159.0,	12/159.0,	15/159.0,	12/159.0,	5/159.0,

+			4/159.0,	9/159.0,	12/159.0,	9/159.0,	4/159.0,

+			2/159.0,	4/159.0,	5/159.0,	4/159.0,	2/159.0	

+		};

+	for ( int pass = 0 ; pass < BlurPass ; pass++ )

+	{

+		if ( (pass & 1) ==0 ) 

+		{

+		src = m_blur0;

+		dst = m_blur1;

+		}

+		else

+		{

+		src = m_blur1;

+		dst = m_blur0;

+		}

+		switch ( filter )

+		{

+		case FMoyen:

+			Filter(  moyen,3, src, dst );

+			break; 

+		case FLaplacien4:

+			Filter( laplacien4,3, src, dst );

+			break;

+		case FLaplacien8:

+			Filter( laplacien8, 3, src, dst );

+			break;

+		case FLaplaciend:

+			Filter( laplaciend, 3, src, dst );

+			break;

+		case FSobel:

+			Filter( sobelv, 3, src, m_filter_v );

+			Filter( sobelh, 3, src, m_filter_h );

+			CombineFilter( m_filter_v, m_filter_h, dst );

+			break;

+		case FPrewitt:

+			Filter( prewittv, 3, src, m_filter_v );

+			Filter( prewitth, 3, src, m_filter_h );

+			CombineFilter( m_filter_v, m_filter_h, dst );

+		case FGaussian:

+			Filter( gaussian, 5, src, dst );

+			break;

+		}

+		

+	}

+	}

+#endif	

+	switch ( Display )

+	{

+	case ARAnalyse::GrayScale:

+		DisplayGrayScale(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Blur:

+		DisplayBlur(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Diff:

+		DisplayDiff(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Threshold:

+		DisplayThreshold(cameraBuffer, dst );

+		break;

+	case ARAnalyse::Moyen:

+		DisplayMoyen(cameraBuffer, dst );

+		break;

+	}

+		

+}

+#endif

+void ARAnalyse::CombineFilter ( pixel_type *src_h, pixel_type *src_v, pixel_type *dst )

+{

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	#pragma omp for

+		for ( int y = height-2 ; y > 0  ; y-- )

+		{

+			//#pragma omp for

+			for ( int x = width-2; x > 0  ; x-- )

+			{

+				long yy;

+				value_type val = 0;

+				// kernel sum

+				yy = y * width + x;

+				

+				val += src_h[ yy ]	* src_h[ yy ];

+				val += src_v[ yy ]	* src_v[ yy ];

+				

+				dst[yy] = sqrt(val);

+				

+			}

+		}

+}

+void ARAnalyse::Filter ( const float *krn, int k_size, pixel_type *src, pixel_type *dst )

+{

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	const float *krn_ptr;

+	int half_kernel = k_size / 2;

+	#pragma omp for

+		for ( int y = height-1-half_kernel ; y >= half_kernel  ; y-- )

+		{

+			//#pragma omp for

+			for ( int x = width-1-half_kernel; x >= half_kernel  ; x-- )

+			{

+				long yy;

+				value_type val = 0;

+				// kernel sum

+				krn_ptr = krn;

+				yy = (y - half_kernel) * width + x ;

+				for ( int j = 0; j < k_size ; j++ )

+				{

+					for ( int i = 0; i < k_size ; i++ )

+						val += src[ yy - half_kernel + i ] * *krn_ptr++;

+				yy +=  width ;

+				}

+				

+				dst[y * width + x] = val;

+				

+			}

+		}

+}

+	

+void ARAnalyse::DisplayGrayScale (unsigned char * cameraBuffer, pixel_type * dest) 

+{

+	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 

+	{

+	#pragma omp for

+	for ( int y = 0; y < m_height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < m_width ; x++ )

+			{

+				long index = (y * m_width + x);

+				unsigned char  * pixel = &cameraBuffer[index*3];

+				value_type val = m_gray[index]; //display gray scale				

+				pixel[0] = (unsigned char)(int)(val);

+				pixel[1] = (unsigned char)(int)(val);

+				pixel[2] = (unsigned char)(int)(val);

+			}

+	}

+	}

+}

+void ARAnalyse::DisplayBlur (unsigned char * cameraBuffer, pixel_type * dest) 

+{

+	// Down sample

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 

+	{

+	#pragma omp for

+	for ( int y = 0; y < m_height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < m_width ; x++ )

+			{

+				long index = (y * m_width + x);

+				unsigned char  * pixel = &cameraBuffer[index*3];

+				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];

+				value_type val=0;

+				val =  blur; // display BLUR

+				

+				pixel[0] = (unsigned char)(int)(val);

+				pixel[1] = (unsigned char)(int)(val);

+				pixel[2] = (unsigned char)(int)(val);

+			}

+	}

+	}

+}

+void ARAnalyse::DisplayDiff (unsigned char * cameraBuffer, pixel_type * dest) 

+{

+	// Down sample

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 

+	{

+	#pragma omp for

+	for ( int y = 0; y < m_height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < m_width ; x++ )

+			{

+				long index = (y * m_width + x);

+				unsigned char  * pixel = &cameraBuffer[index*3];

+				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];

+				value_type val=0;

+				val = m_gray[index] - blur;

+				val += 127;

+				if ( val < 0 ) val = 0;

+				else if ( val > 255 ) val = 255;

+				

+				pixel[0] = (unsigned char)(int)(val);

+				pixel[1] = (unsigned char)(int)(val);

+				pixel[2] = (unsigned char)(int)(val);

+			}

+	}

+	}

+}

+void ARAnalyse::DisplayThreshold (unsigned char * cameraBuffer, pixel_type * dest) 

+{

+	// Down sample

+	int width = m_width / DownSample;

+	int height = m_height / DownSample;

+	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 

+	{

+	#pragma omp for

+	for ( int y = 0; y < m_height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < m_width ; x++ )

+			{

+				long index = (y * m_width + x);

+				unsigned char  * pixel = &cameraBuffer[index*3];

+				value_type blur =  dest[((y/DownSample) * width + (x/DownSample))];

+				value_type val=0;

+				val = m_gray[index] - blur * 0.9;

+						//val =  val  < 0 ? 0 : 255;

+						val += 127;

+						if ( val < g_ThresholdMin  ) val = 0;

+						else if ( val > g_ThresholdMax ) val = 255;

+						else val = 127;

+				

+				pixel[0] = (unsigned char)(int)(val);

+				pixel[1] = (unsigned char)(int)(val);

+				pixel[2] = (unsigned char)(int)(val);

+			}

+	}

+	}

+}

+void ARAnalyse::DisplayMoyen (unsigned char * cameraBuffer, pixel_type * dest) 

+{

+	#pragma omp parallel  //default(none) shared( cameraBuffer, width,height,dst ) 

+	{

+	#pragma omp for

+	for ( int y = 0; y < m_height ; y++ )

+	{

+			//#pragma omp for

+			for ( int x = 0; x < m_width ; x++ )

+			{

+				long index = (y * m_width + x);

+				unsigned char  * pixel = &cameraBuffer[index*3];

+				value_type val = (m_sum[index] / nb_images);

+				pixel[0] = (unsigned char)(int)(val);

+				pixel[1] = (unsigned char)(int)(val);

+				pixel[2] = (unsigned char)(int)(val);

+			}

+	}

+	}

+}

+

+void ARAnalyse::CalcMoyen()

+{

+	//DBGMSG(TEXT("Before CalcMoyen Frame %d first %d, last %d:\n"), FrameCount, m_first_image, m_last_image);

+            

+	// calcul sum - first 

+	if ( FrameCount > (nb_images-1) ) 

+	{

+		pixel_type *first_image = m_images[m_first_image];

+		for ( int i = 0; i < nb_pixel ; i++ )

+		{

+			m_sum[i] -= first_image[i];

+		}

+		m_first_image++;

+		if ( m_first_image >= nb_images ) m_first_image = 0;

+	}

+	// calcul sum + last = m_gray !

+	for ( int i = 0; i < nb_pixel ; i++ )

+	{

+		m_sum[i] += m_gray[i];

+		 m_images[m_last_image][i] = m_gray[i]; // copy input to last image

+		m_gray[i] = m_sum[i]/nb_images;			// copy and calculate moyen 

+	}

+	m_last_image++;

+	if ( m_last_image >= nb_images ) m_last_image = 0;

+	//DBGMSG(TEXT("After CalcMoyen first %d, last %d:\n"), m_first_image, m_last_image);

+    

+}
\ No newline at end of file