matcher_bag.cpp
Go to the documentation of this file.
00001 
00002 #define EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API
00003 #include "Eigen/Core"
00004 #include "Eigen/Geometry"
00005 #include <map>
00006 #include <string>
00007 #include <sstream>
00008 #include <fstream>
00009 #include <iostream>
00010 #include <cstdio>
00011 #include <cstdlib>
00012 #include <ctime>
00013 #include "boost/foreach.hpp"
00014 #include "message_filters/subscriber.h"
00015 #include "message_filters/time_synchronizer.h"
00016 #include "sensor_msgs/PointCloud2.h"
00017 #include "tf/tfMessage.h"
00018 #include "tf2/buffer_core.h"
00019 #include "rosbag/bag.h"
00020 #include "rosbag/query.h"
00021 #include "rosbag/view.h"
00022 #include "sensor_msgs/CameraInfo.h"
00023 #include <argtable2.h>
00024 #include <regex.h>      /* REG_ICASE */
00025 #include "pointmatcher/PointMatcher.h"
00026 #include "pointmatcher/Timer.h"
00027 #include "pointmatcher_ros/point_cloud.h"
00028 
00029 using namespace std;
00030 using namespace Eigen;
00031 using namespace PointMatcherSupport;
00032 
00033 typedef map<string, string> Params;
00034 
00035 // FIXME: use yaml ?
00036 
00037 Params params;
00038 
00039 bool hasParam(const std::string& name)
00040 {
00041         return params.find(name) != params.end();
00042 }
00043 
00044 template<typename T>
00045 T getParam(const std::string& name, const T& defaultValue)
00046 {
00047         Params::const_iterator it(params.find(name));
00048         if (it != params.end())
00049         {
00050                 T v;
00051                 istringstream iss(it->second);
00052                 iss >> v;
00053                 cerr << "Found parameter: " << name << ", value: " << v << endl;
00054                 return v;
00055         }
00056         else
00057         {
00058                 cerr << "Cannot find value for parameter: " << name << ", assigning default: " << defaultValue << endl;
00059                 return defaultValue;
00060         }
00061 }
00062 
00063 template<typename T>
00064 T getParam(const std::string& name)
00065 {
00066         Params::const_iterator it(params.find(name));
00067         if (it != params.end())
00068         {
00069                 T v;
00070                 istringstream iss(it->second);
00071                 iss >> v;
00072                 cerr << "Found parameter: " << name << ", value: " << v << endl;
00073                 return v;
00074         }
00075         else
00076         {
00077                 cerr << "Cannot find value for parameter: " << name << endl;
00078                 return T();
00079         }
00080 }
00081 
00082 void reloadParamsFromLine(ifstream& ifs)
00083 {
00084         char line[65536];
00085         ifs.getline(line, sizeof(line));
00086         params.clear();
00087         istringstream iss(line);
00088         while (iss.good())
00089         {
00090                 string keyVal;
00091                 iss >> keyVal;
00092                 if (!keyVal.empty())
00093                 {
00094                         const size_t delPos = keyVal.find_first_of('=');
00095                         if (delPos != string::npos)
00096                         {
00097                                 const string key = keyVal.substr(0, delPos);
00098                                 const string val = keyVal.substr(delPos+1);
00099                                 params[key] = val;
00100                         }
00101                 }
00102         }
00103 }
00104 
00105 void openWriteFile(ofstream& stream, arg_file * file, const int errorVal)
00106 {
00107         if (file->count > 0)
00108         {
00109                 stream.open(file->filename[0]);
00110                 if (!stream.good())
00111                 {
00112                         cerr << "Error, invalid " << file->hdr.glossary << " " << file->filename[0] << endl;
00113                         exit (errorVal);
00114                 }
00115                 else
00116                         cout << "Writing to " << file->hdr.glossary << " " << file->filename[0] << endl;
00117         }
00118 }
00119 
00120 #include "icp_chain_creation.h"
00121 
00122 struct Datum
00123 {
00124         TP transform;
00125         DP cloud;
00126         ros::Time stamp;
00127 };
00128 
00129 typedef vector<Datum> Data;
00130 
00131 TP T_v_to_k(TP::Identity(4, 4));
00132 TP T_k_to_v(TP::Identity(4, 4));
00133 
00134 TP msgTransformToTP(const geometry_msgs::Transform& transform)
00135 {
00136         const geometry_msgs::Vector3 gv(transform.translation);
00137         const geometry_msgs::Quaternion gq(transform.rotation);
00138         const Vector3 vect(gv.x, gv.y, gv.z);
00139         const Quaternion<Scalar> quat(gq.w, gq.x, gq.y, gq.z);
00140         const Transform<Scalar,3,Affine> tr = Translation<Scalar,3>(vect) * quat;
00141         return tr.matrix();
00142 }
00143 
00144 int main(int argc, char **argv)
00145 {
00146         arg_file * bagFile = arg_file1(NULL, NULL, "<bag>", "bag file (mandatory)");
00147         arg_str  * cloudTopic = arg_str1(NULL, NULL, "<cloud_topic>", "cloud topic (mandatory)");
00148         arg_file * paramsFile = arg_file1(NULL, NULL, "<params>", "params file (mandatory)");
00149         arg_file * statFile = arg_file0(NULL, "stat", NULL, "statistic output file (default: no output)");
00150         arg_int  * onlyFirsts = arg_int0(NULL, "only_firsts", "<N>", "use only first <N> clouds (default: unlimited)");
00151         
00152         arg_rex  * cmdGt = arg_rex1(NULL, NULL, "gt", NULL, REG_ICASE, "use ground truth");
00153 
00154         arg_str  * gtTargetFrame = arg_str1(NULL, "gt_target_frame", NULL, "tf target frame of ground truth, without heading \"/\" (mandatory)");
00155         arg_str  * gtSourceFrame = arg_str1(NULL, "gt_source_frame", NULL, "tf source frame of ground truth, without heading \"/\" (mandatory)");
00156         
00157         arg_str  * corr = arg_str0(NULL, "corr", NULL, "correction from gt to icp, as a string containing \"t_x t_y t_z q_x q_y q_z q_w\" (default: no correction)");
00158         
00159         arg_file * tfFile = arg_file0(NULL, "tf", NULL, "tf output file (default: no output)");
00160         arg_file * dtfFile = arg_file0(NULL, "dtf", NULL, "delta tf output file (default: no output)");
00161         arg_int  * dtfSteps = arg_int0(NULL, "dtf_steps", NULL, "steps for computing delta tf (default: 30)");
00162         
00163         struct arg_end  * end = arg_end(20);
00164         
00165         void *argTableCloud[] = {bagFile, cloudTopic, paramsFile, onlyFirsts, statFile, tfFile, end};
00166         void* argTableCloudGt[] = {bagFile, cloudTopic, paramsFile, cmdGt, gtTargetFrame, gtSourceFrame, corr, onlyFirsts, statFile, tfFile, dtfFile, dtfSteps, end};
00167                 
00168         if (arg_nullcheck(argTableCloud) != 0 || 
00169                 arg_nullcheck(argTableCloudGt) != 0)
00170                 abort();
00171         
00172         const int nerrorsCloudGt = arg_parse(argc,argv,argTableCloudGt);
00173         const int nerrorsCloud = arg_parse(argc,argv,argTableCloud);
00174         if (nerrorsCloud > 0 && nerrorsCloudGt > 0)
00175         {
00176                 cout << "Error(s) in command line:\n";
00177                 arg_print_errors(stdout,end,argv[0]);
00178                 cout << "\n";
00179                 cout << "Usage, for cloud without ground-truth:\n";
00180                 cout << argv[0];
00181                 arg_print_syntax(stdout,argTableCloud,"\n");
00182                 arg_print_glossary(stdout,argTableCloud,"  %-28s %s\n");
00183                 cout << "\n";
00184                 cout << "Usage, for cloud with ground-truth:\n";
00185                 cout << argv[0];
00186                 arg_print_syntax(stdout,argTableCloudGt,"\n");
00187                 arg_print_glossary(stdout,argTableCloudGt,"  %-28s %s\n");
00188                 return 1;
00189         }
00190         
00191         const bool useGt = (nerrorsCloudGt == 0);
00192         
00193         srand(time(0));
00194         
00195         // setup correction
00196         if (useGt)
00197         {
00198                 if (corr->count > 0)
00199                 {
00200                         istringstream iss(corr->sval[0]);
00201                         Eigen::Vector3f tr;
00202                         iss >> tr(0);
00203                         iss >> tr(1);
00204                         iss >> tr(2);
00205                         Eigen::eigen2_Quaternionf rot;
00206                         iss >> rot.x();
00207                         iss >> rot.y();
00208                         iss >> rot.z();
00209                         iss >> rot.w();
00210                         T_k_to_v = (Eigen::eigen2_Translation3f(tr) * rot).matrix();
00211                         T_v_to_k = T_k_to_v.inverse();
00212                         cout << "Using ground-truth with correction:\n";
00213                         cout << T_k_to_v << "\n";
00214                 }
00215                 else
00216                         cout << "Using ground-truth without correction" << endl;
00217         }
00218         else
00219                 cout << "Not using ground-truth" << endl;
00220         
00221         // load param list
00222         ifstream ifs(paramsFile->filename[0]);
00223         if (!ifs.good())
00224         {
00225                 cerr << "Error, invalid params file " << paramsFile->filename[0] << endl;
00226                 return 3;
00227         }
00228         
00229         // open stat and tf output files, if requested
00230         ofstream statofs, tfofs, dtfofs;
00231         openWriteFile(statofs, statFile, 4);
00232         openWriteFile(tfofs, tfFile, 5);
00233         openWriteFile(dtfofs, dtfFile, 6);
00234         
00235         int deltaTfSteps(30);
00236         if (useGt && dtfSteps->count > 0)
00237         {
00238                 deltaTfSteps = dtfSteps->ival[0];
00239                 cout << "Using tf steps of " << deltaTfSteps << endl;
00240         }
00241         
00242         // load data
00243         try
00244         {
00245                 sensor_msgs::CameraInfo camInfo;
00246                 
00247                 // 1 year of buffer
00248                 tf2::BufferCore tfBuffer(ros::Duration(31536000,0));
00249                 if (useGt)
00250                 {
00251                         // first pass, read tf
00252                         rosbag::Bag bag(bagFile->filename[0]);
00253                         
00254                         unsigned tfCount(0);
00255                         rosbag::View view(bag, rosbag::TopicQuery("/tf"));
00256                         BOOST_FOREACH(rosbag::MessageInstance const m, view)
00257                         {
00258                                 typedef geometry_msgs::TransformStamped TS;
00259                                 tf::tfMessage::ConstPtr tfMsg = m.instantiate<tf::tfMessage>();
00260                                 for (tf::tfMessage::_transforms_type::const_iterator it = tfMsg->transforms.begin(); it != tfMsg->transforms.end(); ++it)
00261                                 {
00262                                         const TS& ts(*it);
00263                                         if (tfCount % 100 == 0)
00264                                         {
00265                                                 cout << "\rLoading tf: " << tfCount << " loaded...";
00266                                                 cout.flush();
00267                                         }
00268                                         //cout << ts.header.stamp << ":" << ts.header.frame_id << " <- " << ts.child_frame_id << endl;
00269                                         tfBuffer.setTransform(ts, "default");
00270                                         ++tfCount;
00271                                 }
00272                         }
00273                         bag.close();
00274                         cout << "\r" << tfCount << " tf loaded             " << endl;
00275                 }
00276                 
00277                 // create labels
00278                 DP::Labels labels;
00279                 labels.push_back(DP::Label("x", 1));
00280                 labels.push_back(DP::Label("y", 1));
00281                 labels.push_back(DP::Label("z", 1));
00282                 labels.push_back(DP::Label("pad", 1));
00283                 
00284                 // for each line in the experiment file
00285                 unsigned expCount(0);
00286                 while (ifs.good())
00287                 {
00288                         // read line and load params
00289                         reloadParamsFromLine(ifs);
00290                         if (params.empty())
00291                                 break;
00292                         cout << "Exp " << expCount << ", loaded " << params.size() << " parameters:\n";
00293                         ++expCount;
00294                         
00295                         // run experiment
00296                         PM::ICPSequence icp;
00297                         populateParameters(icp);
00298                         cout << endl;
00299                         unsigned failCount(0);
00300                         unsigned processedCount(0);
00301                         double icpTotalDuration(0);
00302                         TP T_gt_init;
00303                         TP T_gt_old;
00304                         TP T_icp_old;
00305                         
00306                         TP T_d_gt_acc(TP::Identity(4,4));
00307                         TP T_d_icp_acc(TP::Identity(4,4));
00308                         
00309                         // open bag
00310                         rosbag::Bag bag(bagFile->filename[0]);
00311                         unsigned cloudLimit(onlyFirsts->count > 0 ? onlyFirsts->ival[0] : numeric_limits<unsigned>::max());
00312                         unsigned cloudCount(0);
00313                         rosbag::View view(bag, rosbag::TopicQuery(cloudTopic->sval[0]));
00314                         BOOST_FOREACH(rosbag::MessageInstance const m, view)
00315                         {
00316                                 // make sure we have not reached the limit
00317                                 if (cloudCount >= cloudLimit)
00318                                         break;
00319                                 
00320                                 // get cloud message
00321                                 sensor_msgs::PointCloud2::ConstPtr cloudMsg = m.instantiate<sensor_msgs::PointCloud2>();
00322                                 if (cloudMsg == 0)
00323                                 {
00324                                         cerr << "E " << cloudCount << " : Null cloud message after deserialization, are you sure the cloud topic is really " << cloudTopic->sval[0] << " ?" << endl;
00325                                         abort();
00326                                 }
00327                                 
00328                                 // getting transform, if gt is used
00329                                 string err;
00330                                 TP T_gt = TP::Identity(4,4);
00331                                 if (useGt)
00332                                 {
00333                                         if (tfBuffer.canTransform(gtTargetFrame->sval[0], gtSourceFrame->sval[0], cloudMsg->header.stamp, &err))
00334                                         {
00335                                                 const geometry_msgs::TransformStamped ts = tfBuffer.lookupTransform(gtTargetFrame->sval[0], gtSourceFrame->sval[0], cloudMsg->header.stamp);
00336                                                 T_gt = msgTransformToTP(ts.transform);
00337                                                 cout << "* " << cloudCount << " : Got transform from " << gtSourceFrame->sval[0] << " to " << gtTargetFrame->sval[0] << " at time: " <<  cloudMsg->header.stamp << endl;
00338                                         }
00339                                         else
00340                                         {
00341                                                 cout << "W " << cloudCount << " : Cannot get transform from " << gtSourceFrame->sval[0] << " to " << gtTargetFrame->sval[0] << " at time: " <<  cloudMsg->header.stamp << endl;
00342                                                 ++cloudCount;
00343                                                 continue;
00344                                         }
00345                                 }
00346                                 
00347                                 // create cloud
00348                                 ++processedCount;
00349                                 DP d(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(*cloudMsg));
00350                                 cout << "  Using " << d.features.cols() << " good points on " << cloudMsg->width * cloudMsg->height << "\n";;
00351                                 
00352                                 // apply icp
00353                                 bool mustInitICP(!icp.hasKeyFrame());
00354                                 timer t;
00355                                 try
00356                                 {
00357                                         icp(d);
00358                                         icpTotalDuration += t.elapsed();
00359                                 }
00360                                 catch (PM::ConvergenceError error)
00361                                 {
00362                                         icpTotalDuration += t.elapsed();
00363                                         ++failCount;
00364                                         cerr << "W ICP failed to converge at cloud " << cloudCount << " : " << error.what() << endl;
00365                                 }
00366                                 
00367                                 // if icp has no key frame, init transforms
00368                                 if (mustInitICP)
00369                                 {
00370                                         T_gt_init = T_gt;
00371                                         T_gt_old = T_gt;
00372                                 }
00373                                 
00374                                 // compute T given ICP in ground-truth coordinates
00375                                 const TP T_icp = T_gt_init * T_k_to_v * icp.getTransform() * T_v_to_k;
00376                                 
00377                                 // if icp has no key frame, init transforms
00378                                 if (mustInitICP)
00379                                         T_icp_old = T_icp;
00380                                 
00381                                 // write tf output
00382                                 if (tfofs.good())
00383                                 {
00384                                         tfofs << cloudMsg->header.stamp << " ";
00385                                         
00386                                         const Vector3 t_icp(T_icp.topRightCorner(3,1));
00387                                         const Quaternion<Scalar> q_icp(Matrix3(T_icp.topLeftCorner(3,3)));
00388                                         tfofs << t_icp(0) << " " << t_icp(1) << " " << t_icp(2) << " " << q_icp.x() << " " << q_icp.y() << " " << q_icp.z() << " " << q_icp.w();
00389                                         if (useGt)
00390                                         {
00391                                                 const Vector3 t_gt(T_gt.topRightCorner(3,1));
00392                                                 const Quaternion<Scalar> q_gt(Matrix3(T_gt.topLeftCorner(3,3)));
00393                                                 tfofs << " " << t_gt(0) << " " << t_gt(1) << " " << t_gt(2) << " " << q_gt.x() << " " << q_gt.y() << " " << q_gt.z() << " " << q_gt.w();
00394                                         }
00395                                         tfofs << "\n";
00396                                 }
00397                                 
00398                                 // if not the first frame
00399                                 if (!mustInitICP)
00400                                 {
00401                                         // compute ground-truth transfrom
00402                                         const TP T_d_gt = T_gt.inverse() * T_gt_old;
00403                                         // compute icp inverse transform
00404                                         const TP T_d_icp = T_icp.inverse() * T_icp_old;
00405                                         
00406                                         // compute errors
00407                                         const TP T_d = T_d_gt * T_d_icp.inverse();
00408                                         if (useGt)
00409                                         {
00410                                                 const Vector3 e_t(T_d.topRightCorner(3,1));
00411                                                 icp.inspector->addStat("e_x", e_t(0));
00412                                                 icp.inspector->addStat("e_y", e_t(1));
00413                                                 icp.inspector->addStat("e_z", e_t(2));
00414                                                 const Quaternion<Scalar> quat(Matrix3(T_d.topLeftCorner(3,3)));
00415                                                 icp.inspector->addStat("e_a", 2 * acos(quat.normalized().w()));
00416                                         }
00417                                         
00418                                         if (cloudCount % deltaTfSteps == 0)
00419                                         {
00420                                                 if (useGt)
00421                                                 {
00422                                                         // compute difference
00423                                                         const TP T_d_acc = T_d_gt_acc * T_d_icp_acc.inverse();
00424                                                         
00425                                                         // compute errors
00426                                                         const Vector3 e_t(T_d_acc.topRightCorner(3,1));
00427                                                         icp.inspector->addStat("e_acc_x", e_t(0));
00428                                                         icp.inspector->addStat("e_acc_y", e_t(1));
00429                                                         icp.inspector->addStat("e_acc_z", e_t(2));
00430                                                         const Quaternion<Scalar> quat(Matrix3(T_d_acc.topLeftCorner(3,3)));
00431                                                         icp.inspector->addStat("e_acc_a", 2 * acos(quat.normalized().w()));
00432                                                 }
00433                                                 
00434                                                 // dump deltas
00435                                                 if (dtfofs.good())
00436                                                 {
00437                                                         dtfofs << cloudMsg->header.stamp << " ";
00438                                                         
00439                                                         const Vector3 t_icp(T_d_icp_acc.topRightCorner(3,1));
00440                                                         const Quaternion<Scalar> q_icp(Matrix3(T_d_icp_acc.topLeftCorner(3,3)));
00441                                                         dtfofs << t_icp(0) << " " << t_icp(1) << " " << t_icp(2) << " " << q_icp.x() << " " << q_icp.y() << " " << q_icp.z() << " " << q_icp.w();
00442                                                         if (useGt)
00443                                                         {
00444                                                                 const Vector3 t_gt(T_d_gt_acc.topRightCorner(3,1));
00445                                                                 const Quaternion<Scalar> q_gt(Matrix3(T_d_gt_acc.topLeftCorner(3,3)));
00446                                                                 dtfofs << " " << t_gt(0) << " " << t_gt(1) << " " << t_gt(2) << " " << q_gt.x() << " " << q_gt.y() << " " << q_gt.z() << " " << q_gt.w();
00447                                                         }
00448                                                         dtfofs << "\n";
00449                                                 }
00450                                                 
00451                                                 // reset accumulators
00452                                                 T_d_gt_acc = TP::Identity(4,4);
00453                                                 T_d_icp_acc = TP::Identity(4,4);
00454                                         }
00455                                         else
00456                                         {
00457                                                 T_d_gt_acc = T_d_gt * T_d_gt_acc;
00458                                                 T_d_icp_acc = T_d_icp * T_d_icp_acc;
00459                                         }
00460                                 }
00461                                 
00462                                 T_gt_old = T_gt;
00463                                 T_icp_old = T_icp;
00464                                 
00465                                 ++cloudCount;
00466                         }
00467                         bag.close();
00468                         
00469                         if (cloudCount == 0)
00470                         {
00471                                 cerr << "E : No cloud processed, are you sure the cloud topic is really " << cloudTopic->sval[0] << " ?" << endl;
00472                                 return 0;
00473                         }
00474                         
00475                         if (failCount > 0)
00476                         {
00477                                 cerr << "W : " << failCount << " registrations failed on " << processedCount-1 << " (" << 100.*double(failCount)/double(processedCount-1) << " %)" << endl;
00478                         }
00479                         
00480                         // write general stats
00481                         if (statofs.good())
00482                         {
00483                                 statofs << processedCount-1 << " " << failCount << " * ";
00484                                 statofs << icpTotalDuration << " * ";
00485                                 icp.inspector->dumpStats(statofs);
00486                                 statofs << " # ";
00487                                 // params for info
00488                                 for (Params::const_iterator it(params.begin()); it != params.end(); ++it)
00489                                         statofs << it->first << "=" << it->second << " ";
00490                                 // finish results
00491                                 statofs << "\n";
00492                         }
00493                 }
00494         }
00495         catch (rosbag::BagException e)
00496         {
00497                 cerr << "Error, error reading bag file: " << e.what() << endl;
00498                 return 2;
00499         }
00500         
00501         return 0;
00502 }


ethzasl_icp_mapper_experiments
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:30