Removed obsolete files referencing laser_t datatype
This commit is contained in:
Simon D. Levy
@@ -1,85 +0,0 @@
|
|||||||
/**
|
|
||||||
*
|
|
||||||
* BreezySLAM: Simple, efficient SLAM in C++
|
|
||||||
*
|
|
||||||
* Laser.cpp - C++ code for Laser model class
|
|
||||||
*
|
|
||||||
* 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/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <stdio.h>
|
|
||||||
#include <math.h>
|
|
||||||
|
|
||||||
#include <iostream>
|
|
||||||
#include <vector>
|
|
||||||
using namespace std;
|
|
||||||
|
|
||||||
#include "Laser.hpp"
|
|
||||||
#include "coreslam.h"
|
|
||||||
|
|
||||||
|
|
||||||
Laser::Laser(
|
|
||||||
int scanSize,
|
|
||||||
float scanRateHz,
|
|
||||||
float detection_angle_degrees,
|
|
||||||
float distance_no_detection_mm,
|
|
||||||
int detection_margin,
|
|
||||||
float offset_mm
|
|
||||||
)
|
|
||||||
{
|
|
||||||
this->laser = new laser_t;
|
|
||||||
|
|
||||||
this->laser->scan_size = scanSize;
|
|
||||||
this->laser->scan_rate_hz = scanRateHz;
|
|
||||||
this->laser->detection_angle_degrees = detection_angle_degrees;
|
|
||||||
this->laser->distance_no_detection_mm = distance_no_detection_mm;
|
|
||||||
this->laser->detection_margin = detection_margin;
|
|
||||||
this->laser->offset_mm = offset_mm;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
Laser::Laser(void)
|
|
||||||
{
|
|
||||||
this->laser = new laser_t;
|
|
||||||
|
|
||||||
this->laser->scan_size = 0;
|
|
||||||
this->laser->scan_rate_hz = 0;
|
|
||||||
this->laser->detection_angle_degrees = 0;
|
|
||||||
this->laser->distance_no_detection_mm = 0;
|
|
||||||
this->laser->detection_margin = 0;
|
|
||||||
this->laser->offset_mm = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
Laser::~Laser(void)
|
|
||||||
{
|
|
||||||
delete this->laser;
|
|
||||||
}
|
|
||||||
|
|
||||||
ostream& operator<< (ostream & out, Laser & laser)
|
|
||||||
{
|
|
||||||
char str[512];
|
|
||||||
char lstr[512];
|
|
||||||
|
|
||||||
laser_string(*laser.laser, lstr);
|
|
||||||
|
|
||||||
sprintf(str, "<%s>", lstr);
|
|
||||||
|
|
||||||
out << str;
|
|
||||||
|
|
||||||
return out;
|
|
||||||
}
|
|
||||||
|
|
||||||
Binary file not shown.
@@ -1,300 +0,0 @@
|
|||||||
/*
|
|
||||||
* mex_coreslam.c : C extensions for BreezySLAM in Matlab
|
|
||||||
*
|
|
||||||
* 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/>.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "mex.h"
|
|
||||||
|
|
||||||
#include "../c/coreslam.h"
|
|
||||||
#include "../c/random.h"
|
|
||||||
|
|
||||||
#define MAXSTR 100
|
|
||||||
|
|
||||||
/* Helpers ------------------------------------------------------------- */
|
|
||||||
|
|
||||||
static int _streq(char * s, const char * t)
|
|
||||||
{
|
|
||||||
return !strcmp(s, t);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _insert_obj_lhs(mxArray *plhs[], void * obj, int pos)
|
|
||||||
{
|
|
||||||
long * outptr = NULL;
|
|
||||||
|
|
||||||
plhs[pos] = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
|
||||||
|
|
||||||
outptr = (long *) mxGetPr(plhs[pos]);
|
|
||||||
|
|
||||||
mexMakeMemoryPersistent(obj);
|
|
||||||
|
|
||||||
*outptr = (long)obj;
|
|
||||||
}
|
|
||||||
|
|
||||||
static double _get_field(const mxArray * pm, const char * fieldname)
|
|
||||||
{
|
|
||||||
mxArray * field_array_ptr = mxGetField(pm, 0, fieldname);
|
|
||||||
|
|
||||||
return mxGetScalar(field_array_ptr);
|
|
||||||
}
|
|
||||||
|
|
||||||
static long _rhs2ptr(const mxArray * prhs[], int index)
|
|
||||||
{
|
|
||||||
long * inptr = (long *) mxGetPr(prhs[index]);
|
|
||||||
|
|
||||||
return *inptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
static scan_t * _rhs2scan(const mxArray * prhs[], int index)
|
|
||||||
{
|
|
||||||
long inptr = _rhs2ptr(prhs, index);
|
|
||||||
|
|
||||||
return (scan_t *)inptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
static map_t * _rhs2map(const mxArray * prhs[], int index)
|
|
||||||
{
|
|
||||||
long inptr = _rhs2ptr(prhs, index);
|
|
||||||
|
|
||||||
return (map_t *)inptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
static position_t _rhs2pos(const mxArray * prhs[], int index)
|
|
||||||
{
|
|
||||||
position_t position;
|
|
||||||
|
|
||||||
position.x_mm = _get_field(prhs[index], "x_mm");
|
|
||||||
position.y_mm = _get_field(prhs[index], "y_mm");
|
|
||||||
position.theta_degrees = _get_field(prhs[index], "theta_degrees");
|
|
||||||
|
|
||||||
return position;
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _rhs2laser(laser_t * laser, const mxArray * prhs[], int index)
|
|
||||||
{
|
|
||||||
laser->scan_size = (int)_get_field(prhs[index], "scan_size");
|
|
||||||
laser->scan_rate_hz = _get_field(prhs[index], "scan_rate_hz");
|
|
||||||
laser->detection_angle_degrees = _get_field(prhs[index], "detection_angle_degrees");
|
|
||||||
laser->distance_no_detection_mm = _get_field(prhs[index], "distance_no_detection_mm");
|
|
||||||
laser->detection_margin = (int)_get_field(prhs[index], "detection_margin");
|
|
||||||
laser->offset_mm = _get_field(prhs[index], "offset_mm");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Class methods ------------------------------------------------------- */
|
|
||||||
|
|
||||||
static void _map_init(mxArray *plhs[], const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
|
|
||||||
int size_pixels = (int)mxGetScalar(prhs[1]);
|
|
||||||
|
|
||||||
double size_meters = mxGetScalar(prhs[2]);
|
|
||||||
|
|
||||||
map_t * map = (map_t *)mxMalloc(sizeof(map_t));
|
|
||||||
|
|
||||||
map_init(map, size_pixels, size_meters);
|
|
||||||
|
|
||||||
_insert_obj_lhs(plhs, map, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _map_disp(const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
char str[MAXSTR];
|
|
||||||
|
|
||||||
map_t * map = _rhs2map(prhs, 1);
|
|
||||||
|
|
||||||
map_string(*map, str);
|
|
||||||
|
|
||||||
printf("%s\n", str);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _map_update(const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
map_t * map = _rhs2map(prhs, 1);
|
|
||||||
|
|
||||||
scan_t * scan = _rhs2scan(prhs, 2);
|
|
||||||
|
|
||||||
position_t position = _rhs2pos(prhs, 3);
|
|
||||||
|
|
||||||
int map_quality = (int)mxGetScalar(prhs[4]);
|
|
||||||
|
|
||||||
double hole_width_mm = mxGetScalar(prhs[5]);
|
|
||||||
|
|
||||||
map_update(map, scan, position, map_quality, hole_width_mm);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _map_get(mxArray *plhs[], const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
map_t * map = _rhs2map(prhs, 1);
|
|
||||||
|
|
||||||
unsigned char * pointer = NULL;
|
|
||||||
|
|
||||||
plhs[0] = mxCreateNumericMatrix(map->size_pixels, map->size_pixels,
|
|
||||||
mxUINT8_CLASS, mxREAL);
|
|
||||||
|
|
||||||
pointer = (unsigned char *)mxGetPr(plhs[0]);
|
|
||||||
|
|
||||||
map_get(map, pointer);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
static void _scan_init(mxArray *plhs[], const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
|
|
||||||
laser_t * laser = (laser_t *)mxMalloc(sizeof(laser_t));
|
|
||||||
|
|
||||||
scan_t * scan = (scan_t *)mxMalloc(sizeof(scan_t));
|
|
||||||
|
|
||||||
int span = (int)mxGetScalar(prhs[2]);
|
|
||||||
|
|
||||||
_rhs2laser(laser, prhs, 1);
|
|
||||||
|
|
||||||
scan_init(scan, laser->scan_size, span);
|
|
||||||
|
|
||||||
|
|
||||||
_insert_obj_lhs(plhs, scan, 0);
|
|
||||||
_insert_obj_lhs(plhs, laser, 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _scan_disp(const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
char str[MAXSTR];
|
|
||||||
|
|
||||||
scan_t * scan = _rhs2scan(prhs, 1);
|
|
||||||
|
|
||||||
scan_string(*scan, str);
|
|
||||||
|
|
||||||
printf("%s\n", str);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _scan_update(const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
scan_t * scan = _rhs2scan(prhs, 1);
|
|
||||||
|
|
||||||
laser_t * laser = (laser_t *)_rhs2ptr(prhs, 2);
|
|
||||||
|
|
||||||
int scansize = (int)mxGetNumberOfElements(prhs[3]);
|
|
||||||
|
|
||||||
int * lidar_mm = (int *)mxGetPr(prhs[3]);
|
|
||||||
|
|
||||||
double hole_width_mm = mxGetScalar(prhs[4]);
|
|
||||||
|
|
||||||
double * velocities = mxGetPr(prhs[5]);
|
|
||||||
|
|
||||||
scan_update(scan, lidar_mm, *laser, hole_width_mm, velocities[0], velocities[1]);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _randomizer_init(mxArray *plhs[], const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
int seed = (int)mxGetScalar(prhs[1]);
|
|
||||||
|
|
||||||
void * r = mxMalloc(random_size());
|
|
||||||
|
|
||||||
random_init(r, seed);
|
|
||||||
|
|
||||||
_insert_obj_lhs(plhs, r, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void _rmhcPositionSearch(mxArray *plhs[], const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
position_t start_pos = _rhs2pos(prhs, 1);
|
|
||||||
|
|
||||||
map_t * map = _rhs2map(prhs, 2);
|
|
||||||
|
|
||||||
scan_t * scan = _rhs2scan(prhs, 3);
|
|
||||||
|
|
||||||
laser_t laser;
|
|
||||||
position_t new_pos;
|
|
||||||
|
|
||||||
double sigma_xy_mm = mxGetScalar(prhs[5]);
|
|
||||||
|
|
||||||
double sigma_theta_degrees = mxGetScalar(prhs[6]);
|
|
||||||
|
|
||||||
int max_search_iter = (int)mxGetScalar(prhs[7]);
|
|
||||||
|
|
||||||
void * randomizer = (void *)(long)mxGetScalar(prhs[8]);
|
|
||||||
|
|
||||||
_rhs2laser(&laser, prhs, 4);
|
|
||||||
|
|
||||||
new_pos = rmhc_position_search(
|
|
||||||
start_pos,
|
|
||||||
map,
|
|
||||||
scan,
|
|
||||||
laser,
|
|
||||||
sigma_xy_mm,
|
|
||||||
sigma_theta_degrees,
|
|
||||||
max_search_iter,
|
|
||||||
randomizer);
|
|
||||||
|
|
||||||
plhs[0] = mxCreateDoubleScalar(new_pos.x_mm);
|
|
||||||
plhs[1] = mxCreateDoubleScalar(new_pos.y_mm);
|
|
||||||
plhs[2] = mxCreateDoubleScalar(new_pos.theta_degrees);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* The gateway function ------------------------------------------------ */
|
|
||||||
void mexFunction( int nlhs, mxArray *plhs[],
|
|
||||||
int nrhs, const mxArray * prhs[])
|
|
||||||
{
|
|
||||||
|
|
||||||
char methodname[MAXSTR];
|
|
||||||
|
|
||||||
mxGetString(prhs[0], methodname, 100);
|
|
||||||
|
|
||||||
if (_streq(methodname, "Map_init"))
|
|
||||||
{
|
|
||||||
_map_init(plhs, prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Map_disp"))
|
|
||||||
{
|
|
||||||
_map_disp(prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Map_update"))
|
|
||||||
{
|
|
||||||
_map_update(prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Map_get"))
|
|
||||||
{
|
|
||||||
_map_get(plhs, prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Scan_init"))
|
|
||||||
{
|
|
||||||
_scan_init(plhs, prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Scan_disp"))
|
|
||||||
{
|
|
||||||
_scan_disp(prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Scan_update"))
|
|
||||||
{
|
|
||||||
_scan_update(prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "Randomizer_init"))
|
|
||||||
{
|
|
||||||
_randomizer_init(plhs, prhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
else if (_streq(methodname, "rmhcPositionSearch"))
|
|
||||||
{
|
|
||||||
_rmhcPositionSearch(plhs, prhs);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Reference in New Issue
Block a user