Tinyslam /Coreslam

最简单的激光slam方法，中文注释是我自己加的，有些理解不到位可以和我交流交流

#include 
#include 
#include 
#ifdef _MSC_VER
   typedef __int64 int64_t;	// Define it from MSVC‘s internal type
#else
   #include 		// Use the C99 official header
#endif
#include “CoreSLAM.h“

void 
ts_map_init（ts_map_t *map）                                //初始化地图把地图每个栅格的值（value）都置为（TS_OBSTACLE + TS_NO_OBSTACLE） / 2；介于障碍和无障碍之间（未知）
{
    int x y initval;
    ts_map_pixel_t *ptr;
    initval = （TS_OBSTACLE + TS_NO_OBSTACLE） / 2;
    for （ptr = map->map y = 0; y < TS_MAP_SIZE; y++） {
	for （x = 0; x < TS_MAP_SIZE; x++ ptr++） {
	    *ptr = initval;
	}
    }
}

int
ts_distance_scan_to_map（ts_scan_t *scan ts_map_t *map ts_position_t *pos）               //把机器人坐标系下的障碍点映射到全局坐标系地图下，计算
{
    double c s;
    int i x y nb_points = 0;
    int64_t sum;

    c = cos（pos->theta * M_PI / 180）;
    s = sin（pos->theta * M_PI / 180）;
    // Translate and rotate scan to robot position
    // and compute the distance
    for （i = 0 sum = 0; i != scan->nb_points; i++） {
        if （scan->value[i] != TS_NO_OBSTACLE） {
            x = （int）floor（（pos->x + c * scan->x[i] - s * scan->y[i]） * TS_MAP_SCALE + 0.5）;    //x，y是雷达点在全局地图中所在的的栅格行数列数
            y = （int）floor（（pos->y + s * scan->x[i] + c * scan->y[i]） * TS_MAP_SCALE + 0.5）;
            // Check boundaries
            if （x >= 0 && x < TS_MAP_SIZE && y >= 0 && y < TS_MAP_SIZE） {
                sum += map->map[y * TS_MAP_SIZE + x];                           //sum计算这一帧每个雷达点转化到全局地图中所在的栅格点的值（value）之和
                nb_points++;
            }
        }
    }
    if （nb_points） sum = sum * 1024 / nb_points;
    else sum = 2000000000;
    return （int）sum;
}

#define SWAP（x y） （x ^= y ^= x ^= y）

void
ts_map_laser_ray（ts_map_t *map int x1 int y1 int x2 int y2 
                 int xp int yp int value int alpha）
																	// x1 y1: 机器人在全局地图的位置，单位:cm
																	// x2 y2: 某一个雷达点所在的“圆洞”（holes）的最远端在全局地图的位置，单位：cm
																	// xp yp: 某一个雷达点在全局地图的位置，单位：cm 
																	// alpha: TS_NO_OBSTACLE时， q = 50 / 4; TS_OBSTACLE。
																	// 如果该雷达点处值（栅格值）为TS_NO_OBSTACLEvalue则为TS_NO_OBSTACLE，65500
																	// 如果其他情况value则为TS_NO_OBSTACLE

{
    int x2c y2c dx dy dxc dyc error errorv derrorv x;
    int incv sincv incerrorv incptrx incptry pixval horiz diago;
    ts_map_pixel_t *ptr;

    if （x1 < 0 || x1 >= TS_MAP_SIZE || y1 < 0 || y1 >= TS_MAP_SIZE）
        return; // Robot is out of map
    
    x2c = x2; y2c = y2;
    // Clipping                                                                 
	// 如果雷达点的位置超出地图范围，就取雷达点所在的那条线与地图的交点处为（x2c，y2c）；
	// 否则雷达点位置未超出地图范围，就取真实雷达点坐标为（x2c，y2c）；
    if （x2c < 0） {
        if （x2c == x1） return;
        y2c += （y2c - y1） * （-x2c） / （x2c - x1）;
        x2c = 0;
    }
    if （x2c >= TS_MAP_SIZE） {
        if （x1 == x2c） return;
        y2c += （y2c - y1） * （TS_MAP_SIZE - 1 - x2c） / （x2c - x1）;
        x2c = TS_MAP_SIZE - 1;
    }
    if （y2c < 0） {
        if （y1 == y2c） return;
        x2c +=

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-03-26 17:07  TINYSLAM\
     文件        1017  2011-05-11 16:33  TINYSLAM\CMakeLists.txt
     文件        9363  2016-03-15 15:42  TINYSLAM\CoreSLAM.c
     文件        4713  2016-03-15 22:09  TINYSLAM\CoreSLAM.h
     文件        3191  2011-05-11 16:33  TINYSLAM\CoreSLAM_ext.c
     文件        1666  2011-05-11 16:33  TINYSLAM\CoreSLAM_loop_closing.c
     文件        4060  2016-03-09 21:09  TINYSLAM\CoreSLAM_random.c
     文件        5283  2011-05-11 16:34  TINYSLAM\CoreSLAM_state.c
     文件         332  2011-05-11 16:34  TINYSLAM\Makefile.am
     文件          33  2011-05-11 16:34  TINYSLAM\authors.txt
     文件          69  2011-05-11 16:34  TINYSLAM\bootstrap.txt
     文件         774  2011-05-11 16:35  TINYSLAM\cible.bmp
     文件        1863  2011-05-11 16:34  TINYSLAM\configure.ac
     文件     1344527  2011-05-11 16:35  TINYSLAM\exp4.dat
     文件         652  2011-05-11 16:35  TINYSLAM\gnuplot.txt
     文件       12342  2011-05-11 16:35  TINYSLAM\icone.bmp
     文件           0  2011-05-11 16:34  TINYSLAM\news.txt
     文件          47  2011-05-11 16:34  TINYSLAM\readme.txt
     文件     1323008  2011-05-11 16:34  TINYSLAM\test_lab.dat
     文件      901120  2011-05-11 16:35  TINYSLAM\test_lab2.dat
     文件       13306  2016-03-18 15:13  TINYSLAM\test_lab_reverse.c
     文件       11660  2011-05-11 16:34  TINYSLAM\test_loop_closing.c