int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); /* struct csm_option* ops = csm_options_allocate(3); csm_options_double(ops, "scale_deg", &p.scale_deg, 0.0, "Scale factor (degrees) "); csm_options_int(ops, "neighbours", &p.neighbours, 1, "How many neighbours to consider (regardless of scale)."); if(!csm_options_parse_args(ops, argc, argv)) { fprintf(stderr, "A simple program for smoothing a sensor scan.\n\nUsage:\n"); csm_options_print_help(ops, stderr); return -1; } */ /* jj_set_stream(open_file_for_writing("ld_cluster_curv.txt")); */ int errors = 0; int count = -1; LDP ld; while( (ld = ld_read_smart(stdin)) ) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); return -1; } ld_cluster_curv(ld); ld_write_as_json(ld, stdout); ld_free(ld); } return errors; }
// free layer data list static void ld_list_free(struct LayerData *list) { struct LayerData *p = list; while (p) { list = list->next; ld_free(p); p = list; } }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); options_banner("ld_purify: Makes sure that the file format is valid. \n * Sets valid=0 if reading is outside interval "); struct ld_purify_params p; struct option* ops = options_allocate(20); options_double(ops, "threshold_min", &p.threshold_min, 0.01, "Sets valid=0 if readings are less than this threshold."); options_double(ops, "threshold_max", &p.threshold_max, 79.0, "Sets valid=0 if readings are more than this threshold."); options_string(ops, "in", &p.file_input, "stdin", "Input file "); options_string(ops, "out", &p.file_output, "stdout", "Output file "); if(!options_parse_args(ops, argc, argv)) { options_print_help(ops, stderr); return -1; } FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out = open_file_for_writing(p.file_output); if(!out) return -2; LDP ld; int count = -1; while( (ld = ld_from_json_stream(in))) { purify(ld, p.threshold_min, p.threshold_max); if(!ld_valid_fields(ld)) { sm_error("Wait, we didn't purify enough (#%d in file)\n", count); continue; } ld_write_as_json(ld, out); ld_free(ld); } return 0; }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); int errors = 0; int count = -1; LDP ld; while( (ld = ld_read_smart(stdin)) ) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); errors++; continue; } ld_linearize(ld); ld_write_as_json(ld, stdout); ld_free(ld); } return errors; }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); csm_options_banner("ld_noise: Adds noise to readings in a scan"); struct ld_noise_params p; struct csm_option* ops = csm_options_allocate(20); csm_options_double(ops, "discretization", &p.discretization, 0.0, "Size of discretization (disabled if 0)"); csm_options_double(ops, "sigma", &p.sigma, 0.0, "Std deviation of gaussian noise (disabled if 0)"); csm_options_int(ops, "lambertian", &p.lambertian, 0, "Use lambertian model cov = sigma^2 / cos(beta^2) where beta is the incidence. Need have alpha or true_alpha."); csm_options_int(ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable)."); csm_options_string(ops, "in", &p.file_input, "stdin", "Input file "); csm_options_string(ops, "out", &p.file_output, "stdout", "Output file "); if(!csm_options_parse_args(ops, argc, argv)) { fprintf(stderr, "A simple program for adding noise to sensor scans.\n\nUsage:\n"); csm_options_print_help(ops, stderr); return -1; } FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out = open_file_for_writing(p.file_output); if(!out) return -2; gsl_rng_env_setup(); gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0); if(p.seed != 0) gsl_rng_set(rng, (unsigned int) p.seed); LDP ld; int count = 0; while( (ld = ld_from_json_stream(in))) { if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); continue; } int i; for(i=0;i<ld->nrays;i++) { if(!ld->valid[i]) continue; double * reading = ld->readings + i; if(p.sigma > 0) { double add_sigma = p.sigma; if(p.lambertian) { int have_alpha = 0; double alpha = 0; if(!is_nan(ld->true_alpha[i])) { alpha = ld->true_alpha[i]; have_alpha = 1; } else if(ld->alpha_valid[i]) { alpha = ld->alpha[i];; have_alpha = 1; } else have_alpha = 0; if(have_alpha) { /* Recall that alpha points outside the surface */ double beta = (alpha+M_PI) - ld->theta[i]; add_sigma = p.sigma / cos(beta); } else { sm_error("Because lambertian is active, I need either true_alpha[] or alpha[]"); ld_write_as_json(ld, stderr); return -1; } } *reading += gsl_ran_gaussian(rng, add_sigma); if(is_nan(ld->readings_sigma[i])) { ld->readings_sigma[i] = add_sigma; } else { ld->readings_sigma[i] = sqrt(square(add_sigma) + square(ld->readings_sigma[i])); } } if(p.discretization > 0) *reading -= fmod(*reading , p.discretization); } ld_write_as_json(ld, out); ld_free(ld); } return 0; }
void PSMpositionNode::getMotion_can(const sensor_msgs::LaserScan& scan, std::vector <depthPoint> *depthLine)//*********************************************getMotion_can() { ROS_DEBUG("Received scan"); // **** if this is the first scan, initialize and leave the function here if (!initialized_can) { initialized_can = initializeCan(scan); if (initialized_can) ROS_INFO("Matcher initialized"); return; } // **** attmempt to match the two scans // CSM is used in the following way: // The reference scan (prevLDPcan_) always has a pose of 0 // The new scan (currLDPScan) has a pose equal to the movement // of the laser in the world frame since the last scan (btTransform change) // The computed correction is then propagated using the tf machinery prevLDPScan_->odometry[0] = 0; prevLDPScan_->odometry[1] = 0; prevLDPScan_->odometry[2] = 0; prevLDPScan_->estimate[0] = 0; prevLDPScan_->estimate[1] = 0; prevLDPScan_->estimate[2] = 0; prevLDPScan_->true_pose[0] = 0; prevLDPScan_->true_pose[1] = 0; prevLDPScan_->true_pose[2] = 0; btTransform currWorldToBase; btTransform change; change.setIdentity(); // what odometry model to use if (useTfOdometry_) { // get the current position of the base in the world frame // if no transofrm is available, we'll use the last known transform getCurrentEstimatedPose(currWorldToBase, scan); change = laserToBase_ * prevWorldToBase_.inverse() * currWorldToBase * baseToLaser_; } else if (useImuOdometry_) { imuMutex_.lock(); double dTheta = currImuAngle_ - prevImuAngle_; prevImuAngle_ = currImuAngle_; change.getRotation().setRPY(0.0, 0.0, dTheta); imuMutex_.unlock(); } geometry_msgs::Pose2D p; tfToPose2D(change, p); LDP currLDPScan = rosToLDPScan(scan, p); input_.laser_ref = prevLDPScan_; input_.laser_sens = currLDPScan; input_.first_guess[0] = 0; input_.first_guess[1] = 0; input_.first_guess[2] = 0; sm_icp(&input_, &output_); if (!output_.valid) { ROS_WARN("Error in scan matching"); ld_free(prevLDPScan_); prevLDPScan_ = currLDPScan; return; } // **** calculate change in position double dx = output_.x[0]; double dy = output_.x[1]; double da = output_.x[2]; // change = scan match result for how much laser moved between scans, // in the world frame change.setOrigin(btVector3(dx, dy, 0.0)); btQuaternion q; q.setRPY(0, 0, da); change.setRotation(q); frameP_ = worldFrame_; heightLine = ransac(depthLine); // **** publish the new estimated pose as a tf if(take_vicon == true) { prevWorldToBase_.setOrigin(btVector3(pos_vicon[0],pos_vicon[1], pos_vicon[2])); btQuaternion vicon_q(quat_vicon.x(),quat_vicon.y(),quat_vicon.z(),quat_vicon.w()); prevWorldToBase_.setRotation(vicon_q); } currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_; if (publishTf_ ) publishTf (currWorldToBase, scan.header.stamp); if (publishPose_) publishPose(currWorldToBase, scan.header.stamp, frameP_, heightLine); // **** swap old and new ld_free(prevLDPScan_); prevLDPScan_ = currLDPScan; prevWorldToBase_ = currWorldToBase; // **** timing information - needed for profiling only }
int main(int argc, const char ** argv) { sm_set_program_name(argv[0]); struct ld_exp_tro1_params p; options_banner(banner); struct option* ops = options_allocate(10); options_double(ops, "max_xy_error", &p.max_xy_error, 10.0, "Maximum error for x,y (m)"); options_double(ops, "max_theta_error_deg", &p.max_theta_error_deg, 10.0, "Maximum error for orientation (deg)"); options_int (ops, "seed", &p.seed, 0, "Seed for random number generator (if 0, use GSL_RNG_SEED env. variable)."); options_int(ops, "num_per_scan", &p.num_per_scan, 10, "Number of trials for each scan."); options_string(ops, "in", &p.file_input, "stdin", "Input file "); options_string(ops, "out1", &p.file_output1, "stdout", "Output file for first scan"); options_string(ops, "out2", &p.file_output2, "stdout", "Output file for second scan"); options_int(ops, "debug", &p.debug, 0, "Shows debug information"); if(!options_parse_args(ops, argc, argv)) { options_print_help(ops, stderr); return -1; } sm_debug_write(p.debug); gsl_rng_env_setup(); gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0); if(p.seed != 0) gsl_rng_set(rng, (unsigned int) p.seed); /* Open the two output files (possibly the same one) */ FILE * in = open_file_for_reading(p.file_input); if(!in) return -3; FILE * out1 = open_file_for_writing(p.file_output1); if(!out1) return -2; FILE * out2; if(!strcmp(p.file_output1, p.file_output2)) { out1 = out2; } else { out2 = open_file_for_writing(p.file_output2); if(!out2) return -2; } /* Read laser data from input file */ LDP ld; int count=0; while( (ld = ld_read_smart(in))) { count++; if(!ld_valid_fields(ld)) { sm_error("Invalid laser data (#%d in file)\n", count); continue; } for(int n=0; n < p.num_per_scan; n++) { ld->true_pose[0] = 0; ld->true_pose[1] = 0; ld->true_pose[2] = 0; ld->odometry[0] = 0; ld->odometry[1] = 0; ld->odometry[2] = 0; ld_write_as_json(ld, out1); ld->odometry[0] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error; ld->odometry[1] = 2*(gsl_rng_uniform(rng)-0.5) * p.max_xy_error; ld->odometry[2] = 2*(gsl_rng_uniform(rng)-0.5) * deg2rad(p.max_theta_error_deg); ld_write_as_json(ld, out2); } ld_free(ld); } return 0; }
int main(int argc, const char * argv[]) { sm_set_program_name(argv[0]); const char *in_filename; const char *ref_filename; const char *out_filename; const char *ref_field_string; ld_reference ref_field; const char *out_field_string; ld_reference out_field; struct option* ops = options_allocate(15); options_string(ops, "in", &in_filename, "stdin", "scan matching log"); options_string(ops, "ref", &ref_filename, "ref.log", "slam log"); options_string(ops, "out", &out_filename, "stdout", "output file"); options_string(ops, "ref_field", &ref_field_string, "estimate", "What field to find in ref."); options_string(ops, "out_field", &out_field_string, "true_pose", "What field to copy to."); if(!options_parse_args(ops, argc, argv)) { fprintf(stderr, " This program works on two logs: A and B. " "For each scan in A, the program searches for the scan in B having the same timestamp. " "Then, the true_pose field in B is copied to the scan form A, and it is written to the output.\n"); options_print_help(ops, stderr); return -1; } ref_field = ld_string_to_reference(ref_field_string); out_field = ld_string_to_reference(out_field_string); FILE * in_stream = open_file_for_reading(in_filename); FILE * ref_stream = open_file_for_reading(ref_filename); FILE * out_stream = open_file_for_writing(out_filename); if(!in_stream || !ref_stream || !out_stream) return -1; LDP ld_in; while((ld_in = ld_read_smart(in_stream))) { int matched = 0; while(1) { LDP ld_ref = ld_read_smart(ref_stream); if(!ld_ref) break; if(same_scan(ld_in, ld_ref)) { matched = 1; const double *ref_pose = ld_get_reference_pose(ld_ref, ref_field); double *out_pose = ld_get_reference_pose_silent(ld_in, out_field); copy_d(ref_pose, 3, out_pose); ld_write_as_json(ld_in, out_stream); fputs("\n", out_stream); break; } ld_free(ld_ref); } if(!matched) { sm_error("Could not match %s. \n", short_desc(ld_in)); if(feof(ref_stream)) { sm_error("..because ref stream has ended.\n"); break; } continue; } ld_free(ld_in); } return 0; }
int main(int argc, const char*argv[]) { sm_set_program_name(argv[0]); struct sm_params params; struct sm_result result; struct option* ops = options_allocate(100); options_string(ops, "in", &p.file_in, "stdin", "Input file "); options_string(ops, "out", &p.file_out, "stdout", "Output file "); options_string(ops, "out_stats", &p.file_out_stats, "", "Output file (stats) "); options_string(ops, "file_jj", &p.file_jj, "", "File for journaling -- if left empty, journal not open."); options_int(ops, "algo", &p.algo, 0, "Which algorithm to use (0:(pl)ICP 1:gpm-stripped 2:HSM) "); options_int(ops, "debug", &p.debug, 0, "Shows debug information"); options_int(ops, "recover_from_error", &p.recover_from_error, 0, "If true, tries to recover from an ICP matching error"); p.format = 0; /* options_int(ops, "format", &p.format, 0, "Output format (0: log in JSON format, 1: log in Carmen format (not implemented))");*/ sm_options(¶ms, ops); if(!options_parse_args(ops, argc, argv)) { fprintf(stderr, "\n\nUsage:\n"); options_print_help(ops, stderr); return -1; } sm_debug_write(p.debug); /* Open input and output files */ FILE * file_in = open_file_for_reading(p.file_in); if(!file_in) return -1; FILE * file_out = open_file_for_writing(p.file_out); if(!file_out) return -1; if(strcmp(p.file_jj, "")) { FILE * jj = open_file_for_writing(p.file_jj); if(!jj) return -1; jj_set_stream(jj); } FILE * file_out_stats = 0; if(strcmp(p.file_out_stats, "")) { file_out_stats = open_file_for_writing(p.file_out_stats); if(!file_out_stats) return -1; } /* Read first scan */ LDP laser_ref; if(!(laser_ref = ld_read_smart(file_in))) { sm_error("Could not read first scan.\n"); return -1; } if(!ld_valid_fields(laser_ref)) { sm_error("Invalid laser data in first scan.\n"); return -2; } /* For the first scan, set estimate = odometry */ copy_d(laser_ref->odometry, 3, laser_ref->estimate); spit(laser_ref, file_out); int count=-1; LDP laser_sens; while( (laser_sens = ld_read_smart(file_in)) ) { count++; if(!ld_valid_fields(laser_sens)) { sm_error("Invalid laser data in (#%d in file).\n", count); return -(count+2); } params.laser_ref = laser_ref; params.laser_sens = laser_sens; /* Set first guess as the difference in odometry */ if( any_nan(params.laser_ref->odometry,3) || any_nan(params.laser_sens->odometry,3) ) { sm_error("The 'odometry' field is set to NaN so I don't know how to get an initial guess. I usually use the difference in the odometry fields to obtain the initial guess.\n"); sm_error(" laser_ref->odometry = %s \n", friendly_pose(params.laser_ref->odometry) ); sm_error(" laser_sens->odometry = %s \n", friendly_pose(params.laser_sens->odometry) ); sm_error(" I will quit it here. \n"); return -3; } double odometry[3]; pose_diff_d(laser_sens->odometry, laser_ref->odometry, odometry); double ominus_laser[3], temp[3]; ominus_d(params.laser, ominus_laser); oplus_d(ominus_laser, odometry, temp); oplus_d(temp, params.laser, params.first_guess); /* Do the actual work */ switch(p.algo) { case(0): sm_icp(¶ms, &result); break; case(1): sm_gpm(¶ms, &result); break; case(2): sm_hsm(¶ms, &result); break; default: sm_error("Unknown algorithm to run: %d.\n",p.algo); return -1; } if(!result.valid){ if(p.recover_from_error) { sm_info("One ICP matching failed. Because you passed -recover_from_error, I will try to recover." " Note, however, that this might not be good in some cases. \n"); sm_info("The recover is that the displacement is set to 0. No result stats is output. \n"); /* For the first scan, set estimate = odometry */ copy_d(laser_ref->estimate, 3, laser_sens->estimate); ld_free(laser_ref); laser_ref = laser_sens; } else { sm_error("One ICP matching failed. Because I process recursively, I will stop here.\n"); sm_error("Use the option -recover_from_error if you want to try to recover.\n"); ld_free(laser_ref); return 2; } } else { /* Add the result to the previous estimate */ oplus_d(laser_ref->estimate, result.x, laser_sens->estimate); /* Write the corrected log */ spit(laser_sens, file_out); /* Write the statistics (if required) */ if(file_out_stats) { JO jo = result_to_json(¶ms, &result); fputs(jo_to_string(jo), file_out_stats); fputs("\n", file_out_stats); jo_free(jo); } ld_free(laser_ref); laser_ref = laser_sens; } } ld_free(laser_ref); return 0; }
void rb_sm_cleanup() { if(rb_sm_params.laser_ref) ld_free(rb_sm_params.laser_ref); if(rb_sm_params.laser_sens) ld_free(rb_sm_params.laser_sens); }
void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time) { ros::WallTime start = ros::WallTime::now(); // CSM is used in the following way: // The scans are always in the laser frame // The reference scan (prevLDPcan_) has a pose of [0, 0, 0] // The new scan (currLDPScan) has a pose equal to the movement // of the laser in the laser frame since the last scan // The computed correction is then propagated using the tf machinery prev_ldp_scan_->odometry[0] = 0.0; prev_ldp_scan_->odometry[1] = 0.0; prev_ldp_scan_->odometry[2] = 0.0; prev_ldp_scan_->estimate[0] = 0.0; prev_ldp_scan_->estimate[1] = 0.0; prev_ldp_scan_->estimate[2] = 0.0; prev_ldp_scan_->true_pose[0] = 0.0; prev_ldp_scan_->true_pose[1] = 0.0; prev_ldp_scan_->true_pose[2] = 0.0; input_.laser_ref = prev_ldp_scan_; input_.laser_sens = curr_ldp_scan; // **** estimated change since last scan double dt = (time - last_icp_time_).toSec(); double pr_ch_x, pr_ch_y, pr_ch_a; getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt); // the predicted change of the laser's position, in the fixed frame tf::Transform pr_ch; createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch); // account for the change since the last kf, in the fixed frame pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse()); // the predicted change of the laser's position, in the laser frame tf::Transform pr_ch_l; pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ; input_.first_guess[0] = pr_ch_l.getOrigin().getX(); input_.first_guess[1] = pr_ch_l.getOrigin().getY(); input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation()); // *** scan match - using point to line icp from CSM sm_icp(&input_, &output_); tf::Transform corr_ch; if (output_.valid) { // the correction of the laser's position, in the laser frame tf::Transform corr_ch_l; createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l); // the correction of the base's position, in the base frame corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_; // update the pose in the world frame f2b_ = f2b_kf_ * corr_ch; // **** publish if (publish_pose_) { // unstamped Pose2D message geometry_msgs::Pose2D::Ptr pose_msg; pose_msg = boost::make_shared<geometry_msgs::Pose2D>(); pose_msg->x = f2b_.getOrigin().getX(); pose_msg->y = f2b_.getOrigin().getY(); pose_msg->theta = tf::getYaw(f2b_.getRotation()); pose_publisher_.publish(pose_msg); } if (publish_pose_stamped_) { // stamped Pose message geometry_msgs::PoseStamped::Ptr pose_stamped_msg; pose_stamped_msg = boost::make_shared<geometry_msgs::PoseStamped>(); pose_stamped_msg->header.stamp = time; pose_stamped_msg->header.frame_id = fixed_frame_; tf::poseTFToMsg(f2b_, pose_stamped_msg->pose); pose_stamped_publisher_.publish(pose_stamped_msg); } if (publish_tf_) { tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_); tf_broadcaster_.sendTransform (transform_msg); } } else { corr_ch.setIdentity(); ROS_WARN("Error in scan matching"); } // **** swap old and new if (newKeyframeNeeded(corr_ch)) { // generate a keyframe ld_free(prev_ldp_scan_); prev_ldp_scan_ = curr_ldp_scan; f2b_kf_ = f2b_; } else { ld_free(curr_ldp_scan); } last_icp_time_ = time; // **** statistics double dur = (ros::WallTime::now() - start).toSec() * 1e3; ROS_DEBUG("Scan matcher total duration: %.1f ms", dur); }