context: /*
      Sentience 3D Perception System
      Copyright (C) 2000-2007 Bob Mottram
      fuzzgun@gmail.com
  
      This program is free software: you can redistribute it and/or modify
      it under the terms of the GNU General Public License as published by
      the Free Software Foundation, either version 3 of the License, or
      (at your option) any later version.
  
      This program is distributed in the hope that it will be useful,
      but WITHOUT ANY WARRANTY; without even the implied warranty of
      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      GNU General Public License for more details.
  
      You should have received a copy of the GNU General Public License
      along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
  
  using System;
  using System.IO;
  using System.Xml;
  using System.Collections.Generic;
  using CenterSpace.Free;
  using sluggish.utilities;
  
  namespace sentience.core
  {
      /// <summary>
      /// fundamental stereo vision maths and models
      /// </summary>
      public sealed class stereoModel
      {
          // random number generator
          //MersenneTwister rnd = new MersenneTwister(0);
          Random rnd = new Random(0);
  
          public bool mapping = true;
  
          // field of vision
          public float FOV_horizontal = 78 * (float)Math.PI / 180.0f;  // horizontal field of vision
          public float FOV_vertical = 78/2 * (float)Math.PI / 180.0f;  // vertical field of vision
          public int image_width = 320;   // horizontal image size
          public int image_height = 240;  // vertical image size
          public float focal_length = 5;  // focal length in millimetres
          public float sensor_pixels_per_mm = 100;
          public float baseline = 100;    // distance between the centres of the two cameras in millimetres
          public float sigma = 0.04f;     //0.005f;  //angular uncertainty magnitude (standard deviation) pixels per mm of range
          private float max_prob = 0.0f;
          private int max_prob_x, max_prob_y;
          private int starting_y;
          public bool undistort = true;
          
          // vergence angle between the two cameras
          public float vergence_radians = 0.0f * (float)Math.PI / 180.0f;
  
          private int ray_model_normal_length = 500;
          private int ray_model_max_length = 1500;
          public rayModelLookup ray_model = null;
  
          // number of features to use when updating or probing the grid
          public int no_of_stereo_features = 100;
  
          public int divisor = 1;
  
          // Yes, it's prime time...
          private int[] primes = {    
     1019,   1021,   1031,   1033,   1039,  1049,  1051,   1061,   1063,   1069, 
     1087,   1091,  1093,   1097,   1103,   1109,   1117,   1123,   1129,   1151, 
     1153,   1163,  1171,   1181,   1187,   1193,   1201,   1213,   1217,  1223, 
     1229,   1231,   1237,   1249,   1259,   1277,   1279,   1283,   1289,   1291, 
     1297,   1301,   1303,   1307,   1319,   1321,   1327,   1361,   1367,   1373, 
     1381,   1399,   1409,   1423,   1427,   1429,   1433,   1439,   1447,   1451, 
     1453,   1459,   1471,   1481,   1483,   1487,   1489,   1493,   1499,   1511, 
     1523,   1531,   1543,   1549,   1553,   1559,   1567,   1571,   1579,   1583, 
     1597,   1601,   1607,   1609,   1613,   1619,   1621,   1627,   1637,   1657, 
     1663,   1667,   1669,   1693,   1697,   1699,   1709,   1721,   1723,   1733, 
     1741,   1747,   1753,   1759,   1777,   1783,   1787,   1789,   1801,   1811, 
     1823,   1831,   1847,   1861,   1867,   1871,   1873,   1877,   1879,   1889, 
     1901,   1907,   1913,   1931,   1933,   1949,   1951,   1973,   1979,   1987, 
     1993,   1997,   1999,   2003,   2011,   2017,   2027,   2029,   2039,   2053, 
     2063,   2069,   2081,   2083,   2087,   2089,   2099,   2111,   2113,   2129, 
     2131,   2137,   2141,   2143,   2153,   2161,   2179,   2203,   2207,   2213, 
     2221,   2237,   2239,   2243,   2251,   2267,   2269,   2273,   2281,   2287, 
     2293,   2297,   2309,   2311,   2333,   2339,   2341,   2347,   2351,   2357, 
     2371,   2377,   2381,   2383,   2389,   2393,   2399,   2411,   2417,   2423, 
     2437,   2441,   2447,   2459,   2467,   2473,   2477,   2503,   2521,   2531, 
     2539,   2543,   2549,   2551,   2557,   2579,   2591,   2593,   2609,   2617, 
     2621,   2633,   2647,   2657,   2659,   2663,   2671,   2677,   2683,   2687, 
     2689,   2693,   2699,   2707,   2711,   2713,   2719,   2729,   2731,   2741, 
     2749,   2753,   2767,   2777,   2789,   2791,   2797,   2801,   2803,   2819, 
     2833,   2837,   2843,   2851,   2857,   2861,   2879,   2887,   2897,   2903, 
     2909,   2917,   2927,   2939,   2953,   2957,   2963,   2969,   2971,   2999, 
     3001,   3011,   3019,   3023,   3037,   3041,   3049,   3061,   3067,   3079, 
     3083,   3089,   3109,   3119,   3121,   3137,   3163,   3167,   3169,   3181, 
     3187,   3191,   3203,   3209,   3217,   3221,   3229,   3251,   3253,   3257, 
     3259,   3271,   3299,   3301,   3307,   3313,   3319,   3323,   3329,   3331, 
     3343,   3347,   3359,   3361,   3371,   3373,   3389,   3391,   3407,   3413, 
     3433,   3449,   3457,   3461,   3463,   3467,   3469,   3491,   3499,   3511, 
     3517,   3527,   3529,   3533,   3539,   3541,   3547,   3557,   3559,   3571, 
     3581,   3583,   3593,   3607,   3613,   3617,   3623,   3631,   3637,   3643, 
     3659,   3671,   3673,   3677,   3691,   3697,   3701,   3709,   3719,   3727, 
     3733,   3739,   3761,   3767,   3769,   3779,   3793,   3797,   3803,   3821, 
     3823,   3833,   3847,   3851,   3853,   3863,   3877,   3881,   3889,   3907, 
     3911,   3917,   3919,   3923,   3929,   3931,   3943,   3947,   3967,   3989, 
     4001,   4003,   4007,   4013,   4019,   4021,   4027,   4049,   4051,   4057, 
     4073,   4079,   4091,   4093,   4099,   4111,   4127,   4129,   4133,   4139, 
     4153,   4157,   4159,   4177,   4201,   4211,   4217,   4219,   4229,   4231, 
     4241,   4243,   4253,   4259,   4261,   4271,   4273,   4283,   4289,   4297, 
     4327,   4337,   4339,   4349,   4357,   4363,   4373,   4391,   4397,   4409, 
     4421,   4423,  4441,   4447,   4451,   4457,   4463,   4481,   4483,   4493, 
     4507,   4513,   4517,   4519,   4523,   4547,   4549,   4561,   4567,   4583, 
     4591,   4597,   4603,   4621,   4637,   4639,   4643,   4649,   4651,   4657, 
     4663,   4673,   4679,   4691,   4703,   4721,   4723,   4729,   4733,   4751, 
     4759,   4783,   4787,   4789,   4793,   4799,   4801,   4813,   4817,   4831,
     4861,   4871,   4877,   4889,   4903,   4909,   4919,   4931,   4933,   4937, 
     4943,   4951,   4957,   4967,   4969,   4973,   4987,   4993,   4999,   5003, 
     5009,   5011,   5021,   5023,   5039,   5051,   5059,   5077,  5081,   5087 };
  
  
          #region "converting disparity to distance"
          
          /// <summary>
          /// returns the distance for the given stereo disparity
          /// </summary>
          /// <param name="disparity_pixels">disparity in pixels</param>
          /// <param name="focal_length_mm">focal length in millimetres</param>
          /// <param name="sensor_pixels_per_mm">number of pixels per millimetre on the sensor chip</param>
          /// <param name="baseline_mm">distance between cameras in millimetres</param>
          /// <returns>range in millimetres</returns>        
          public static float DisparityToDistance(float disparity_pixels,
                                                  float focal_length_mm,
                                                  float sensor_pixels_per_mm,
                                                  float baseline_mm)
          {
              float focal_length_pixels = focal_length_mm * sensor_pixels_per_mm;
              float distance_mm = baseline_mm * focal_length_pixels / disparity_pixels;
              return(distance_mm);
          }
  
          /// <summary>
          /// returns the distance for the given stereo disparity
          /// </summary>
          /// <param name="disparity_pixels">disparity in pixels</param>
          /// <param name="focal_length_pixels">focal length in pixels</param>
          /// <param name="baseline_mm">distance between cameras in millimetres</param>
          /// <returns>range in millimetres</returns>        
          public static float DisparityToDistance(float disparity_pixels,
                                                  float focal_length_pixels,
                                                  float baseline_mm)
          {
              float distance_mm = baseline_mm * focal_length_pixels / disparity_pixels;
              return(distance_mm);
          }        
          
          #endregion
  
          #region "calculation of the sensor model lookup table"
  
          /// <summary>
          /// creates a lookup table for sensor models at different visual disparities
          /// </summary>
          public void createLookupTables(stereoHead robot_head, int gridCellSize_mm)
          {
              int width = 100;
              int height = 100;
              Byte[] img_result = new Byte[width * height * 3];
  
              for (int cam = 0; cam < robot_head.no_of_stereo_cameras; cam++)
              {
                  // update parameters based upon the calibration data
                  image_width = robot_head.calibration[cam].leftcam.image_width;
                  image_height = robot_head.calibration[cam].leftcam.image_height;
                  baseline = robot_head.calibration[cam].baseline;
                  FOV_horizontal = robot_head.calibration[cam].leftcam.camera_FOV_degrees * (float)Math.PI / 180.0f;
                  FOV_vertical = FOV_horizontal * image_height / image_width;
  
                  // create the lookup
                  createLookupTable(gridCellSize_mm, img_result, width, height);
  
                  // attach the lookup table to the relevant camera
                  robot_head.sensormodel[cam] = ray_model;
              }
          }
  
          public void createLookupTable(int gridCellSize_mm)
          {
              int width = 320;
              int height = 240;
              byte[] img_result = new byte[width * height * 3];
              createLookupTable(gridCellSize_mm, img_result, width, height);
          }
  
          public void createLookupTable(
  		    int gridCellSize_mm, 
  		    int image_width, 
  		    int image_height)
          {
  			this.image_width = image_width;
  			this.image_height = image_height;
              byte[] img_result = new byte[image_width * image_height * 3];
              createLookupTable(gridCellSize_mm, img_result, image_width, image_height);
          }		
  		
          /// <summary>
          /// creates a lookup table for sensor models at different visual disparities
          /// </summary>
          /// <param name="img_result"></param>
          /// <param name="width"></param>
          /// <param name="height"></param>
          public void createLookupTable(
  		    int gridCellSize_mm, 
  		    byte[] img_result, 
  		    int width, int height)
          {
              bool mirror = false;
              int divisor = 40;
              int grid_dimension = 20000;
              float[,,] grid_layer = new float[grid_dimension / divisor, grid_dimension, 3];
              updateRayModel(grid_layer, grid_dimension, 
                             img_result, width, height,
                             divisor, false, gridCellSize_mm, mirror);
              ray_model.Save("InverseSensorModels.xml");
          }
  
          /// <summary>
          /// Ok, so we've calculated the ray models in stupifying detail
          /// now let's compress them down to some more compact and less memory-hogging form
          /// so that each array element corresponds to a single occupancy grid cell, which
          /// makes updating the grid a simple process
          /// </summary>
          /// <param name="gridCellSize_mm">Size of each occupancy grid cell in millimetres</param>
          private void compressRayModels(int gridCellSize_mm)
          {
              if (ray_model != null)
              {
                  ray_model_normal_length = ray_model.dimension_probability / gridCellSize_mm;
  
                  rayModelLookup new_ray_model = new rayModelLookup(ray_model.dimension_disparity, ray_model_normal_length);
  
                  for (int d = 1; d < ray_model.dimension_disparity; d++)
                  {
                      int prev_index = 0;
                      for (int i = 0; i < ray_model_normal_length; i++)
                      {
                          int next_index = (i + 1) * ray_model.dimension_probability / new_ray_model.dimension_probability;
                          // sum the probabilities
                          float total_probability = 0;
                          for (int j = prev_index; j < next_index; j++)
                          {
                              // is there a DJ in the house ?
                              total_probability += ray_model.probability[d][j];
                          }
                          if (total_probability > 0)
                              new_ray_model.probability[d][i] = total_probability;
                          prev_index = next_index;
                      }
                      new_ray_model.length[d] = ray_model.length[d] * new_ray_model.dimension_probability / ray_model.dimension_probability;
  
                      // if there's only one grid cell give it the full probability
                      // (it's gotta be in there somewhere!)
                      if (new_ray_model.length[d] == 1) new_ray_model.probability[d][0] = 1.0f;
                  }
  
                  // finally swap the arrays
                  ray_model = new_ray_model;
                  //ray_model_length = new_ray_model_length;
              }
          }
  
          /// <summary>
          /// creates a lookup table for sensor models
          /// </summary>
          /// <param name="grid_layer">the grid upon which the rays will be drawn</param>
          /// <param name="grid_dimension">size of the grid</param>
          /// <param name="img">image within which to display the results</param>
          /// <param name="img_width">width of the image</param>
          /// <param name="img_height">height of the image</param>
          /// <param name="divisor">a dividing factor used to make the width of the grid smaller than its length</param>
          /// <param name="apply_smoothing">try to smooth the data to remove pixelation effects</param>
          /// <param name="gridCellSize_mm">Size of each occupancy grid cell in millimetres</param>
          /// <param name="mirror">apply mirroring</param>
          private void updateRayModel(
  		    float[, ,] grid_layer, int grid_dimension, 
              byte[] img, int img_width, int img_height, 
              int divisor, bool apply_smoothing,
              int gridCellSize_mm, bool mirror)
          {
              // half a pixel of horizontal uncertainty
              sigma = 1.0f / (image_width * 1) * FOV_horizontal;
              //sigma *= image_width / 320;
              this.divisor = divisor;
  
              float max_disparity = (10 * image_width / 100);
  
              ray_model_max_length = grid_dimension;
              ray_model = new rayModelLookup((int)(max_disparity) + 1, ray_model_max_length);
  
              int min_dist = (int)baseline;
              int max_dist = grid_dimension;
              int x = (image_width)*499/1000;
  
              for (float disparity_pixels = 1; disparity_pixels < max_disparity; disparity_pixels += 0.5f)
              {
                  float xx = x + disparity_pixels;
  
                  // clear the grid
                  max_prob = 0.0f;
                  for (int gx = 0; gx < grid_dimension / divisor; gx++)
                      for (int gy = 0; gy < grid_dimension; gy++)
                      {
                          grid_layer[gx, gy, 0] = 0;
                          grid_layer[gx, gy, 1] = 0;
                          grid_layer[gx, gy, 2] = 0;
                      }
  
                  float x_start = 0, y_start = 0;
                  float x_end = 0, y_end = 0;
                  float x_left = 0, y_left = 0;
                  float x_right = 0, y_right = 0;
                  float confidence = 1;
                  float distance = 100;
                  raysIntersection(xx, x, grid_dimension, confidence, distance,
                                   ref x_start, ref y_start, ref x_end, ref y_end,
                                   ref x_left, ref y_left, ref x_right, ref y_right);
                  if (x_right > x_left)
                  {
                      if (y_start < -1) y_start = -y_start;
                      if (y_end < -1) y_end = -y_end;
  
                      if (y_start > 0)
                          min_dist = (int)y_start;
                      else
                          min_dist = 100;
  
                      max_dist = grid_dimension;
                      if ((y_end > 0) && (y_end < grid_dimension))
                          max_dist = (int)y_end;
  
                      throwRay(-baseline / 2, xx, vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 1, mirror);
                      throwRay(baseline / 2, x, -vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 2, mirror);
  
                      int start = -1;
  
                      float min_prob = 0.0f;
                      int max_length = 0;
                      int x2 = (grid_layer.GetLength(0) / 2);
                      int winner = x2;
                      float total_probability = 0;
                      for (int xx2 = x2 - (grid_dimension / (divisor * 4)); xx2 <= x2 + (grid_dimension / (divisor * 4)); xx2++)
                      {
                          int length = 0;
                          float tot = 0;
                          for (int l = 0; l < ray_model_max_length; l++)
                              if ((grid_layer[xx2, l, 2] == 2) && 
                                  (grid_layer[xx2, l, 0] > 1))
                              {
                                  float cellval = grid_layer[xx2, l, 1];
                                  if (cellval > min_prob)
                                  {
                                      length++;
                                      tot += cellval;
                                  }
                              }
                          if (length > max_length)
                          {
                              max_length = length;
                              winner = xx2;
                              total_probability = tot;
                          }
                      }
                      float scaling_factor = 1;
                      if (total_probability > 0)
                      {
                          if (max_length > 0) scaling_factor = 1.0f / total_probability;
  
                          // record the length of the ray model
                          ray_model.length[(int)(disparity_pixels * 2)] = max_length+1;
  
                          float max = 0;
                          int max_index = 0;
                          for (int l = 0; l < ray_model_max_length; l++)
                          {
                              int y2 = ray_model_max_length - 1 - l;
                              if ((y2 > -1) && (y2 < grid_dimension))
                              {
                                  if ((grid_layer[winner, y2, 2] == 2) &&
                                      (grid_layer[winner, y2, 1] != 0) &&
                                      (grid_layer[winner, y2, 0] > 1))
                                  {
                                      float cellval = grid_layer[winner, y2, 1] * scaling_factor;
                                      if (cellval > min_prob)
                                      {
                                          if (start == -1) start = l;
                                          ray_model.probability[(int)(disparity_pixels * 2)][l - start] = cellval;
                                          if (cellval > max)
                                          {
                                              max = cellval;
                                              max_index = l - start;
                                          }
                                      }
                                  }
                              }
                          }
  
  
                          if (apply_smoothing)
                          {
                              float[] probvalues = new float[ray_model_max_length];
                              int radius = 20;
                              for (int itt = 0; itt < 10; itt++)
                              {
                                  for (int i = 0; i < ray_model_max_length; i++)
                                  {
                                      float value = 0;
                                      int hits = 0;
                                      for (int j = i - radius; j < i + radius; j++)
                                      {
                                          if ((j >= 0) && (j < ray_model_max_length))
                                          {
                                              value += ((j - (i - radius)) * ray_model.probability[(int)(disparity_pixels * 2)][j]);
                                              hits++;
                                          }
                                      }
                                      if (hits > 0) value /= hits;
                                      probvalues[i] = value;
                                  }
                                  for (int i = 0; i < ray_model_max_length; i++)
                                      if (ray_model.probability[(int)(disparity_pixels * 2)][i] > max / 200.0f)
                                          ray_model.probability[(int)(disparity_pixels * 2)][i] = probvalues[i];
                              }
                          }
                      }
                      
                  }
              }
  
              // compress the array down to a more managable size
              compressRayModels(gridCellSize_mm);
  
              ray_model_to_graph_image(img, img_width, img_height);
  
              this.divisor = 1;
          }
  
          #endregion
  
          /// <summary>
          /// show a single ray model, with both occupancy and vacancy
          /// </summary>
          /// <param name="grid_layer">the grid upon which the rays will be drawn</param>
          /// <param name="grid_dimension">size of the grid</param>
          /// <param name="img">image within which to display the result</param>
          /// <param name="img_width">width of the image</param>
          /// <param name="img_height">height of the image</param>
          /// <param name="divisor">a dividing factor used to make the width of the grid smaller than its length</param>
          /// <param name="show_outline">draw a border around the probably occupied area</param>
          /// <param name="mirror">apply mirroring</param>
          public void showSingleRay(float[, ,] grid_layer, int grid_dimension, 
                                    byte[] img, int img_width, int img_height, 
                                    int divisor, bool show_outline, bool mirror)
          {
              // half a pixel of horizontal uncertainty
              sigma = 1.8f / (image_width * 1) * FOV_horizontal;
              sigma *= image_width / 320;
              this.divisor = divisor;
  
              int min_dist = (int)baseline;
              int max_dist = grid_dimension;
              int x = (image_width) * 499 / 1000;
              
              //x = (image_width) * 550 / 1000;
  
              float disparity_pixels = 3;
  
              float xx = x + disparity_pixels;
  
              // clear the grid
              max_prob = 0.0f;
              for (int gx = 0; gx < grid_dimension / divisor; gx++)
                  for (int gy = 0; gy < grid_dimension; gy++)
                  {
                      grid_layer[gx, gy, 0] = 0;
                      grid_layer[gx, gy, 1] = 0;
                      grid_layer[gx, gy, 2] = 0;
                  }
  
              float x_start = 0, y_start = 0;
              float x_end = 0, y_end = 0;
              float x_left = 0, y_left = 0;
              float x_right = 0, y_right = 0;
              float confidence = 1;
              float distance = 100;
              raysIntersection(xx, x, grid_dimension, confidence, distance,
                               ref x_start, ref y_start, ref x_end, ref y_end,
                               ref x_left, ref y_left, ref x_right, ref y_right);
              if (x_right > x_left)
              {
                  if (y_start < -1) y_start = -y_start;
                  if (y_end < -1) y_end = -y_end;
  
                  if (y_start > 0)
                      min_dist = (int)y_start;
                  else
                      min_dist = 100;
  
                  max_dist = grid_dimension;
                  if ((y_end > 0) && (y_end < grid_dimension))
                      max_dist = (int)y_end;
  
                  min_dist = 0;
  
                  throwRay(-baseline / 2, xx, vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 1,mirror);
                  throwRay(baseline / 2, x, -vergence_radians, min_dist, max_dist, grid_layer, grid_dimension, 2,mirror);
                  grid_layer_to_image(grid_layer, grid_dimension, img, img_width, img_height, true);
  
                  if (show_outline)
                  {
  
                      drawing.drawLine(img, img_width, img_height, (int)(x_left * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_left * img_height / grid_dimension),
                                   (int)(x_right * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_right * img_height / grid_dimension),
                                   255, 0, 0, 0, false);
                      drawing.drawLine(img, img_width, img_height, (int)(x_left * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_left * img_height / grid_dimension),
                                   (int)(x_start * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_start * img_height / grid_dimension),
                                   255, 0, 0, 0, false);
                      drawing.drawLine(img, img_width, img_height, (int)(x_right * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_right * img_height / grid_dimension),
                                   (int)(x_start * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_start * img_height / grid_dimension),
                                   255, 0, 0, 0, false);
                      drawing.drawLine(img, img_width, img_height, (int)(x_left * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_left * img_height / grid_dimension),
                                   (int)(x_end * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_end * img_height / grid_dimension),
                                   255, 0, 0, 0, false);
                      drawing.drawLine(img, img_width, img_height, (int)(x_right * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_right * img_height / grid_dimension),
                                   (int)(x_end * img_width / (grid_dimension / divisor)), img_height - 1 - (int)(y_end * img_height / grid_dimension),
                                   255, 0, 0, 0, false);
                  }
  
              }
              this.divisor = 1;
              
          }
  
  
          /// <summary>
          /// create a list of rays to be stored within poses
          /// </summary>
          /// <param name="head">head configuration</param>
          /// <param name="stereo_camera_index">index number for the stereo camera</param>
          /// <returns>list of evidence rays, centred at (0, 0, 0)</returns>
          public List<evidenceRay> createObservation(
  		    stereoHead head, 
  		    int stereo_camera_index)
          {
              List<evidenceRay> result;
  			
  			if (head.features[stereo_camera_index] != null)
  			{
  	            result = createObservation(
  			        head.cameraPosition[stereo_camera_index],
  			        head.calibration[stereo_camera_index].baseline,
  			        head.calibration[stereo_camera_index].leftcam.image_width,
  			        head.calibration[stereo_camera_index].leftcam.image_height,
  			        head.calibration[stereo_camera_index].leftcam.camera_FOV_degrees,
  			        head.features[stereo_camera_index].features,
  			        head.features[stereo_camera_index].colour,
  			        head.features[stereo_camera_index].uncertainties,
  				    false);
  			}
  			else
  			{
  				result = new List<evidenceRay>();
  			}
  			
              return (result);
          }
  
  		/// <summary>
  		/// returns a set of evidence rays based upon the given observation
  		/// </summary>
  		/// <param name="observer">observer position and orientation</param>
  		/// <param name="baseline">baseline distance</param>
  		/// <param name="image_width">camera image width</param>
  		/// <param name="image_height">camera image height</param>
  		/// <param name="FOV_degrees">horizontal camera field of view in degrees</param>
  		/// <param name="stereo_features">stereo features, as groups of three figures</param>
  		/// <param name="stereo_features_colour">colour of each stereo feature</param>
  		/// <param name="stereo_features_uncertainties">uncertainty value associated with each stereo feature</param>
  		/// <param name="translate">translate the ray to be relative to the observer position, otherwise it's relative to (0,0,0)</param>
  		/// <returns>list of evidence rays, centred at (0, 0, 0)</returns>
          public List<evidenceRay> createObservation(
  		    pos3D observer,
  		    float baseline,
  		    int image_width,
  		    int image_height,
  		    float FOV_degrees,
  		    float[] stereo_features,
  		    byte[,] stereo_features_colour,
  		    float[] stereo_features_uncertainties,
  		    bool translate)
          {
              List<evidenceRay> result = new List<evidenceRay>();
  
              // get essential data for this stereo camera
              FOV_horizontal = FOV_degrees * (float)Math.PI / 180.0f;
              FOV_vertical = FOV_horizontal * image_height / image_width;
  
              // calc uncertainty in angle (+/- half a pixel)
              float angular_uncertainty = FOV_horizontal / (image_width * 2);
  			//sigma = 100 * (float)Math.Tan(angular_uncertainty); // 100 is the factor used in RaysIntersection
  			sigma = 1.0f / (image_width * 1) * FOV_horizontal;
  
              // some head geometry
              pos3D headOrientation = observer;
              pos3D cameraOrientation = observer;
  			if (!translate) cameraOrientation = new pos3D(0, 0, 0);
              cameraOrientation.pan = headOrientation.pan;
              cameraOrientation.tilt = headOrientation.tilt;
              cameraOrientation.roll = headOrientation.roll;
  
              if (stereo_features != null)  // if there are stereo features associated with this camera
              {
                  int f2 = 0;
                  for (int f = 0; f < stereo_features.Length; f += 3)
                  {
                      // get the parameters of the feature
                      float image_x = stereo_features[f];
                      float image_y = stereo_features[f + 1];
                      float disparity = stereo_features[f + 2];
  
                      // create a ray
                      evidenceRay ray = 
  						createRay(
  						    image_x, image_y, disparity, 
  						    1 + stereo_features_uncertainties[f2],
                              stereo_features_colour[f2, 0],
                              stereo_features_colour[f2, 1],
                              stereo_features_colour[f2, 2]);
  
                      if (ray != null)
                      {
                          // convert from camera-centric coordinates to head coordinates
                          ray.translateRotate(cameraOrientation);
  
                          // add to the result
                          result.Add(ray);
                      }
                      f2++;
                  }
              }
  
              return (result);
          }
  
  		
          /// <summary>
          /// shows the gaussian distribution thrown into the grid
          /// this is only used for visualisation/debugging
          /// </summary>
          /// <param name="img">image data</param>
          /// <param name="img_width">width of the image</param>
          /// <param name="img_height">height of the image</param>
          public void showDistribution(byte[] img, int img_width, int img_height)
          {
              for (int i = 0; i < img.Length; i++) img[i] = 0;
  
              for (int x = 0; x < img_width; x++)
              {
                  float prob = Gaussian((x - (img_width / 2)) / (float)(img_width / 2));
                  for (int y = img_height - 1; y > img_height - 1 - (int)(prob * img_height); y--)
                  {
                      int n = ((img_width * y) + x) * 3;
                      img[n + 1] = 255;
                  }
              }
          }
  
  
  
          /// <summary>
          /// do the two given rays intersect?  If so then return the intersection point
          /// this isn't really used anymore and should probably be removed
          /// </summary>
          /// <param name="ray1">the first ray</param>
          /// <param name="ray2">the second ray</param>
          /// <param name="intersectPos">intersection point of the two rays</param>
          /// <returns>true if the rays overlap</returns>
          /*
          public static bool raysOverlap(evidenceRay ray1, evidenceRay ray2,
                                         pos3D intersectPos, float separation_tollerance,
                                         int ray_thickness, ref float separation)
          {
              bool intersect = false;
              float x0, y0, x1, y1, x2, y2, x3, y3, x4, y4, x5, y5;
              float xi = 0, yi = 0;
  
gt:             float w = (ray1.width + ray2.width) * separation_tollerance;