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>
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
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
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
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
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
00243 try
00244 {
00245 sensor_msgs::CameraInfo camInfo;
00246
00247
00248 tf2::BufferCore tfBuffer(ros::Duration(31536000,0));
00249 if (useGt)
00250 {
00251
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
00269 tfBuffer.setTransform(ts, "default");
00270 ++tfCount;
00271 }
00272 }
00273 bag.close();
00274 cout << "\r" << tfCount << " tf loaded " << endl;
00275 }
00276
00277
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
00285 unsigned expCount(0);
00286 while (ifs.good())
00287 {
00288
00289 reloadParamsFromLine(ifs);
00290 if (params.empty())
00291 break;
00292 cout << "Exp " << expCount << ", loaded " << params.size() << " parameters:\n";
00293 ++expCount;
00294
00295
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
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
00317 if (cloudCount >= cloudLimit)
00318 break;
00319
00320
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
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
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
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
00368 if (mustInitICP)
00369 {
00370 T_gt_init = T_gt;
00371 T_gt_old = T_gt;
00372 }
00373
00374
00375 const TP T_icp = T_gt_init * T_k_to_v * icp.getTransform() * T_v_to_k;
00376
00377
00378 if (mustInitICP)
00379 T_icp_old = T_icp;
00380
00381
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
00399 if (!mustInitICP)
00400 {
00401
00402 const TP T_d_gt = T_gt.inverse() * T_gt_old;
00403
00404 const TP T_d_icp = T_icp.inverse() * T_icp_old;
00405
00406
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
00423 const TP T_d_acc = T_d_gt_acc * T_d_icp_acc.inverse();
00424
00425
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
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
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
00481 if (statofs.good())
00482 {
00483 statofs << processedCount-1 << " " << failCount << " * ";
00484 statofs << icpTotalDuration << " * ";
00485 icp.inspector->dumpStats(statofs);
00486 statofs << " # ";
00487
00488 for (Params::const_iterator it(params.begin()); it != params.end(); ++it)
00489 statofs << it->first << "=" << it->second << " ";
00490
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 }