hector_gps_calibration.cpp
Go to the documentation of this file.
00001 #include "hector_gps_calibration/hector_gps_calibration.h"
00002 #include "hector_gps_calibration/transform_delta_cost_functor.h"
00003 #include "hector_gps_calibration/angle_local_parameterization.h"
00004 
00005 #include <ceres/ceres.h>
00006 #include <geodesy/utm.h>
00007 #include <sensor_msgs/NavSatFix.h>
00008 
00009 #include <iostream>
00010 #include <fstream>
00011 
00012 GPSCalibration::GPSCalibration(ros::NodeHandle &nh)
00013   : tf_listener(tf_buffer),
00014     translation_{{0,0}},
00015     rotation_(0.0)
00016 {
00017   nh.param<double>("translation_x", translation_[0], 0.0);
00018   nh.param<double>("translation_y", translation_[1], 0.0);
00019 
00020   nh.param<double>("orientation", rotation_, 0.0);
00021 
00022   nh.param<bool>("write_debug_file", write_debug_file_, false);
00023   nh.param<double>("max_covariance", max_covariance_, 10.0);
00024   nh.param<double>("min_pose_distance", min_pose_distance_, 0.2);
00025 
00026   ROS_INFO("Initial GPS transformation: \n t: %f %f \n r: %f", translation_[0], translation_[1], rotation_);
00027 
00028   nav_sat_sub_ = nh.subscribe("/odom_gps", 1, &GPSCalibration::navSatCallback, this);
00029   optimize_sub_ = nh.subscribe("gps/run_optimization", 1, &GPSCalibration::navSatCallback, this);
00030 
00031   nav_sat_fix_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps_calibration/gps/fix", 5);
00032 
00033   /*TEST
00034     Eigen::Matrix<double, 3, 1> pos_gps(0, 0, 10);
00035     gps_poses_.emplace_back(pos_gps);
00036     pos_gps = Eigen::Matrix<double, 3, 1>(0, 1, 10);
00037     gps_poses_.emplace_back(pos_gps);
00038     pos_gps = Eigen::Matrix<double, 3, 1>(0, 2, 10);
00039     gps_poses_.emplace_back(pos_gps);
00040     pos_gps = Eigen::Matrix<double, 3, 1>(0, 3, 10);
00041     gps_poses_.emplace_back(pos_gps);
00042 
00043     Eigen::Matrix<double, 3, 1> pos_world(0.1, 0, 0);
00044     world_poses_.emplace_back(pos_world);
00045     pos_world = Eigen::Matrix<double, 3, 1>(0, 1, 0.1);
00046     world_poses_.emplace_back(pos_world);
00047     pos_world = Eigen::Matrix<double, 3, 1>(0, 2, 0);
00048     world_poses_.emplace_back(pos_world);
00049     pos_world = Eigen::Matrix<double, 3, 1>(0, 3, 0);
00050     world_poses_.emplace_back(pos_world);
00051     optimize();*/
00052 
00053   wall_timers_.push_back(nh.createWallTimer(ros::WallDuration(0.1), &GPSCalibration::publishTF, this));
00054 }
00055 
00056 void GPSCalibration::navSatCallback(nav_msgs::Odometry msg)
00057 {
00058   msg.header.stamp = ros::Time::now(); //Ugly hack to work around timestamp issues specific to our robot
00059 
00060   if(msg.pose.covariance[0] > max_covariance_ ) {
00061     //ROS_WARN("Dropping GPS data. Covariance limit exceeded. Covariance: %f > %f", msg.pose.covariance[0], max_covariance_);
00062     return;
00063   }
00064 
00065   Eigen::Matrix<double, 2, 1> pos_gps(msg.pose.pose.position.x,
00066                                       msg.pose.pose.position.y);
00067 
00068   geometry_msgs::TransformStamped transformStamped;
00069   try{
00070     transformStamped = tf_buffer.lookupTransform("world", "navsat_link",
00071                                                  msg.header.stamp, ros::Duration(1.0));
00072   }
00073   catch (tf2::TransformException &ex) {
00074     ROS_WARN("%s",ex.what());
00075 
00076     return;
00077   }
00078 
00079   Eigen::Matrix<double, 2, 1> pos_world(transformStamped.transform.translation.x,
00080                                         transformStamped.transform.translation.y);
00081   bool redundant_data = false;
00082   if(gps_poses_.size() > 0)
00083   {
00084     Eigen::Matrix<double, 2, 1> delta_pose = world_poses_[gps_poses_.size() - 1] - pos_world;
00085     double pose_distance = std::sqrt(delta_pose.transpose() * delta_pose);
00086     if(pose_distance < min_pose_distance_)
00087       redundant_data = true;
00088   }
00089   if(!redundant_data)
00090   {
00091     gps_poses_.emplace_back(pos_gps);
00092     world_poses_.emplace_back(pos_world);
00093     covariances_.emplace_back(msg.pose.covariance[0]);
00094   }
00095 
00096   if((world_poses_.size() % 10 == 0) && world_poses_.size() > 0)
00097     optimize();
00098 
00099 }
00100 
00101 void GPSCalibration::optimizeCallback(std_msgs::Empty msg)
00102 {
00103   optimize();
00104 }
00105 
00106 
00107 void GPSCalibration::optimize()
00108 {
00109   int i = 0;
00110 
00111   ceres::Problem problem;
00112 
00113   for(i = 0; i < world_poses_.size(); ++i)
00114   {
00115     problem.AddResidualBlock(
00116           new ceres::AutoDiffCostFunction<TransformDeltaCostFunctor,
00117           2, 2, 1>(
00118             new TransformDeltaCostFunctor(world_poses_[i],
00119                                           gps_poses_[i],
00120                                           covariances_[i])),
00121           nullptr, translation_.data(), &rotation_);
00122   }
00123 
00124   ceres::LocalParameterization* angle_local_parameterization =
00125       ceres::examples::AngleLocalParameterization::Create();
00126 
00127   problem.SetParameterization(&rotation_, angle_local_parameterization);
00128 
00129 
00130   ceres::Solver::Options options;
00131   options.linear_solver_type = ceres::DENSE_QR;
00132   //  options.minimizer_progress_to_stdout = true;
00133   ceres::Solver::Summary summary;
00134   ceres::Solve(options, &problem, &summary);
00135   if(summary.termination_type != ceres::TerminationType::CONVERGENCE)
00136     ROS_WARN("%s", summary.FullReport().c_str());
00137   ROS_INFO("Translation %f %f", translation_[0], translation_[1]);
00138   ROS_INFO("Rotation %f", rotation_);
00139 
00140   if(write_debug_file_)
00141   {
00142     std::ofstream myfile;
00143     myfile.open ("gps_alignment_solution.csv");
00144     const Rigid3<double> transform = Rigid3<double>(
00145           Eigen::Matrix<double, 3, 1>(translation_[0], translation_[1], 0.0),
00146         Eigen::Quaternion<double>(std::cos(rotation_/2.0), 0.0, 0.0,
00147                                   std::sin(rotation_/2.0)));
00148 
00149     myfile <<"gps_x"<<","<<"gps_y"<<","
00150           <<"world_x"<<","<<"world_y"<<","<<"covariance"<<"\n";
00151     for(size_t i = 0; i<gps_poses_.size(); ++i)
00152     {
00153       const Eigen::Matrix<double, 3, 1> pos_world_gps = transform * Eigen::Matrix<double, 3, 1>(world_poses_[i][0], world_poses_[i][1], 0.0);
00154       myfile << std::setprecision (15)<< gps_poses_[i][0]<<","<<gps_poses_[i][1]<<","
00155              <<pos_world_gps[0]<<","<<pos_world_gps[1]<<","<<covariances_[i]<<"\n";
00156     }
00157     myfile.close();
00158   }
00159 }
00160 
00161 void GPSCalibration::publishTF(const ::ros::WallTimerEvent& unused_timer_event)
00162 {
00163   ros::Time publish_time = ros::Time::now();
00164 
00165   geometry_msgs::TransformStamped utm_world_transform;
00166   utm_world_transform.header.stamp = publish_time;
00167   utm_world_transform.header.frame_id = "utm";
00168   utm_world_transform.child_frame_id = "world";
00169   utm_world_transform.transform.translation.x = translation_[0];
00170   utm_world_transform.transform.translation.y = translation_[1];
00171   utm_world_transform.transform.translation.z = 0;
00172   utm_world_transform.transform.rotation.w = std::cos(rotation_/2.0);
00173   utm_world_transform.transform.rotation.x = 0.0;
00174   utm_world_transform.transform.rotation.y = 0.0;
00175   utm_world_transform.transform.rotation.z = std::sin(rotation_/2.0);
00176   //tf_broadcaster.sendTransform(utm_world_transform);
00177 
00178 
00179   geometry_msgs::TransformStamped worldenu_world_transform;
00180   worldenu_world_transform.header.stamp = publish_time;
00181   worldenu_world_transform.header.frame_id = "world_enu";
00182   worldenu_world_transform.child_frame_id = "world";
00183   worldenu_world_transform.transform.translation.x = 0;
00184   worldenu_world_transform.transform.translation.y = 0;
00185   worldenu_world_transform.transform.translation.z = 0;
00186   worldenu_world_transform.transform.rotation.w = std::cos(rotation_/2.0);
00187   worldenu_world_transform.transform.rotation.x = 0.0;
00188   worldenu_world_transform.transform.rotation.y = 0.0;
00189   worldenu_world_transform.transform.rotation.z = std::sin(rotation_/2.0);
00190   tf_broadcaster.sendTransform(worldenu_world_transform);
00191 
00192   geometry_msgs::TransformStamped world_navsat_transform;
00193   try{
00194     world_navsat_transform = tf_buffer.lookupTransform("world", "base_link",
00195                                                  publish_time, ros::Duration(1.0));
00196   }
00197   catch (tf2::TransformException &ex) {
00198     ROS_WARN("%s",ex.what());
00199     return;
00200   }
00201 
00202 
00203   geometry_msgs::TransformStamped utm_navsat_transform;
00204   tf2::doTransform(world_navsat_transform, utm_navsat_transform, utm_world_transform);
00205 
00206   geodesy::UTMPoint utm_point;
00207   utm_point.band = 'U';
00208   utm_point.zone = 32;
00209   utm_point.easting = utm_navsat_transform.transform.translation.x;
00210   utm_point.northing = utm_navsat_transform.transform.translation.y;
00211   utm_point.altitude = 0.0;
00212   geographic_msgs::GeoPoint geo_point = geodesy::toMsg(utm_point);
00213 
00214   sensor_msgs::NavSatFix nav_sat_fix;
00215   nav_sat_fix.header.stamp = publish_time;
00216   nav_sat_fix.header.frame_id = "base_link";
00217   nav_sat_fix.latitude = geo_point.latitude;
00218   nav_sat_fix.longitude = geo_point.longitude;
00219   nav_sat_fix.altitude = 0.0;
00220   nav_sat_fix_pub_.publish(nav_sat_fix);
00221 
00222 }


hector_gps_calibration
Author(s):
autogenerated on Wed Sep 6 2017 02:41:37