Last active
July 29, 2019 15:52
-
-
Save catree/62d086714ab0b0ec71c38172a3389aa1 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #include <algorithm> // std::transform | |
| #include <map> | |
| #include <fstream> | |
| #include <iostream> | |
| #include <opencv2/core.hpp> | |
| #include <opencv2/core/utils/filesystem.hpp> | |
| #include <opencv2/calib3d.hpp> | |
| namespace | |
| { | |
| using namespace cv; | |
| using namespace std; | |
| static Point3d transform(const Point3d& pt, const Matx33d& R, const Matx31d& tvec) | |
| { | |
| Point3d pt_trans; | |
| pt_trans.x = R(0,0)*pt.x + R(0,1)*pt.y + R(0,2)*pt.z + tvec(0); | |
| pt_trans.y = R(1,0)*pt.x + R(1,1)*pt.y + R(1,2)*pt.z + tvec(1); | |
| pt_trans.z = R(2,0)*pt.x + R(2,1)*pt.y + R(2,2)*pt.z + tvec(2); | |
| return pt_trans; | |
| } | |
| static void generatePose(const vector<Point3d>& points, RNG& randGen, Matx31d& rvec, Matx31d& tvec, int nbTrials=10) | |
| { | |
| bool validPose = false; | |
| for (int trial = 0; trial < nbTrials && !validPose; trial++) { | |
| rvec = Matx31d(randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0)); | |
| tvec = Matx31d(randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0), randGen.uniform(0.5, 2.0)); | |
| Matx33d R; | |
| Rodrigues(rvec, R); | |
| bool positiveDepth = true; | |
| for (size_t i = 0; i < points.size() && positiveDepth; i++) { | |
| Point3d pt_cam = transform(points[i], R, tvec); | |
| if (pt_cam.z <= 0) { | |
| positiveDepth = false; | |
| } | |
| } | |
| validPose = positiveDepth; | |
| } | |
| } | |
| static double max3(double a, double b, double c) | |
| { | |
| double max1 = (a < b) ? b : a; | |
| return (max1 < c) ? c : max1; | |
| } | |
| static double safeAcos(double x) | |
| { | |
| if (x < -1.0) x = -1.0; | |
| else if (x > 1.0) x = 1.0; | |
| return acos (x) ; | |
| } | |
| static double getRotationError(const Matx31d& rvec_ref, const Matx31d& rvec_est) | |
| { | |
| Mat R_ref, R_est; | |
| Rodrigues(rvec_ref, R_ref); | |
| Rodrigues(rvec_est, R_est); | |
| double err0 = safeAcos( Mat(R_ref.col(0).t() * R_est.col(0)).at<double>(0,0) ) * 180 / M_PI; | |
| double err1 = safeAcos( Mat(R_ref.col(1).t() * R_est.col(1)).at<double>(0,0) ) * 180 / M_PI; | |
| double err2 = safeAcos( Mat(R_ref.col(2).t() * R_est.col(2)).at<double>(0,0) ) * 180 / M_PI; | |
| return max3(err0, err1, err2); | |
| } | |
| static double getTranslationError(const Matx31d& tvec_ref, const Matx31d& tvec_est) | |
| { | |
| return sqrt(norm((tvec_ref - tvec_est), NORM_L2SQR)) / sqrt(norm(tvec_est, NORM_L2SQR)) * 100; | |
| } | |
| struct ErrorInfo | |
| { | |
| double rotationError; | |
| double translationError; | |
| double computationTime; | |
| ErrorInfo(double rotErr, double tErr, double time) : | |
| rotationError(rotErr), translationError(tErr), computationTime(time) {} | |
| }; | |
| static double opExtractRotation(const ErrorInfo& err) | |
| { | |
| return err.rotationError; | |
| } | |
| static double opExtractTranslation(const ErrorInfo& err) | |
| { | |
| return err.translationError; | |
| } | |
| static double opExtractTime(const ErrorInfo& err) | |
| { | |
| return err.computationTime; | |
| } | |
| static void getVectorOfErrors(const vector<ErrorInfo>& errors, vector<double>& rotationErrors, | |
| vector<double>& translationErrors, vector<double>& computationTimes) | |
| { | |
| rotationErrors.resize(errors.size()); | |
| translationErrors.resize(errors.size()); | |
| computationTimes.resize(errors.size()); | |
| transform(errors.begin(), errors.end(), rotationErrors.begin(), opExtractRotation); | |
| transform(errors.begin(), errors.end(), translationErrors.begin(), opExtractTranslation); | |
| transform(errors.begin(), errors.end(), computationTimes.begin(), opExtractTime); | |
| } | |
| static ErrorInfo getPoseError(const Matx31d& rvec_ref, const Matx31d& tvec_ref, const Matx31d& rvec_est, const Matx31d& tvec_est, double time) | |
| { | |
| return ErrorInfo(getRotationError(rvec_ref, rvec_est), getTranslationError(tvec_ref, tvec_est), time); | |
| } | |
| static void runTest(const vector<Point3d>& objectPoints, | |
| vector<ErrorInfo>& errorsEPnP, | |
| vector<ErrorInfo>& errorsDLS, | |
| vector<ErrorInfo>& errorsIterative, | |
| vector<ErrorInfo>& errorsIPPE, | |
| vector<ErrorInfo>& errorsEPnPVVS, | |
| vector<ErrorInfo>& errorsDLSVVS, | |
| vector<ErrorInfo>& errorsIPPEVVS, | |
| vector<ErrorInfo>& errorsEPnPLM, | |
| vector<ErrorInfo>& errorsDLSLM, | |
| vector<ErrorInfo>& errorsIPPELM, | |
| vector<ErrorInfo>& errorsUPnP, | |
| bool planar, | |
| RNG& rng, | |
| int gaussianSigma=0, int nbTrials=100) | |
| { | |
| Matx33d cameraMatrix(600.0, 0.0, 320.0, | |
| 0.0, 600.0, 240.0, | |
| 0.0, 0.0, 1.0); | |
| for (int trial = 0; trial < nbTrials; trial++) { | |
| Matx31d rvec_ref, tvec_ref; | |
| generatePose(objectPoints, rng, rvec_ref, tvec_ref); | |
| vector<Point2d> imagePoints; | |
| projectPoints(objectPoints, rvec_ref, tvec_ref, cameraMatrix, noArray(), imagePoints); | |
| if (gaussianSigma > 0) { | |
| for (size_t i = 0; i < imagePoints.size(); i++) { | |
| imagePoints[i] += Point2d(rng.gaussian(gaussianSigma), rng.gaussian(gaussianSigma)); | |
| } | |
| } | |
| { | |
| TickMeter tm1; | |
| tm1.start(); | |
| Matx31d rvec, tvec; | |
| bool res = solvePnP(objectPoints, imagePoints, cameraMatrix, noArray(), rvec, tvec, false, SOLVEPNP_EPNP); | |
| tm1.stop(); | |
| if (res) { | |
| errorsEPnP.push_back(getPoseError(rvec_ref, tvec_ref, rvec, tvec, tm1.getTimeMilli())); | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineVVS(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.stop(); | |
| errorsEPnPVVS.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineLM(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.stop(); | |
| errorsEPnPLM.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| } | |
| } | |
| { | |
| TickMeter tm1; | |
| tm1.start(); | |
| Matx31d rvec, tvec; | |
| bool res = solvePnP(objectPoints, imagePoints, cameraMatrix, noArray(), rvec, tvec, false, SOLVEPNP_DLS); | |
| tm1.stop(); | |
| if (res) { | |
| errorsDLS.push_back(getPoseError(rvec_ref, tvec_ref, rvec, tvec, tm1.getTimeMilli())); | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineVVS(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.stop(); | |
| errorsDLSVVS.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineLM(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.stop(); | |
| errorsDLSLM.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| } | |
| } | |
| { | |
| TickMeter tm1; | |
| tm1.start(); | |
| Matx31d rvec, tvec; | |
| bool res = solvePnP(objectPoints, imagePoints, cameraMatrix, noArray(), rvec, tvec, false, SOLVEPNP_ITERATIVE); | |
| tm1.stop(); | |
| if (res) { | |
| errorsIterative.push_back(getPoseError(rvec_ref, tvec_ref, rvec, tvec, tm1.getTimeMilli())); | |
| } | |
| } | |
| if (planar) | |
| { | |
| TickMeter tm1; | |
| tm1.start(); | |
| Matx31d rvec, tvec; | |
| bool res = solvePnP(objectPoints, imagePoints, cameraMatrix, noArray(), rvec, tvec, false, SOLVEPNP_IPPE); | |
| tm1.stop(); | |
| if (res) { | |
| errorsIPPE.push_back(getPoseError(rvec_ref, tvec_ref, rvec, tvec, tm1.getTimeMilli())); | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineVVS(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.stop(); | |
| errorsIPPEVVS.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| { | |
| TickMeter tm2; | |
| tm2.start(); | |
| Matx31d rvec2 = rvec, tvec2 = tvec; | |
| solvePnPRefineLM(objectPoints, imagePoints, cameraMatrix, noArray(), rvec2, tvec2); | |
| tm2.getTimeMilli(); | |
| errorsIPPELM.push_back(getPoseError(rvec_ref, tvec_ref, rvec2, tvec2, tm1.getTimeMilli()+tm2.getTimeMilli())); | |
| } | |
| } | |
| } | |
| { | |
| TickMeter tm1; | |
| tm1.start(); | |
| Matx31d rvec, tvec; | |
| bool res = solvePnP(objectPoints, imagePoints, cameraMatrix, noArray(), rvec, tvec, false, SOLVEPNP_UPNP); | |
| tm1.stop(); | |
| if (res) { | |
| errorsUPnP.push_back(getPoseError(rvec_ref, tvec_ref, rvec, tvec, tm1.getTimeMilli())); | |
| } | |
| } | |
| } | |
| } | |
| static vector<Point3d> generateRandomObjectPointsSkew(int nbPoints, RNG& randGen, bool planar=false, double size=0.2) | |
| { | |
| vector<Point3d> points; | |
| points.reserve(nbPoints); | |
| for (int i = 0; i < nbPoints; i++) { | |
| points.push_back(Point3d(randGen.uniform(0.0, 2*size), | |
| randGen.uniform(0.0, 2*size), | |
| planar ? 0.0 : randGen.uniform(0.0, 2*size))); | |
| } | |
| if (planar) { | |
| Matx31d rvec(randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0)); | |
| Matx31d tvec(randGen.uniform(-size, size), randGen.uniform(-size, size), randGen.uniform(-size, size)); | |
| Matx33d R; | |
| Rodrigues(rvec, R); | |
| for (size_t i = 0; i < points.size(); i++) { | |
| points[i] = transform(points[i], R, tvec); | |
| } | |
| } | |
| return points; | |
| } | |
| static vector<Point3d> generateRandomObjectPoints(int nbPoints, RNG& randGen, bool planar=false, double size=0.2) | |
| { | |
| vector<Point3d> points; | |
| points.reserve(nbPoints); | |
| for (int i = 0; i < nbPoints; i++) { | |
| points.push_back(Point3d(randGen.uniform(-size, size), | |
| randGen.uniform(-size, size), | |
| planar ? 0.0 : randGen.uniform(-size, size))); | |
| } | |
| if (planar) { | |
| Matx31d rvec(randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0), randGen.uniform(-1.0, 1.0)); | |
| Matx31d tvec; | |
| Matx33d R; | |
| Rodrigues(rvec, R); | |
| for (size_t i = 0; i < points.size(); i++) { | |
| points[i] = transform(points[i], R, tvec); | |
| } | |
| } | |
| return points; | |
| } | |
| struct TestResults | |
| { | |
| double meanRotationError; | |
| double meanTranslationError; | |
| double medianRotationError; | |
| double medianTranslationError; | |
| double stdRotationError; | |
| double stdTranslationError; | |
| TestResults(double meanRot, double meanTrans, double medianRot, double medianTrans, | |
| double stdRot, double stdTrans) : | |
| meanRotationError(meanRot), meanTranslationError(meanTrans), medianRotationError(medianRot), | |
| medianTranslationError(medianTrans), stdRotationError(stdRot), stdTranslationError(stdTrans) {} | |
| TestResults() : | |
| meanRotationError(0), meanTranslationError(0), medianRotationError(0), | |
| medianTranslationError(0), stdRotationError(0), stdTranslationError(0) {} | |
| }; | |
| struct FullTestResults | |
| { | |
| vector<double> rotationErrors; | |
| vector<double> translationErrors; | |
| vector<double> computationTimes; | |
| FullTestResults(const vector<double>& rotErrors, const vector<double>& transErrors, const vector<double>& compTimes) : | |
| rotationErrors(rotErrors), translationErrors(transErrors), computationTimes(compTimes) {} | |
| FullTestResults() : | |
| rotationErrors(), translationErrors(), computationTimes() {} | |
| }; | |
| static void runTestNumberPoints(bool planar, bool skew, int minPoints=4, int maxPoints=100) | |
| { | |
| RNG rng(0x123456798); | |
| map<int, map<string, TestResults> > mapOfResults; | |
| map<int, map<string, FullTestResults> > mapOfFullResults; | |
| for (int nbPoints = minPoints; nbPoints <= maxPoints; nbPoints++) { | |
| vector<Point3d> points; | |
| if (skew) { | |
| points = generateRandomObjectPointsSkew(nbPoints, rng, planar); | |
| } else { | |
| points = generateRandomObjectPoints(nbPoints, rng, planar); | |
| } | |
| vector<ErrorInfo> errorsEPnP, errorsDLS, errorsIterative, errorsIPPE; | |
| vector<ErrorInfo> errorsEPnPVVS, errorsDLSVVS, errorsIPPEVVS; | |
| vector<ErrorInfo> errorsEPnPLM, errorsDLSLM, errorsIPPELM; | |
| vector<ErrorInfo> errorsUPnP; | |
| const int gaussianSigma = 2; //constant Gaussian noise of 2 | |
| runTest(points, | |
| errorsEPnP, errorsDLS, errorsIterative, errorsIPPE, | |
| errorsEPnPVVS, errorsDLSVVS, errorsIPPEVVS, | |
| errorsEPnPLM, errorsDLSLM, errorsIPPELM, | |
| errorsUPnP, | |
| planar, | |
| rng, | |
| gaussianSigma); | |
| map<string, FullTestResults> mapOfFullTestResults; | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnP, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIterative, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["Iterative"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPE, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //VVS | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnPVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLSVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPEVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //LM | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnPLM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLSLM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPELM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //UPnP | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsUPnP, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["UPnP"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| mapOfFullResults[nbPoints] = mapOfFullTestResults; | |
| } | |
| { | |
| string skew_folder = skew ? "skew/" : "no_skew/"; | |
| string planar_folder = planar ? "planar/" : "no_planar/"; | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "EPnP"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "DLS"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "Iterative"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "EPnP_VVS"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "DLS_VVS"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "EPnP_LM"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "DLS_LM"); | |
| if (planar) | |
| { | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "IPPE"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "IPPE_VVS"); | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "IPPE_LM"); | |
| } | |
| utils::fs::createDirectories("Results_CV/Number_of_Points/" + planar_folder + skew_folder + "UPnP"); | |
| for (map<int, map<string, FullTestResults> >::const_iterator it1 = mapOfFullResults.begin(); it1 != mapOfFullResults.end(); ++it1) { | |
| map<string, FullTestResults>::const_iterator it2 = it1->second.find("EPnP"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sEPnP/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sDLS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("Iterative"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sIterative/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sIPPE/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //VVS | |
| it2 = it1->second.find("EPnP VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sEPnP_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sDLS_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sIPPE_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //LM | |
| it2 = it1->second.find("EPnP LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sEPnP_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sDLS_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sIPPE_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //UPnP | |
| it2 = it1->second.find("UPnP"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Number_of_Points/%s%sUPnP/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } | |
| static void runTestGaussianNoise(int nbPoints, bool planar, bool skew, int minGaussianSigma=0, int maxGaussianSigma=20) | |
| { | |
| RNG rng(0x123456798); | |
| map<int, map<string, TestResults> > mapOfResults; | |
| map<int, map<string, FullTestResults> > mapOfFullResults; | |
| for (int gaussianSigma = minGaussianSigma; gaussianSigma <= maxGaussianSigma; gaussianSigma++) { | |
| vector<Point3d> points; | |
| if (skew) { | |
| points = generateRandomObjectPointsSkew(nbPoints, rng, planar); | |
| } else { | |
| points = generateRandomObjectPoints(nbPoints, rng, planar); | |
| } | |
| vector<ErrorInfo> errorsEPnP, errorsDLS, errorsIterative, errorsIPPE; | |
| vector<ErrorInfo> errorsEPnPVVS, errorsDLSVVS, errorsIPPEVVS; | |
| vector<ErrorInfo> errorsEPnPLM, errorsDLSLM, errorsIPPELM; | |
| vector<ErrorInfo> errorsUPnP; | |
| runTest(points, | |
| errorsEPnP, errorsDLS, errorsIterative, errorsIPPE, | |
| errorsEPnPVVS, errorsDLSVVS, errorsIPPEVVS, | |
| errorsEPnPLM, errorsDLSLM, errorsIPPELM, | |
| errorsUPnP, | |
| planar, | |
| rng, | |
| gaussianSigma); | |
| map<string, FullTestResults> mapOfFullTestResults; | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnP, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIterative, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["Iterative"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPE, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //VVS | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnPVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLSVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPEVVS, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE VVS"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //LM | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsEPnPLM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["EPnP LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsDLSLM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["DLS LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| if (planar) | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsIPPELM, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["IPPE LM"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| //UPnP | |
| { | |
| vector<double> rotationErrors, translationErrors, computationTimes; | |
| getVectorOfErrors(errorsUPnP, rotationErrors, translationErrors, computationTimes); | |
| mapOfFullTestResults["UPnP"] = FullTestResults(rotationErrors, translationErrors, computationTimes); | |
| } | |
| mapOfFullResults[gaussianSigma] = mapOfFullTestResults; | |
| } | |
| { | |
| string skew_folder = skew ? "skew/" : "no_skew/"; | |
| string planar_folder = planar ? "planar/" : "no_planar/"; | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "EPnP"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "DLS"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "Iterative"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "EPnP_VVS"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "DLS_VVS"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "EPnP_LM"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "DLS_LM"); | |
| if (planar) | |
| { | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "IPPE"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "IPPE_VVS"); | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "IPPE_LM"); | |
| } | |
| utils::fs::createDirectories("Results_CV/Gaussian_Noise/" + planar_folder + skew_folder + "UPnP"); | |
| for (map<int, map<string, FullTestResults> >::const_iterator it1 = mapOfFullResults.begin(); it1 != mapOfFullResults.end(); ++it1) { | |
| map<string, FullTestResults>::const_iterator it2 = it1->second.find("EPnP"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sEPnP/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sDLS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("Iterative"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sIterative/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sIPPE/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //VVS | |
| it2 = it1->second.find("EPnP VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sEPnP_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sDLS_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE VVS"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sIPPE_VVS/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //LM | |
| it2 = it1->second.find("EPnP LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sEPnP_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("DLS LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sDLS_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| it2 = it1->second.find("IPPE LM"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sIPPE_LM/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| //UPnP | |
| it2 = it1->second.find("UPnP"); | |
| if (it2 != it1->second.end()) { | |
| string pose_filename = format("Results_CV/Gaussian_Noise/%s%sUPnP/pose_%04d.txt", planar_folder.c_str(), skew_folder.c_str(), it1->first); | |
| ofstream f(pose_filename); | |
| if (f.is_open()) { | |
| for (size_t i = 0; i < it2->second.rotationErrors.size(); i++) { | |
| f << it2->second.rotationErrors[i] << " " << it2->second.translationErrors[i] << " " << it2->second.computationTimes[i] << endl; | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } | |
| } //namespace | |
| int main() | |
| { | |
| { | |
| const bool planar = false; | |
| // { | |
| // const bool skew = false; | |
| // const int minPoints = 6; | |
| // runTestNumberPoints(planar, skew, minPoints); | |
| // } | |
| // { | |
| // const bool skew = false; | |
| // const int nbPoints = 30; | |
| // runTestGaussianNoise(nbPoints, planar, skew); | |
| // } | |
| { | |
| const bool skew = true; | |
| const int minPoints = 6; | |
| runTestNumberPoints(planar, skew, minPoints); | |
| } | |
| { | |
| const bool skew = true; | |
| const int nbPoints = 30; | |
| runTestGaussianNoise(nbPoints, planar, skew); | |
| } | |
| } | |
| { | |
| const bool planar = true; | |
| // { | |
| // const bool skew = false; | |
| // const int minPoints = 6; | |
| // runTestNumberPoints(planar, skew, minPoints); | |
| // } | |
| // { | |
| // const bool skew = false; | |
| // const int nbPoints = 30; | |
| // runTestGaussianNoise(nbPoints, planar, skew); | |
| // } | |
| { | |
| const bool skew = true; | |
| const int minPoints = 6; | |
| runTestNumberPoints(planar, skew, minPoints); | |
| } | |
| { | |
| const bool skew = true; | |
| const int nbPoints = 30; | |
| runTestGaussianNoise(nbPoints, planar, skew); | |
| } | |
| } | |
| return EXIT_SUCCESS; | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| # -*- coding: utf-8 -*- | |
| from __future__ import division | |
| from __future__ import print_function | |
| import matplotlib.pyplot as plt | |
| import numpy as np | |
| import argparse | |
| def main(): | |
| parser = argparse.ArgumentParser(description='Plot benchmark solvePnP methods against Gaussian Image Noise.') | |
| parser.add_argument('--plot', type=str, help='Directory path to file errors') | |
| parser.add_argument('--ymax_rot', type=int, default=-1, help='ymax limit for rotation') | |
| parser.add_argument('--ymax_trans', type=int, default=-1, help='ymax limit for translation') | |
| parser.add_argument('--ymax_times', type=int, default=-1, help='ymax limit for computation times') | |
| parser.add_argument('--planar', type=bool, default=False, help='True if planar object') | |
| args = parser.parse_args() | |
| planar = args.planar | |
| font = {'family' : 'normal', | |
| 'weight' : 'normal', | |
| 'size' : 22} | |
| plt.rc('font', **font) | |
| # methods = ['EPnP', 'DLS', 'Iterative'] | |
| # labels = ['EPnP', 'DLS', 'Iterative'] | |
| # methods = ['EPnP', 'EPnP_VVS', 'EPnP_LM'] | |
| # labels = ['EPnP', 'EPnP + VVS', 'EPnP + LM'] | |
| # methods = ['DLS', 'DLS_VVS', 'DLS_LM'] | |
| # labels = ['DLS', 'DLS + VVS', 'DLS + LM'] | |
| # methods = ['EPnP_VVS', 'DLS_VVS', 'Iterative'] | |
| # labels = ['EPnP + VVS', 'DLS + VVS', 'Iterative'] | |
| # methods = ['EPnP_LM', 'DLS_LM', 'Iterative'] | |
| # labels = ['EPnP + LM', 'DLS + LM', 'Iterative'] | |
| methods = ['DLS_VVS', 'DLS_LM', 'Iterative'] | |
| labels = ['DLS + VVS', 'DLS + LM', 'Iterative'] | |
| if (planar): | |
| # methods.append('IPPE') | |
| # labels.append('IPPE') | |
| methods.append('IPPE_VVS') | |
| labels.append('IPPE + VVS') | |
| colors = ['b', 'g', 'r', 'c', 'm', 'y'] | |
| boxplots_rot = [] | |
| boxplots_trans = [] | |
| boxplots_times = [] | |
| nb_methods = len(methods) | |
| min_gaussian_noise = 0 | |
| max_gausssian_noise = 20 | |
| x_step = 1 | |
| _, ax1 = plt.subplots() | |
| _, ax2 = plt.subplots() | |
| _, ax3 = plt.subplots() | |
| for idx, method in enumerate(methods): | |
| method_data_rot = [] | |
| method_data_trans = [] | |
| method_data_times = [] | |
| positions = [] | |
| for i in range(min_gaussian_noise,max_gausssian_noise): | |
| # load data | |
| filename = (args.plot + '/' + method + '/pose_%04d.txt') % i | |
| data_pose = np.loadtxt(filename) | |
| positions.append(i*(nb_methods+1) + idx) | |
| method_data_rot.append(data_pose[:,0]) | |
| method_data_trans.append(data_pose[:,1]) | |
| method_data_times.append(data_pose[:,2]) | |
| boxplots_rot.append(ax1.boxplot(method_data_rot, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| boxplots_trans.append(ax2.boxplot(method_data_trans, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| boxplots_times.append(ax3.boxplot(method_data_times, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| # plt.xticks(rotation=90) | |
| ax1.grid(True) | |
| ax1.set_xticks(np.arange(min_gaussian_noise*(nb_methods+1) + nb_methods/2,max_gausssian_noise*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax1.set_xticklabels(np.arange(min_gaussian_noise,max_gausssian_noise,1), fontdict=None, minor=False) | |
| ax1.legend(boxplots_rot, labels, loc='best') | |
| if args.ymax_rot != -1: | |
| ax1.set_ylim(ymax=args.ymax_rot) | |
| ax1.set_xlabel('Gaussian Image Noise (pixels)') | |
| ax1.set_ylabel('Rotation Error (degrees)') | |
| ax1.set_title('Rotation Error vs Gaussian Image Noise') | |
| ax2.grid(True) | |
| # plt.xticks(rotation=90) | |
| ax2.set_xticks(np.arange(min_gaussian_noise*(nb_methods+1) + nb_methods/2,max_gausssian_noise*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax2.set_xticklabels(np.arange(min_gaussian_noise,max_gausssian_noise,1), fontdict=None, minor=False) | |
| ax2.legend(boxplots_trans, labels, loc='best') | |
| if args.ymax_trans != -1: | |
| ax2.set_ylim(ymax=args.ymax_trans) | |
| ax2.set_xlabel('Gaussian Image Noise (pixels)') | |
| ax2.set_ylabel('Translation Error (%)') | |
| ax2.set_title('Translation Error vs Gaussian Image Noise') | |
| ax3.grid(True) | |
| # plt.xticks(rotation=90) | |
| ax3.set_xticks(np.arange(min_gaussian_noise*(nb_methods+1) + nb_methods/2,max_gausssian_noise*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax3.set_xticklabels(np.arange(min_gaussian_noise,max_gausssian_noise,1), fontdict=None, minor=False) | |
| ax3.legend(boxplots_times, labels, loc='best') | |
| ax3.set_xlim(xmin=-1) | |
| if args.ymax_times != -1: | |
| ax3.set_ylim(ymax=args.ymax_times) | |
| ax3.set_xlabel('Gaussian Image Noise (pixels)') | |
| ax3.set_ylabel('Computation Time (ms)') | |
| ax3.set_title('Computation Times vs Gaussian Image Noise') | |
| plt.show() | |
| if __name__ == "__main__": | |
| main() |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #!/usr/bin/env python | |
| # -*- coding: utf-8 -*- | |
| from __future__ import division | |
| from __future__ import print_function | |
| import matplotlib.pyplot as plt | |
| import numpy as np | |
| import argparse | |
| def main(): | |
| parser = argparse.ArgumentParser(description='Plot benchmark solvePnP methods against the number of points.') | |
| parser.add_argument('--plot', type=str, help='Directory path to file errors') | |
| parser.add_argument('--ymax_rot', type=int, default=-1, help='ymax limit for rotation') | |
| parser.add_argument('--ymax_trans', type=int, default=-1, help='ymax limit for translation') | |
| parser.add_argument('--ymax_times', type=int, default=-1, help='ymax limit for computation times') | |
| parser.add_argument('--planar', type=bool, default=False, help='True if planar object') | |
| args = parser.parse_args() | |
| planar = args.planar | |
| font = {'family' : 'normal', | |
| 'weight' : 'normal', | |
| 'size' : 22} | |
| plt.rc('font', **font) | |
| methods = ['EPnP', 'DLS', 'Iterative'] | |
| labels = ['EPnP', 'DLS', 'Iterative'] | |
| # methods = ['EPnP', 'EPnP_VVS', 'EPnP_LM'] | |
| # labels = ['EPnP', 'EPnP + VVS', 'EPnP + LM'] | |
| # methods = ['EPnP_VVS', 'DLS_VVS', 'Iterative'] | |
| # labels = ['EPnP + VVS', 'DLS + VVS', 'Iterative'] | |
| # methods = ['EPnP', 'DLS', 'Iterative', 'UPnP'] | |
| # labels = ['EPnP', 'DLS', 'Iterative', 'UPnP'] | |
| if (planar): | |
| methods.append('IPPE') | |
| labels.append('IPPE') | |
| # methods.append('IPPE_VVS') | |
| # labels.append('IPPE + VVS') | |
| colors = ['b', 'g', 'r', 'c', 'm', 'y'] | |
| boxplots_rot = [] | |
| boxplots_trans = [] | |
| boxplots_times = [] | |
| nb_methods = len(methods) | |
| min_num_points = 6 | |
| max_num_points = 30 | |
| x_step = 1 | |
| _, ax1 = plt.subplots() | |
| _, ax2 = plt.subplots() | |
| _, ax3 = plt.subplots() | |
| for idx, method in enumerate(methods): | |
| method_data_rot = [] | |
| method_data_trans = [] | |
| method_data_times = [] | |
| positions = [] | |
| for i in range(min_num_points,max_num_points): | |
| # load data | |
| filename = (args.plot + '/' + method + '/pose_%04d.txt') % i | |
| data_pose = np.loadtxt(filename) | |
| positions.append(i*(nb_methods+1) + idx) | |
| method_data_rot.append(data_pose[:,0]) | |
| method_data_trans.append(data_pose[:,1]) | |
| method_data_times.append(data_pose[:,2]) | |
| boxplots_rot.append(ax1.boxplot(method_data_rot, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| boxplots_trans.append(ax2.boxplot(method_data_trans, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| boxplots_times.append(ax3.boxplot(method_data_times, positions=positions, | |
| boxprops=dict(color=colors[idx]), whiskerprops=dict(color=colors[idx]), | |
| capprops=dict(color=colors[idx]), medianprops=dict(color=colors[idx]), | |
| showfliers=False)["boxes"][0]) | |
| # plt.xticks(rotation=90) | |
| ax1.grid(True) | |
| ax1.set_xticks(np.arange(min_num_points*(nb_methods+1) + nb_methods/2,max_num_points*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax1.set_xticklabels(np.arange(min_num_points,max_num_points,1), fontdict=None, minor=False) | |
| ax1.legend(boxplots_rot, labels, loc='best') | |
| ax1.set_xlim(xmin=5*(nb_methods+1)) | |
| if args.ymax_rot != -1: | |
| ax1.set_ylim(ymax=args.ymax_rot) | |
| ax1.set_xlabel('Number of Points') | |
| ax1.set_ylabel('Rotation Error (degrees)') | |
| ax1.set_title(r'Rotation Error vs Number of Points with Gaussian Noise $ \sigma = 2 $') | |
| ax2.grid(True) | |
| # plt.xticks(rotation=90) | |
| ax2.set_xticks(np.arange(min_num_points*(nb_methods+1) + nb_methods/2,max_num_points*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax2.set_xticklabels(np.arange(min_num_points,max_num_points,1), fontdict=None, minor=False) | |
| ax2.legend(boxplots_trans, labels, loc='best') | |
| ax2.set_xlim(xmin=5*(nb_methods+1)) | |
| if args.ymax_trans != -1: | |
| ax2.set_ylim(ymax=args.ymax_trans) | |
| ax2.set_xlabel('Number of Points') | |
| ax2.set_ylabel('Translation Error (%)') | |
| ax2.set_title(r'Translation Error vs Number of Points with Gaussian Noise $ \sigma = 2 $') | |
| ax3.grid(True) | |
| # plt.xticks(rotation=90) | |
| ax3.set_xticks(np.arange(min_num_points*(nb_methods+1) + nb_methods/2,max_num_points*(nb_methods+1) + nb_methods/2,(nb_methods+1)), minor=False) | |
| ax3.set_xticklabels(np.arange(min_num_points,max_num_points,1), fontdict=None, minor=False) | |
| ax3.legend(boxplots_times, labels, loc='best') | |
| ax3.set_xlim(xmin=5*(nb_methods+1)) | |
| if args.ymax_times != -1: | |
| ax3.set_ylim(ymax=args.ymax_times) | |
| ax3.set_xlabel('Number of Points') | |
| ax3.set_ylabel('Computation Times (ms)') | |
| ax3.set_title(r'Computation Times vs Number of Points with Gaussian Noise $ \sigma = 2 $') | |
| plt.show() | |
| if __name__ == "__main__": | |
| main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment