optimize.cpp
Go to the documentation of this file.
00001 // kate: replace-tabs off; indent-width 4; indent-mode normal
00002 // vim: ts=4:sw=4:noexpandtab
00003 
00004 #define EIGEN_DONT_VECTORIZE
00005 #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
00006 #include <Eigen/Eigen>
00007 #include <Eigen/LU>
00008 #include <iostream>
00009 #include <fstream>
00010 #include <cmath>
00011 #include <limits>
00012 #include <vector>
00013 #include <map>
00014 
00015 using namespace std;
00016 
00017 static double inlierRatio(0.8);
00018 static int restartCount(1);
00019 static int generationCount(64);
00020 
00021 double uniformRand()
00022 {
00023         return double(rand())/RAND_MAX;
00024 }
00025 
00026 double gaussianRand(double mean, double sigm)
00027 {
00028         // Generation using the Polar (Box-Mueller) method.
00029         // Code inspired by GSL, which is a really great math lib.
00030         // http://sources.redhat.com/gsl/
00031         // C++ wrapper available.
00032         // http://gslwrap.sourceforge.net/
00033         double r, x, y;
00034 
00035         // Generate random number in unity circle.
00036         do
00037         {
00038                 x = uniformRand()*2 - 1;
00039                 y = uniformRand()*2 - 1;
00040                 r = x*x + y*y;
00041         }
00042         while (r > 1.0 || r == 0);
00043 
00044         // Box-Muller transform.
00045         return sigm * y * sqrt (-2.0 * log(r) / r) + mean;
00046 }
00047 
00048 struct TrainingEntry
00049 {
00050         double timeStamp;
00051         Eigen::Vector3d odom_tr;
00052         Eigen::Quaterniond odom_rot;
00053         Eigen::Vector3d icp_tr;
00054         Eigen::Quaterniond icp_rot;
00055 
00056         TrainingEntry(){}
00057         TrainingEntry(std::istream& is)
00058         {
00059                 is >> timeStamp;
00060                 double t_x = 0, t_y = 0, t_z = 0, q_x = 0, q_y = 0, q_z = 0, q_w = 1;
00061                 is >> t_x;
00062                 is >> t_y;
00063                 is >> t_z;
00064                 icp_tr = Eigen::Vector3d(t_x, t_y, t_z);
00065                 is >> q_x;
00066                 is >> q_y;
00067                 is >> q_z;
00068                 is >> q_w;
00069                 icp_rot = Eigen::Quaterniond(q_w, q_x, q_y, q_z).normalized();
00070                 is >> t_x;
00071                 is >> t_y;
00072                 is >> t_z;
00073                 odom_tr = Eigen::Vector3d(t_x, t_y, t_z);
00074                 is >> q_x;
00075                 is >> q_y;
00076                 is >> q_z;
00077                 is >> q_w;
00078                 odom_rot = Eigen::Quaterniond(q_w, q_x, q_y, q_z).normalized();
00079                 //odom_tr = odom_rot*odom_tr;
00080                 //cerr << icp_rot.x() << " " << icp_rot.y() << " " << icp_rot.z() << " " << icp_rot.w() <<endl;
00081                 //cerr << icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << endl;
00082                 // FIXME: bug in Eigen ?
00083                 //icp_tr = icp_rot*icp_tr;
00084                 //cerr << icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << endl;
00085         }
00086 
00087         void dump(ostream& stream) const
00088         {
00089                 stream << 
00090                         timeStamp << " : " << 
00091                         icp_tr.x() << " " << icp_tr.y() << " " << icp_tr.z() << " " <<
00092                         icp_rot.x() << " " << icp_rot.y() << " " << icp_rot.z() << " " << icp_rot.w() << " " <<
00093                         odom_tr.x() << " " << odom_tr.y() << " " << odom_tr.z() << " " <<
00094                         odom_rot.x() << " " << odom_rot.y() << " " << odom_rot.z() << " " << odom_rot.w();
00095         }
00096 };
00097 
00098 struct TrainingSet: public std::vector<TrainingEntry>
00099 {
00100         void dump()
00101         {
00102                 for (TrainingSet::const_iterator it(begin()); it != end(); ++it)
00103                 {
00104                         const TrainingEntry& entry(*it);
00105                         entry.dump(cout);
00106                         cout << "\n";
00107                 }
00108         }
00109 };
00110 
00111 TrainingSet trainingSet;
00112 
00113 struct Params
00114 {
00115         Eigen::Vector3d tr;
00116         Eigen::Quaterniond rot;
00117         
00118         Params():tr(0,0,0),rot(1,0,0,0) {}
00119         Params(const Eigen::Vector3d& _tr, const Eigen::Quaterniond& _rot):
00120                 tr(_tr),
00121                 rot(_rot)
00122                 {}
00123         Params(const double transVariance):
00124                 tr(
00125                         gaussianRand(0, transVariance),
00126                         gaussianRand(0, transVariance),
00127                         gaussianRand(0, transVariance)
00128                 ),
00129                 rot(
00130                         Eigen::Quaterniond(1,0,0,0) *
00131                         Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitX())) *
00132                         Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitY())) *
00133                         Eigen::Quaterniond(Eigen::AngleAxisd(uniformRand() * M_PI*2, Eigen::Vector3d::UnitZ()))
00134                 )
00135                 {}
00136         
00137         // add random noise
00138         const Params& mutate(const double amount = 1.0)
00139         {
00140                 const double count = fabs(gaussianRand(0, 1));
00141                 for (int i = 0; i < int(count + 1); i++)
00142                 {
00143                         const int toMutate = rand() % 6;
00144                         switch (toMutate)
00145                         {
00146                                 case 0: tr.x() += gaussianRand(0, 0.1) * amount; break;
00147                                 case 1: tr.y() += gaussianRand(0, 0.1) * amount; break;
00148                                 case 2: tr.z() += gaussianRand(0, 0.1) * amount; break;
00149                                 case 3: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitX())); break;
00150                                 case 4: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitY())); break;
00151                                 case 5: rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitZ())); break;
00152                                 default: break;
00153                         };
00154                 }
00155                 /*
00156                 tr.x() += gaussianRand(0, 0.1) * amount;
00157                 tr.y() += gaussianRand(0, 0.1) * amount;
00158                 tr.z() += gaussianRand(0, 0.1) * amount;
00159                 rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitX()));
00160                 rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitY()));
00161                 rot *= Eigen::Quaterniond(Eigen::AngleAxisd(gaussianRand(0, M_PI / 8) * amount, Eigen::Vector3d::UnitZ()));
00162                 */
00163                 return *this;
00164         }
00165         
00166         // renormalize quaternion
00167         void normalize()
00168         {
00169                 rot.normalize();
00170                 if (rot.x() < 0)
00171                 {
00172                         rot.x() = -rot.x();
00173                         rot.y() = -rot.y();
00174                         rot.z() = -rot.z();
00175                         rot.w() = -rot.w();
00176                 }
00177         }
00178         
00179         // dump content of this transform
00180         void dumpTransform(ostream& stream) const
00181         {
00182                 stream << tr.x() << " " << tr.y() << " " << tr.z() << " " <<
00183                                 rot.x() << " " << rot.y() << " " << rot.z() << " " << rot.w();
00184         }
00185         
00186         // dump what to copy/paste
00187         void dumpCopyPaste(ostream& stream) const
00188         {
00189                 /*stream <<
00190                         "translation: x=" << tr.x() << " y=" << tr.y() << " z=" << tr.z() << " " <<
00191                         "quaternion: x=" << rot.x() << " y=" << rot.y() << " z=" << rot.z() << " w=" << rot.w();
00192                 */
00193                 stream << "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"base_link_to_kinect\" args=\"";
00194                 dumpTransform(stream);
00195                 stream << " /base_link /kinect 100\"/>" 
00196                 " \n\n OR \n\n"
00197                 "rosrun tf static_transform_publisher ";
00198                 dumpTransform(stream);
00199                 stream << " /base_link /kinect 100\n";
00200         }
00201 };
00202 
00203 // we have two typedefs for the same physical object because they are conceptually different
00204 typedef vector<Params> Genome;
00205 typedef vector<Params> ParamsVector;
00206 
00207 void normalizeParams(ParamsVector& params)
00208 {
00209         for (size_t i = 0; i < params.size(); ++i)
00210                 params[i].normalize();
00211 }
00212 
00213 void dumpParamsStats(ostream& stream, const ParamsVector& params)
00214 {
00215         Eigen::Vector3d trMean(0,0,0);
00216         Eigen::Quaterniond::Coefficients rotMean(0,0,0,0);
00217         for (size_t i = 0; i < params.size(); ++i)
00218         {
00219                 trMean += params[i].tr;
00220                 rotMean += params[i].rot.coeffs();
00221         }
00222         trMean /= double(params.size());
00223         rotMean /= double(params.size());
00224         
00225         Params mean(trMean,Eigen::Quaterniond(rotMean));
00226         mean.normalize();
00227         mean.dumpCopyPaste(stream); stream << "\n";
00228         
00229         Eigen::Vector3d trVar(0,0,0);
00230         Eigen::Quaterniond::Coefficients rotVar(0,0,0,0);
00231         for (size_t i = 0; i < params.size(); ++i)
00232         {
00233                 trVar += (params[i].tr - trMean).cwiseProduct(params[i].tr - trMean);
00234                 rotVar += (params[i].rot.coeffs() - rotMean).cwiseProduct(params[i].rot.coeffs() - rotMean);
00235         }
00236         trVar /= double(params.size());
00237         rotVar /= double(params.size());
00238         
00239         stream << "\nVariance:\n";
00240         Params var(trVar,Eigen::Quaterniond(rotVar));
00241         var.dumpTransform(stream); stream << "\n";
00242         
00243         stream << "\nValues:\n";
00244         for (size_t i = 0; i < params.size(); ++i)
00245         {
00246                 params[i].dumpTransform(stream); stream << "\n";
00247         }
00248         stream << endl;
00249 }
00250 
00251 double computeError(const Params& p, const TrainingEntry& e)
00252 {
00253         /*
00254         // version with Eigen::Matrix4d
00255         Eigen::Matrix4d blk(Eigen::Matrix4d::Identity());
00256         blk.corner(Eigen::TopLeft,3,3) = p.rot.toRotationMatrix();
00257         blk.corner(Eigen::TopRight,3,1) = p.tr;
00258         
00259         Eigen::Matrix4d odom(Eigen::Matrix4d::Identity());
00260         odom.corner(Eigen::TopLeft,3,3) = e.odom_rot.toRotationMatrix();
00261         odom.corner(Eigen::TopRight,3,1) = e.odom_tr;
00262         
00263         Eigen::Matrix4d blk_i(blk.inverse());
00264         
00265         //const Eigen::Matrix4d pred_icp = blk * odom * blk_i;
00266         const Eigen::Matrix4d pred_icp = blk_i * odom * blk;
00267         
00268         const Eigen::Matrix3d pred_icp_rot_m = pred_icp.corner(Eigen::TopLeft,3,3);
00269         const Eigen::Quaterniond pred_icp_rot = Eigen::Quaterniond(pred_icp_rot_m);
00270         const Eigen::Vector3d pred_icp_tr = pred_icp.corner(Eigen::TopRight,3,1);
00271         */
00272         
00273         // version with Eigen::Transform3d
00274         typedef Eigen::Transform<double, 3, Eigen::Affine> Transform3d;
00275         typedef Eigen::Translation<double, 3> Translation3d;
00276         
00277         const Transform3d blk = Translation3d(p.tr) * p.rot;
00278         const Transform3d blk_i = Transform3d(blk.inverse(Eigen::Isometry));
00279         const Transform3d odom = Translation3d(e.odom_tr) * e.odom_rot;
00280         //const Eigen::Transform3d pred_icp = blk * odom * blk_i;
00281         const Transform3d pred_icp = blk_i * odom * blk;
00282         
00283         const Eigen::Matrix3d pred_icp_rot_m = pred_icp.matrix().topLeftCorner(3,3);
00284         const Eigen::Quaterniond pred_icp_rot = Eigen::Quaterniond(pred_icp_rot_m);
00285         const Eigen::Vector3d pred_icp_tr = pred_icp.translation();
00286         
00287         
00288         // identity checked ok
00289         //cerr << "mat:\n" << blk.matrix() * blk_i.matrix() << endl;
00290         
00291         //cout << "dist: " << (e.icp_tr - pred_icp.translation()).norm() << endl;
00292         //cout << "ang dist: " << e.icp_rot.angularDistance(pred_icp_rot) << endl;
00293         //cout << "tr pred icp:\n" << pred_icp.translation() << "\nicp:\n" << e.icp_tr << "\n" << endl;
00294         //cout << "rot pred icp:\n" << pred_icp_rot << "\nicp:\n" << e.icp_rot << "\n" << endl;
00295         // FIXME: tune coefficient for rot vs trans
00296         
00297         const double e_tr((e.icp_tr - pred_icp_tr).norm());
00298         const double e_rot(e.icp_rot.angularDistance(pred_icp_rot));
00299         /*if (e_tr < 0)
00300                 abort();
00301         if (e_rot < 0)
00302                 abort();*/
00303         //cerr << e_tr << " " << e_rot << endl;
00304         return e_tr + e_rot;
00305 }
00306 
00307 // compute errors with outlier rejection
00308 double computeError(const Params& p)
00309 {
00310         vector<double> errors;
00311         errors.reserve(trainingSet.size());
00312         for (TrainingSet::const_iterator it(trainingSet.begin()); it != trainingSet.end(); ++it)
00313         {
00314                 const TrainingEntry& entry(*it);
00315                 errors.push_back(computeError(p, entry));
00316         }
00317         sort(errors.begin(), errors.end());
00318         const size_t inlierCount(errors.size() * inlierRatio);
00319         double error = 0;
00320         for (size_t i = 0; i < inlierCount; ++i)
00321                 error += errors[i];
00322         return error;
00323 }
00324 
00325 double evolveOneGen(Genome& genome, double annealing = 1.0, Params* bestParams = 0)
00326 {
00327         typedef multimap<double, Params> EvaluationMap;
00328         typedef EvaluationMap::iterator EvaluationMapIterator;
00329         EvaluationMap evalutationMap;
00330 
00331         double totalError = 0;
00332         double bestError = numeric_limits<double>::max();
00333         int bestInd = 0;
00334         for (size_t ind = 0; ind < genome.size(); ind++)
00335         {
00336                 const double error = computeError(genome[ind]);
00337                 if (error < bestError)
00338                 {
00339                         bestError = error;
00340                         bestInd = ind;
00341                 }
00342 
00343                 totalError += error;
00344                 evalutationMap.insert(make_pair(error, genome[ind]));
00345 
00346                 /*cout << "E " << ind << " : ";
00347                 genome[ind].dump(cout);
00348                 cout << " = " << error << "\n";*/
00349         }
00350 
00351         if (bestParams)
00352                 *bestParams = genome[bestInd];
00353 
00354         assert((genome.size() / 4) * 4 == genome.size());
00355 
00356         size_t ind = 0;
00357         for (EvaluationMapIterator it = evalutationMap.begin(); ind < genome.size() / 4; ++it, ++ind )
00358         {
00359                 //cout << "S " << it->first << "\n";
00360                 genome[ind * 4] = it->second;
00361                 genome[ind * 4 + 1] = it->second.mutate(annealing);
00362                 genome[ind * 4 + 2] = it->second.mutate(annealing);
00363                 genome[ind * 4 + 3] = it->second.mutate(annealing);
00364         }
00365 
00366         return bestError;
00367 }
00368 
00369 int main(int argc, char** argv)
00370 {
00371         srand(time(0));
00372         
00373         if (argc < 2)
00374         {
00375                 cerr << "Usage " << argv[0] << " LOG_FILE_NAME [INLIER_RATIO] [RESTART_COUNT] [GEN_COUNT]\n";
00376                 cerr << "  LOG_FILE_NAME   name of file to load containing delta tf\n";
00377                 cerr << "  INLIER_RATIO    ratio of inlier to use for error computation (range: ]0:1], default: 0.8)\n";
00378                 cerr << "  RESTART_COUNT   number of random restart (default: 1)\n";
00379                 cerr << "  GEN_COUNT       number of generation for the ES (default: 64)\n";
00380                 cerr << endl;
00381                 return 1;
00382         }
00383         if (argc >= 3)
00384         {
00385                 inlierRatio = atof(argv[2]);
00386                 if (inlierRatio <= 0 || inlierRatio > 1)
00387                 {
00388                         cerr << "Invalid inline ratio: " << inlierRatio << ", must be within range ]0:1]" << endl;
00389                         return 2;
00390                 }
00391         }
00392         cout << "Inlier ratio: " << inlierRatio << endl;
00393         if (argc >= 4)
00394         {
00395                 restartCount = atoi(argv[3]);
00396                 if (restartCount < 1)
00397                 {
00398                         cerr << "Invalid restart count: " << restartCount << ", must be greater than 0" << endl;
00399                         return 3;
00400                 }
00401         }
00402         cout << "Restart count: " << restartCount << endl;
00403         if (argc >= 5)
00404         {
00405                 generationCount = atoi(argv[4]);
00406                 if (generationCount < 1)
00407                 {
00408                         cerr << "Invalid generation count: " << generationCount << ", must be greater than 0" << endl;
00409                         return 4;
00410                 }
00411         }
00412         cout << "Generation count: " << generationCount << endl;
00413         
00414         ifstream ifs(argv[1]);
00415         while (ifs.good())
00416         {
00417                 TrainingEntry e(ifs);
00418                 if (ifs.good())
00419                 {
00420                         trainingSet.push_back(e);
00421                         //e.dump(cerr); cerr << endl;
00422                 }
00423         }
00424         cout << "Loaded " << trainingSet.size() << " training entries" << endl;
00425         //trainingSet.dump();
00426         
00427         ParamsVector bests;
00428         for (int i = 0; i < restartCount; ++i)
00429         {
00430                 cout << "Starting " << i << " restart on " << restartCount << endl;
00431                 Genome genome(1024);
00432                 for (size_t g = 0; g < genome.size(); ++g)
00433                         genome[g] = Params(0.5);
00434                 for (int g = 0; g < generationCount; ++g)
00435                 {
00436                         cout << "\r" << g << " best has error " << evolveOneGen(genome, 2. * (double)(generationCount - g) / (double)(generationCount)) << "            ";
00437                         cout.flush();
00438                 }
00439                 Params best;
00440                 cout << "\r  Best error of restart " << i << ": " << evolveOneGen(genome, 1.0, &best) << endl;
00441                 bests.push_back(best);
00442         }
00443         
00444         cout << "\nOptimization completed, code to COPY-PASTE in to use the transformation:\n\n";
00445         if (restartCount == 1)
00446         {
00447                 bests[0].dumpCopyPaste(cout);
00448         }
00449         else
00450         {
00451                 normalizeParams(bests);
00452                 dumpParamsStats(cout, bests);
00453         }
00454         
00455         return 0;
00456 }


ethzasl_extrinsic_calibration
Author(s): François Pomerleau, Francis Colas, and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:40