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

No commit message

No commit message
parent 36aafd4b
No related branches found
No related tags found
No related merge requests found
......@@ -33,3 +33,25 @@ Create the animation:
$ sm_animate -in matching13.txt
### Help! ICP doesn't work ###
Actually, there are a million reasons for which it shouldn't work.
If it gives strange results, try the following:
1) Plot the data! Plot the input and plot the output using `log2pdf`.
2) Plot the animation! Use the procedure above and inspect the resulting videos.
3) Double-check the parameters you are using. Note that there are some like
`max_correspondence_dist` which depend on the scale of your data. A value
of 2m might work for a big robot making large movements, but not for a little
Khepera.
4) Smooth your data -- if your sensor is very noisy, like an Hokuyo, it's worth
to do simple low-pass filtering. Especially for PLICP which uses the orientation
information.
......@@ -33,6 +33,8 @@ typedef struct {
/* Padding, in meters, to be added */
double padding;
int write_info;
} anim_params ;
int draw_animation( anim_params* p, JO jo, const char*filename);
......@@ -50,6 +52,7 @@ int main(int argc, const char** argv)
options_string(ops, "in", &p.file_input, "stdin", "Input file (defaults to stdin)");
options_string(ops, "out", &p.file_output, "sm_animate_%02d.pdf", "Output file ");
options_int(ops, "write_info", &p.write_info, 0, "Writes informations and statistics in the picture.");
options_int(ops, "max_iterations", &p.max_iterations, 10, "Maximum number of iterations");
options_int(ops, "zoom_ray", &p.zoom_ray, -1, "If >= 0, the action is zoomed on a particular ray.");
options_int(ops, "width_pt", &p.width_pt, 500, "Maximum width, in points, of the PDF.");
......@@ -61,7 +64,7 @@ int main(int argc, const char** argv)
ls_add_options(&(p.corr), ops, "corr_", "");
if(!options_parse_args(ops, argc, argv)) {
sm_info("Draws ICP animation. It reads the output of sm2 with the 'file_jj' witch. \n\nUsage:\n");
sm_info("Draws ICP animation. It reads the output created by sm2 when given the 'file_jj' switch. \n\nUsage:\n");
options_print_help(ops, stderr);
return -1;
}
......@@ -138,7 +141,7 @@ int draw_animation(anim_params* p, JO jo, const char*filename) {
int niterations = json_object_array_length(iterations);
if(niterations>p->max_iterations) niterations = p->max_iterations;
sm_error("Displaying %d iterations.\n", niterations);
sm_info("Displaying %d iterations.\n", niterations);
int it;
for(it=0;it<niterations;it++) {
......@@ -179,19 +182,21 @@ int draw_animation(anim_params* p, JO jo, const char*filename) {
cairo_restore(cr);
if(0) {
cairo_save(cr);
cairo_identity_matrix(cr);
cairo_set_font_size (cr, 0.1);
cairo_select_font_face (cr, "Sans",
CAIRO_FONT_SLANT_NORMAL,
CAIRO_FONT_WEIGHT_NORMAL);
char text[100];
sprintf(text, "Iteration #%d: %s", it, friendly_pose(x_old));
cairo_move_to(cr, 0.0, 0.0 );
cairo_show_text(cr, text);
cairo_restore(cr);
if(p->write_info) {
cairo_save(cr);
cairo_identity_matrix(cr);
cairo_set_font_size (cr, 20);
cairo_select_font_face (cr, "Sans",
CAIRO_FONT_SLANT_NORMAL,
CAIRO_FONT_WEIGHT_NORMAL);
char text[100];
sprintf(text, "Iteration #%d: x_old: %s", it, friendly_pose(x_old));
cairo_move_to(cr, 0.0, -20.0 );
cairo_show_text(cr, text);
sm_info("%s\n",text);
cairo_restore(cr);
}
cairo_show_page (cr);
......
......@@ -66,6 +66,11 @@ int options_parse_args(struct option*ops, int argc, const char* argv[]) {
exit(0);
}
if(!strcmp("help",name) || !strcmp("h",name) ) {
options_print_help(ops, stdout);
exit(0);
}
if(!strcmp("config", name)) {
if(i>=argc-1) {
fprintf(stderr, "Please specify config file.\n");
......@@ -313,6 +318,13 @@ void options_dump(struct option * options, FILE*f, int write_desc) {
}
void options_print_help(struct option * options, FILE*f) {
fprintf(f,
"Generic options: \n"
" -help Displays this help.\n"
" -config_dump Dumps the configuration on the standard output. \n"
" -config FILE Loads a config file in the format used by config_dump.\n"
"\n");
options_dump(options, f, 1);
}
......
......@@ -15,6 +15,7 @@ extern "C" {
enum option_type { OPTION_STRING=0, OPTION_INT=1, OPTION_DOUBLE=2 };
struct option {
/** Name of the option (or 0 if this is the last element). */
const char * name;
......
only_corr=-sens_countour_draw 0 -ref_countour_draw 0 -sens_points_draw 0 -ref_points_draw 0 -sens_rays_draw 0 -ref_rays_draw 0 -write_info 1
all:
log2pdf -config hokuyo.config -use odometry -in stallo2.log
sm2 -restart 0 -in stallo2.log -out stallo2_plicp.log -max_iterations 10 -file_jj stallo2_plicp.journal
sm_animate $(only_corr) -in stallo2_plicp.journal -out 'stallo2_plicp_%02d.pdf'
sm2 -restart 0 -in stallo2.log -out stallo2_icp.log -use_point_to_line_distance 0 -max_iterations 10 -file_jj stallo2_icp.journal
sm_animate $(only_corr) -in stallo2_icp.journal -out 'stallo2_icp_%02d.pdf'
sm2 -in stallo2.log -out stallo2_gpm.log -algo 1 -file_jj stallo2_gpm.journal
log2pdf -config hokuyo.config -use estimate -in stallo2_gpm.log
sm2 -restart 0 -max_correspondence_dist 0.2 -in stallo2.log -out stallo2_plicp2.log -max_iterations 10 -file_jj stallo2_plicp2.journal
sm_animate $(only_corr) -in stallo2_plicp2.journal -out 'stallo2_plicp2_%02d.pdf'
sm2 -restart 0 -max_correspondence_dist 0.2 -in stallo2.log -out stallo2_icp2.log -max_iterations 10 -file_jj stallo2_icp2.journal -use_point_to_line_distance 0
sm_animate $(only_corr) -in stallo2_icp2.journal -out 'stallo2_icp2_%02d.pdf'
in stdin
padding 0.2
dimension 500
offset_theta_deg 0
use estimate
distance_xy 5
distance_th_deg 45
start_pose_width 0.01
laser_rays_draw 1
laser_rays_color #f00
laser_rays_width 0.0002
laser_countour_draw 1
laser_countour_color black
laser_countour_width 0.002
laser_points_draw 0
laser_points_color #f00
laser_points_width 0.002
laser_points_radius 0.003
laser_pose_draw 1
laser_pose_color #f73
laser_pose_width 0.002
laser_pose_radius 0.01
laser_normals_draw 0
laser_normals_color black
laser_normals_width 0.002
laser_normals_length 0.1
laser_connect_threshold 0.4
laser_horizon 10
path_draw 1
path_color #f00
path_width 0.01
This diff is collapsed.
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment