bool CanonicalScan::processScan2D(const LDP& curr_ldp_scan,
const LDP& prev_ldp_scan,
const gtsam::Pose2& initial_rel_pose,
gtsam::Pose2& output_rel_pose,
gtsam::Matrix& noise_matrix)
{
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of 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
for(unsigned int i = 0; i < 3; i++)
{
prev_ldp_scan->odometry[i] = 0;
prev_ldp_scan->estimate[i] = 0;
prev_ldp_scan->true_pose[i] = 0;
}
input_.laser_sens = curr_ldp_scan;
input_.laser_ref = prev_ldp_scan;
// **** estimated change since last scan
input_.first_guess[0] = initial_rel_pose.x();
input_.first_guess[1] = initial_rel_pose.y();
input_.first_guess[2] = initial_rel_pose.theta();
// *** scan match - using icp (xy means x and y are already computed)
sm_icp(&input_, &output_);
if (output_.valid)
{
output_rel_pose = gtsam::Pose2(output_.x[0], output_.x[1], output_.x[2]);
//bool valid;
//noise_matrix = gsl_to_gtsam_matrix(output_.cov_x_m, valid);
}
return output_.valid;
}
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 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;
}
int rb_sm_icp() {
sm_icp(&rb_sm_params, &rb_sm_result);
return rb_sm_result.valid;
}
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);
}