Skip to content
Snippets Groups Projects
Commit 3f6c4a4e authored by Andrea Censi's avatar Andrea Censi
Browse files

No commit message 

No commit message
parent bfa79f4c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 793 additions and 49 deletions
sm/CMakeLists.txt
+ 19
− 1
View file @ 3f6c4a4e
......@@ -63,6 +63,7 @@ PROJECT (scan_matching_lib C)
########### Options ##########
SUBDIRS(lib/options)
SUBDIRS(lib/json-c-0.7)
########### SM ##########
......@@ -70,10 +71,12 @@ PROJECT (scan_matching_lib C)
src/journal.c
src/laser_data.c
src/laser_data_fisher.c
src/laser_data_json.c
src/math_utils.c
src/orientation.c
src/clustering.c
src/logging.c
src/icp/icp.c
src/icp/icp_loop.c
src/icp/icp_corr_dumb.c
......@@ -90,11 +93,17 @@ PROJECT (scan_matching_lib C)
lib/egsl/egsl_ops.c
lib/egsl/egsl_conversions.c
lib/egsl/egsl_misc.c
lib/JSON_checker/JSON_checker.c
)
INCLUDE_DIRECTORIES(lib/JSON_checker)
INCLUDE_DIRECTORIES(lib/egsl)
INCLUDE_DIRECTORIES(lib/gpc)
INCLUDE_DIRECTORIES(lib/json-c-0.7)
TARGET_LINK_LIBRARIES(sm json-c)
# #SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Ilib/gpc")
# SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Ilib/egsl")
......@@ -110,6 +119,15 @@ PROJECT (scan_matching_lib C)
ADD_EXECUTABLE(test_math_utils src/math_utils_test.c)
TARGET_LINK_LIBRARIES(test_math_utils sm ${GSL_LIBRARIES})
ADD_EXECUTABLE(test_json_pipe apps/test_json_pipe.c)
TARGET_LINK_LIBRARIES(test_json_pipe sm ${GSL_LIBRARIES})
ADD_EXECUTABLE(test_json_ld apps/test_json_ld.c)
TARGET_LINK_LIBRARIES(test_json_ld sm ${GSL_LIBRARIES})
ADD_EXECUTABLE(test_json apps/test_json.c)
TARGET_LINK_LIBRARIES(test_json sm ${GSL_LIBRARIES})
########### Carmen2pdf ##########
IF(CAIRO_FOUND)
ADD_EXECUTABLE(carmen2pdf apps/carmen2pdf.c)
......
sm/apps/test_json.c 0 → 100644
+ 11
− 0
View file @ 3f6c4a4e
#include "../src/laser_data_json.h"
int main(int argc, char * argv[]) {
LDP ld = ld_alloc_new(50);
JO jo = ld_to_json(ld);
printf(json_object_to_json_string(jo));
return 0;
}
\ No newline at end of file
sm/apps/test_json_ld.c 0 → 100644
+ 23
− 0
View file @ 3f6c4a4e
#include "../src/laser_data_json.h"
int main(int argc, char * argv[]) {
JO jo; // the monkey
LDP ld;
while((jo = json_read_stream(stdin))) {
if(!(ld = json_to_ld(jo))) {
fprintf(stderr, "Could not transform to laser_data:\n\n");
fprintf(stderr, "-----\n");
fprintf(stderr, json_object_to_json_string(jo));
fprintf(stderr, "-----\n");
continue;
}
jo = ld_to_json(ld);
printf(json_object_to_json_string(jo));
printf("\n");
}
return 0;
}
\ No newline at end of file
sm/apps/test_json_pipe.c 0 → 100644
+ 13
− 0
View file @ 3f6c4a4e
#include "../src/laser_data_json.h"
int main(int argc, char * argv[]) {
JO jo; // the monkey
while((jo = json_read_stream(stdin))) {
printf(json_object_to_json_string(jo));
}
return 0;
}
\ No newline at end of file
sm/lib/JSON_checker/JSON_checker.c 0 → 100644
+ 403
− 0
View file @ 3f6c4a4e
/* JSON_checker.c */
/* 2005-12-30 */
/*
Copyright (c) 2005 JSON.org
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
The Software shall be used for Good, not Evil.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include "JSON_checker.h"
#define true 1
#define false 0
/*
Characters are mapped into these 32 symbol classes. This allows for
significant reductions in the size of the state transition table.
*/
/* error */
#define S_ERR -1
/* space */
#define S_SPA 0
/* other whitespace */
#define S_WSP 1
/* { */
#define S_LBE 2
/* } */
#define S_RBE 3
/* [ */
#define S_LBT 4
/* ] */
#define S_RBT 5
/* : */
#define S_COL 6
/* , */
#define S_COM 7
/* " */
#define S_QUO 8
/* \ */
#define S_BAC 9
/* / */
#define S_SLA 10
/* + */
#define S_PLU 11
/* - */
#define S_MIN 12
/* . */
#define S_DOT 13
/* 0 */
#define S_ZER 14
/* 123456789 */
#define S_DIG 15
/* a */
#define S__A_ 16
/* b */
#define S__B_ 17
/* c */
#define S__C_ 18
/* d */
#define S__D_ 19
/* e */
#define S__E_ 20
/* f */
#define S__F_ 21
/* l */
#define S__L_ 22
/* n */
#define S__N_ 23
/* r */
#define S__R_ 24
/* s */
#define S__S_ 25
/* t */
#define S__T_ 26
/* u */
#define S__U_ 27
/* ABCDF */
#define S_A_F 28
/* E */
#define S_E 29
/* everything else */
#define S_ETC 30
/*
This table maps the 128 ASCII characters into the 32 character classes.
The remaining Unicode characters should be mapped to S_ETC.
*/
static int ascii_class[128] = {
S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR,
S_ERR, S_WSP, S_WSP, S_ERR, S_ERR, S_WSP, S_ERR, S_ERR,
S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR,
S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR, S_ERR,
S_SPA, S_ETC, S_QUO, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC,
S_ETC, S_ETC, S_ETC, S_PLU, S_COM, S_MIN, S_DOT, S_SLA,
S_ZER, S_DIG, S_DIG, S_DIG, S_DIG, S_DIG, S_DIG, S_DIG,
S_DIG, S_DIG, S_COL, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC,
S_ETC, S_A_F, S_A_F, S_A_F, S_A_F, S_E , S_A_F, S_ETC,
S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC,
S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC, S_ETC,
S_ETC, S_ETC, S_ETC, S_LBT, S_BAC, S_RBT, S_ETC, S_ETC,
S_ETC, S__A_, S__B_, S__C_, S__D_, S__E_, S__F_, S_ETC,
S_ETC, S_ETC, S_ETC, S_ETC, S__L_, S_ETC, S__N_, S_ETC,
S_ETC, S_ETC, S__R_, S__S_, S__T_, S__U_, S_ETC, S_ETC,
S_ETC, S_ETC, S_ETC, S_LBE, S_ETC, S_RBE, S_ETC, S_ETC
};
/*
The state transition table takes the current state and the current symbol,
and returns either a new state or an action. A new state is a number between
0 and 29. An action is a negative number between -1 and -9. A JSON text is
accepted if the end of the text is in state 9 and mode is MODE_DONE.
*/
static int state_transition_table[30][31] = {
/* 0*/ { 0, 0,-8,-1,-6,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/* 1*/ { 1, 1,-1,-9,-1,-1,-1,-1, 3,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/* 2*/ { 2, 2,-8,-1,-6,-5,-1,-1, 3,-1,-1,-1,20,-1,21,22,-1,-1,-1,-1,-1,13,-1,17,-1,-1,10,-1,-1,-1,-1},
/* 3*/ { 3,-1, 3, 3, 3, 3, 3, 3,-4, 4, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3},
/* 4*/ {-1,-1,-1,-1,-1,-1,-1,-1, 3, 3, 3,-1,-1,-1,-1,-1,-1, 3,-1,-1,-1, 3,-1, 3, 3,-1, 3, 5,-1,-1,-1},
/* 5*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 6, 6, 6, 6, 6, 6, 6, 6,-1,-1,-1,-1,-1,-1, 6, 6,-1},
/* 6*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 7, 7, 7, 7, 7, 7, 7, 7,-1,-1,-1,-1,-1,-1, 7, 7,-1},
/* 7*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 8, 8, 8, 8, 8, 8, 8, 8,-1,-1,-1,-1,-1,-1, 8, 8,-1},
/* 8*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 3, 3, 3, 3, 3, 3, 3, 3,-1,-1,-1,-1,-1,-1, 3, 3,-1},
/* 9*/ { 9, 9,-1,-7,-1,-5,-1,-3,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*10*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,11,-1,-1,-1,-1,-1,-1},
/*11*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,12,-1,-1,-1},
/*12*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*13*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,14,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*14*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,15,-1,-1,-1,-1,-1,-1,-1,-1},
/*15*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,16,-1,-1,-1,-1,-1},
/*16*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*17*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,18,-1,-1,-1},
/*18*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,19,-1,-1,-1,-1,-1,-1,-1,-1},
/*19*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1, 9,-1,-1,-1,-1,-1,-1,-1,-1},
/*20*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,21,22,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*21*/ { 9, 9,-1,-7,-1,-5,-1,-3,-1,-1,-1,-1,-1,23,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*22*/ { 9, 9,-1,-7,-1,-5,-1,-3,-1,-1,-1,-1,-1,23,22,22,-1,-1,-1,-1,24,-1,-1,-1,-1,-1,-1,-1,-1,24,-1},
/*23*/ { 9, 9,-1,-7,-1,-5,-1,-3,-1,-1,-1,-1,-1,-1,23,23,-1,-1,-1,-1,24,-1,-1,-1,-1,-1,-1,-1,-1,24,-1},
/*24*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,25,25,-1,26,26,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*25*/ {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,26,26,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*26*/ { 9, 9,-1,-7,-1,-5,-1,-3,-1,-1,-1,-1,-1,-1,26,26,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*27*/ {27,27,-1,-1,-1,-1,-2,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1},
/*28*/ {28,28,-8,-1,-6,-1,-1,-1, 3,-1,-1,-1,20,-1,21,22,-1,-1,-1,-1,-1,13,-1,17,-1,-1,10,-1,-1,-1,-1},
/*29*/ {29,29,-1,-1,-1,-1,-1,-1, 3,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1}
};
static int the_state;
/*
These modes can be pushed on the PDA stack.
*/
#define MODE_DONE 1
#define MODE_KEY 2
#define MODE_OBJECT 3
#define MODE_ARRAY 4
/*
A stack maintains the states of nested structures.
*/
#define MAX_DEPTH 20
static int the_stack[MAX_DEPTH];
static int the_top;
static int the_index = 0;
/*
Push a mode onto the stack. Return false if there is overflow.
*/
static int
push(int mode)
{
the_top += 1;
if (the_top >= MAX_DEPTH) {
return false;
}
the_stack[the_top] = mode;
return true;
}
/*
Pop the stack, assuring that the current mode matches the expectation.
Return false if there is underflow or if the modes mismatch.
*/
static int
pop(int mode)
{
if (the_top < 0 || the_stack[the_top] != mode) {
return false;
}
the_stack[the_top] = 0;
the_top -= 1;
return true;
}
/*
The JSON_checker takes a UTF-16 encoded string and determines if it is a
syntactically correct JSON text.
It is implemented as a Pushdown Automaton; that means it is a finite state
machine with a stack.
*/
int
JSON_checker(unsigned short p[], int length)
{
int b; /* the next character */
int c; /* the next character class */
int s; /* the next state */
the_state = 0;
the_top = -1;
push(MODE_DONE);
for (the_index = 0; the_index < length; the_index += 1) {
b = p[the_index];
if ((b & 127) == b) {
c = ascii_class[b];
if (c <= S_ERR) {
return false;
}
} else {
c = S_ETC;
}
/*
Get the next state from the transition table.
*/
s = state_transition_table[the_state][c];
if (s < 0) {
/*
Perform one of the predefined actions.
*/
switch (s) {
/*
empty }
*/
case -9:
if (!pop(MODE_KEY)) {
return false;
}
the_state = 9;
break;
/*
{
*/
case -8:
if (!push(MODE_KEY)) {
return false;
}
the_state = 1;
break;
/*
}
*/
case -7:
if (!pop(MODE_OBJECT)) {
return false;
}
the_state = 9;
break;
/*
[
*/
case -6:
if (!push(MODE_ARRAY)) {
return false;
}
the_state = 2;
break;
/*
]
*/
case -5:
if (!pop(MODE_ARRAY)) {
return false;
}
the_state = 9;
break;
/*
"
*/
case -4:
switch (the_stack[the_top]) {
case MODE_KEY:
the_state = 27;
break;
case MODE_ARRAY:
case MODE_OBJECT:
the_state = 9;
break;
default:
return false;
}
break;
/*
,
*/
case -3:
switch (the_stack[the_top]) {
case MODE_OBJECT:
if (pop(MODE_OBJECT) && push(MODE_KEY)) {
the_state = 29;
}
break;
case MODE_ARRAY:
the_state = 28;
break;
default:
return false;
}
break;
/*
:
*/
case -2:
if (pop(MODE_KEY) && push(MODE_OBJECT)) {
the_state = 28;
break;
}
/*
syntax error
*/
case -1:
return false;
}
} else {
/*
Change the state and iterate.
*/
the_state = s;
}
}
return the_state == 9 && pop(MODE_DONE);
}
/*
Get the current character number.
*/
int
JSON_checker_at_character()
{
return the_index;
}
sm/lib/JSON_checker/JSON_checker.h 0 → 100644
+ 5
− 0
View file @ 3f6c4a4e
/* JSON_checker.h */
extern int JSON_checker(unsigned short p[], int length);
extern int JSON_checker_at_character();
sm/src/laser_data.c
+ 12
− 6
View file @ 3f6c4a4e
......@@ -5,16 +5,22 @@
#include "math_utils.h"
#include "journal.h"
void ld_alloc(struct laser_data*ld, int nrays) {
LDP ld_alloc_new(int nrays) {
LDP ld = malloc(sizeof(struct laser_data));
ld_alloc(ld, nrays);
return ld;
}
void ld_alloc(LDP ld, int nrays) {
ld->nrays = nrays;
ld->valid = (int*) malloc(sizeof(int)*nrays);
ld->readings = (double*) malloc(sizeof(double)*nrays);
ld->theta = (double*) malloc(sizeof(double)*nrays);
ld->cluster = (int*) malloc(sizeof(int) *nrays);
ld->alpha = (double*) malloc(sizeof(double)*nrays);
ld->cov_alpha = (double*) malloc(sizeof(double)*nrays);
ld->alpha_valid = (int*) malloc(sizeof(int) *nrays);
ld->cluster = (int*) malloc(sizeof(int) *nrays);
ld->alpha = (double*) malloc(sizeof(double)*nrays);
ld->cov_alpha = (double*) malloc(sizeof(double)*nrays);
ld->alpha_valid = (int*) malloc(sizeof(int) *nrays);
ld->up_bigger = (int*) malloc(sizeof(int) *nrays);
ld->up_smaller = (int*) malloc(sizeof(int) *nrays);
......@@ -129,7 +135,7 @@ int ld_valid_corr(LDP ld, int i) {
}
// Read next FLASER line in file (initializes ld). Returns 0 if error or eof.
/// Read next FLASER line in file (initializes ld). Returns 0 if error or EOF.
int ld_read_next_laser_carmen(FILE*file, LDP ld) {
#define MAX_LINE_LENGTH 10000
char line[MAX_LINE_LENGTH];
......
sm/src/laser_data.h
+ 43
− 3
View file @ 3f6c4a4e
......@@ -4,9 +4,49 @@
#include <stdio.h>
#include <egsl.h>
#include "sm.h"
#define LDP struct laser_data*
struct correspondence;
struct laser_data {
int nrays;
double min_theta;
double max_theta;
double *theta;
int*valid;
double *readings;
int *cluster;
double *alpha;
double *cov_alpha;
int *alpha_valid;
/* Jump tables */
int *up_bigger, *up_smaller, *down_bigger, *down_smaller;
/* Cartesian points */
gsl_vector**p;
struct correspondence* corr;
double odometry[3];
double estimate[3];
};
struct correspondence {
int valid;
int j1; int j2;
};
typedef struct laser_data* LDP;
LDP ld_alloc_new(int nrays);
void ld_alloc(LDP, int nrays);
void ld_free(LDP);
void ld_compute_cartesian(LDP);
void ld_create_jump_tables(LDP);
......@@ -15,7 +55,7 @@ void ld_create_jump_tables(LDP);
int ld_next_valid(LDP ld, int i, int dir);
int ld_valid_ray(struct laser_data* ld, int i);
int ld_valid_ray(LDP ld, int i);
int ld_valid_corr(LDP ld, int i);
int ld_num_valid_correspondences(LDP ld);
......
sm/src/laser_data_json.c 0 → 100644
+ 222
− 0
View file @ 3f6c4a4e
#include <errno.h>
#include <string.h>
#include <math.h>
#include <ctype.h>
#include "logging.h"
//#include <strerror.h>
#include <JSON_checker.h>
#include "laser_data_json.h"
JO json_double_array(const double *v, int n) {
JO array = json_object_new_array();
int i; for(i=0;i<n;i++) {
JO value = v[i] == v[i] ?
json_object_new_double(v[i]) : json_tokener_parse("null");
json_object_array_add(array, value);
}
return array;
}
JO json_int_array(const int *v, int n) {
JO array = json_object_new_array();
int i; for(i=0;i<n;i++) {
json_object_array_add(array, json_object_new_int(v[i]));
}
return array;
}
JO ld_to_json(LDP ld) {
JO jo = json_object_new_object();
int n = ld->nrays;
json_object_object_add(jo, "nrays", json_object_new_int(ld->nrays));
json_object_object_add(jo, "min_theta", json_object_new_double(ld->min_theta));
json_object_object_add(jo, "max_theta", json_object_new_double(ld->max_theta));
json_object_object_add(jo, "theta", json_double_array(ld->theta, n));
json_object_object_add(jo, "readings", json_double_array(ld->readings, n));
json_object_object_add(jo, "valid", json_int_array(ld->valid, n));
json_object_object_add(jo, "cluster", json_int_array(ld->cluster, n));
json_object_object_add(jo, "alpha", json_double_array(ld->alpha, n));
json_object_object_add(jo, "cov_alpha", json_double_array(ld->cov_alpha, n));
json_object_object_add(jo, "alpha_valid", json_int_array(ld->alpha_valid, n));
return jo;
/* int *up_bigger, *up_smaller, *down_bigger, *down_smaller;
gsl_vector**p;
struct correspondence* corr;
*/
}
int json_read_int(JO jo, const char*name, int*p) {
JO v = json_object_object_get(jo, (char*)name);
if(!v) {
sm_error("Field '%s' not found.", name);
return 0;
}
if(!json_object_is_type(v, json_type_int)) {
sm_error("I was looking for a int, instead got '%s'.",
json_object_to_json_string(v));
return 0;
}
*p = json_object_get_int(v);
return 1;
}
double convert_to_double(JO jo) {
if(json_object_is_type(jo, json_type_double))
return json_object_get_double(jo);
else
return NAN;
}
int json_read_double(JO jo, const char*name, double*p) {
JO v = json_object_object_get(jo, (char*)name);
if(!v) {
sm_error("Field '%s' not found.", name);
return 0;
}
*p = convert_to_double(v);
return 1;
}
int json_read_double_array(JO s, const char*name, double*p, int n, double when_null) {
JO jo = json_object_object_get(s, (char*)name);
if(!jo) {
sm_error("Field '%s' not found.", name);
return 0;
}
if(!json_object_is_type(jo, json_type_array)) {
sm_error("This is not an array: '%s'",json_object_to_json_string(jo));
return 0;
}
int size = json_object_array_length(jo);
if(size < n) {
sm_error("I expected at least %d elements, got %d. \nArray: '%s'",
n, size, json_object_to_json_string(jo));
return 0;
}
int i; for(i=0;i<n;i++) {
JO v = json_object_array_get_idx(jo, i);
if(!v || !json_object_is_type(v, json_type_double))
p[i] = when_null;
else
p[i] = json_object_get_double(v);
}
return 0;
}
int json_read_int_array(JO s, const char*name, int*p, int n, int when_null) {
JO jo = json_object_object_get(s, (char*)name);
if(!jo) {
sm_error("Field '%s' not found.", name);
return 0;
}
if(!json_object_is_type(jo, json_type_array)) {
sm_error("This is not an array: '%s'",json_object_to_json_string(jo));
return 0;
}
int size = json_object_array_length(jo);
if(size < n) {
sm_error("I expected at least %d elements, got %d. \nArray: '%s'",
n, size, json_object_to_json_string(jo));
return 0;
}
int i; for(i=0;i<n;i++) {
JO v = json_object_array_get_idx(jo, i);
if(!v || !json_object_is_type(v, json_type_int))
p[i] = when_null;
else
p[i] = json_object_get_int(v);
}
return 0;
}
LDP json_to_ld(JO jo) {
int n;
if(!json_read_int(jo, "nrays", &n)) {
sm_error("Could not read nrays.\n");
return 0;
}
LDP ld = ld_alloc_new(n);
json_read_double(jo, "min_theta", &ld->min_theta);
json_read_double(jo, "max_theta", &ld->max_theta);
json_read_double_array(jo, "theta", ld->theta, n, NAN);
json_read_double_array(jo, "readings", ld->readings, n, NAN);
json_read_int_array(jo, "valid", ld->valid, n, 0);
json_read_int_array(jo, "cluster", ld->cluster, n, -1);
json_read_double_array(jo, "alpha", ld->alpha, n, NAN);
json_read_double_array(jo, "cov_alpha", ld->cov_alpha, n, NAN);
json_read_int_array(jo, "alpha_valid", ld->alpha_valid, n, 0);
return ld;
}
struct json_object* json_tokener_parse_len(char *str, int len)
{
struct json_tokener* tok;
struct json_object* obj;
tok = json_tokener_new();
obj = json_tokener_parse_ex(tok, str, len);
if(tok->err != json_tokener_success)
obj = error_ptr(-tok->err);
json_tokener_free(tok);
return obj;
}
JO json_read_stream(FILE*f) {
#define MAX_SIZE 100000
char buf[MAX_SIZE+1];
unsigned short bufs[MAX_SIZE+1];
int count = 0;
while(count < MAX_SIZE) {
char c;
if(1 != fread(&c,1,1,f)) {
if(feof(f)) {
if(count==0) return 0;
sm_error("EOF while %d were read: \n\t'%.*s'. \n", count, count, buf);
return 0;
}
sm_error("Reading error: %s\n", strerror(errno));
return 0;
} else {
if(count==0 && isspace(c)) continue;
buf[count] = c;
bufs[count] = c;
count++;
if(JSON_checker(bufs, count)) {
// sm_de("Found object.\n");
JO jo = json_tokener_parse_len(buf, count);
return jo;
}
}
}
sm_error("Object is bigger than MAX_SIZE = %d\n", MAX_SIZE);
return 0;
}
sm/src/laser_data_json.h 0 → 100644
+ 14
− 0
View file @ 3f6c4a4e
#ifndef H_LASER_DATA_JSON
#define H_LASER_DATA_JSON
#include "laser_data.h"
#include <json.h>
typedef struct json_object* JO;
JO ld_to_json(LDP);
LDP json_to_ld(JO);
JO json_read_stream(FILE*);
#endif
\ No newline at end of file
sm/src/logging.c 0 → 100644
+ 17
− 0
View file @ 3f6c4a4e
#include <sys/param.h>
#include <stdarg.h>
#include <stdio.h>
#include "logging.h"
void sm_error(const char *msg, ...)
{
va_list ap;
va_start(ap, msg);
#if HAVE_VSYSLOG
if(_syslog) {
vsyslog(LOG_ERR, msg, ap);
} else
#endif
vfprintf(stderr, msg, ap);
}
sm/src/logging.h 0 → 100644
+ 10
− 0
View file @ 3f6c4a4e
#ifndef SM_LOGGING_H
#define SM_LOGGING_H
extern void sm_abort(const char *msg, ...);
extern void sm_debug(const char *msg, ...);
extern void sm_error(const char *msg, ...);
extern void sm_info(const char *msg, ...);
#endif
sm/src/sm.h
+ 1
− 39
View file @ 3f6c4a4e
#ifndef H_SCAN_MATCHING_LIB
#define H_SCAN_MATCHING_LIB
#include "laser_data.h"
#include <gsl/gsl_vector.h>
struct correspondence {
int valid;
int j1; int j2;
};
struct laser_data {
int nrays;
double min_theta;
double max_theta;
double *theta;
int*valid;
double *readings;
int *cluster;
double *alpha;
double *cov_alpha;
int *alpha_valid;
/* Jump tables */
int *up_bigger, *up_smaller, *down_bigger, *down_smaller;
/* Cartesian points */
gsl_vector**p;
struct correspondence* corr;
double odometry[3];
double estimate[3];
};
void ld_alloc(struct laser_data*, int nrays);
void ld_free(struct laser_data*);
struct sm_params {
struct laser_data laser_ref;
struct laser_data laser_sens;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment