Create coreslam.h

c/coreslam.h

@@ -0,0 +1,177 @@
+/*
+
+coreslam.h adapted from CoreSLAM.h downloaded from openslam.org on 01 January 2014.  
+Contains efficient implementations of scan and map structures.
+
+Copyright (C) 2014 Simon D. Levy
+
+This code is free software: you can redistribute it and/or modify
+it under the terms of the GNU Lesser General Public License as 
+published by the Free Software Foundation, either version 3 of the 
+License, or (at your option) any later version.
+
+This code 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 Lesser General Public License 
+along with this code.  If not, see <http:#www.gnu.org/licenses/>.
+*/
+
+/* Default parameters --------------------------------------------------------*/
+
+static const int    DEFAULT_MAP_QUALITY         = 50; /* out of 255 */
+static const double DEFAULT_HOLE_WIDTH_MM       = 600;
+
+static const double DEFAULT_SIGMA_XY_MM         = 100;
+static const double DEFAULT_SIGMA_THETA_DEGREES = 20;
+
+static const double DEFAULT_MAX_SEARCH_ITER     = 1000;
+
+
+/* Core types --------------------------------------------------------------- */
+
+typedef struct position_t
+{
+    double x_mm;
+    double y_mm;
+    double theta_degrees;
+    
+} position_t;
+
+typedef unsigned short pixel_t;
+
+typedef struct map_t {
+    
+    pixel_t * pixels;
+    int size_pixels;
+    double size_meters;
+    
+    double scale_pixels_per_mm;
+    
+} map_t;
+
+
+typedef struct scan_t
+{
+    double * x_mm;
+    double * y_mm;
+    int * value;
+    int npoints;
+    int span;
+    
+    /* for SSE */
+    float * obst_x_mm;
+    float * obst_y_mm;
+    int obst_npoints;
+        
+} scan_t;
+
+
+typedef struct laser_t
+{
+    int scan_size;                      /* number of points per scan */
+    double scan_rate_hz;                /* scans per second */
+    double detection_angle_degrees;     /* e.g. 240, 360 */
+    double distance_no_detection_mm;    /* default value when the laser returns 0 */
+    int detection_margin;               /* first scan element to consider */
+    double offset_mm;                   /* position of the laser wrt center of rotation */
+    
+} laser_t;
+
+
+/* Exported functions ------------------------------------------------------- */
+
+#ifdef __cplusplus 
+extern "C" 
+{
+#endif
+    
+int * 
+int_alloc(
+    int size);
+
+void 
+map_init(
+    map_t * map, 
+    int size_pixels, 
+    double size_meters);
+
+void
+map_free(
+    map_t * map);
+
+void map_string(
+    map_t map,
+    char * str);
+
+void
+map_update(
+    map_t * map, 
+    scan_t * scan, 
+    position_t position,
+    int map_quality, 
+    double hole_width_mm);
+
+void scan_init(
+    scan_t * scan, 
+    int size,
+    int span);
+
+void 
+scan_free(
+    scan_t * scan);
+
+void scan_string(
+    scan_t scan,
+    char * str);
+
+void 
+scan_update(
+    scan_t * scan, 
+    int * lidar_mm, 
+    laser_t laser,
+    double hole_width_mm,
+    double velocities_dxy_mm,
+    double velocities_dtheta_degrees);
+
+void
+map_get(
+    map_t * map, 
+    char * bytes);
+
+void
+map_set(
+    map_t * map, 
+    char * bytes);
+    
+void 
+laser_string(
+    laser_t laser, 
+    char * str);
+
+/* Returns -1 for infinity */
+int 
+distance_scan_to_map(
+    map_t *  map,
+    scan_t * scan,
+    position_t position);
+
+
+/* Random-Mutation Hill-Climbing search */
+position_t 
+rmhc_position_search(
+    position_t start_pos,
+	map_t * map,
+    scan_t * scan,
+    laser_t laser,
+	double sigma_xy_mm,
+	double sigma_theta_degrees,
+	int max_search_iter,
+	void * randomizer);
+
+#ifdef __cplusplus 
+}
+#endif
+