--- /dev/null
+#include <iostream>
+#include <boost/lexical_cast.hpp>
+
+#include "cv.h"
+#include "highgui.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/nonfree/features2d.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include "Mesh.h"
+#include "Model.h"
+#include "PnPProblem.h"
+#include "RobustMatcher.h"
+#include "ModelRegistration.h"
+#include "Utils.h"
+
+#include "CsvWriter.h"
+
+
+ /*
+ * Set up the images paths
+ */
+
+ std::string img_verification_path = "../Data/resized_IMG_3872.JPG";
+ std::string ply_read_path = "../Data/box.ply";
+ std::string yml_read_path = "../Data/cookies_ORB.yml";
+
+ // Boolean the know if the registration it's done
+ bool end_registration = false;
+
+ // Setup the points to register in the image
+ // In the order of the *.ply file and starting at 1
+ int n = 7;
+ int pts[] = {1, 2, 3, 5, 6, 7, 8};
+
+ /*
+ * Set up the intrinsic camera parameters: CANON
+ */
+ double f = 43;
+ double sx = 22.3, sy = 14.9;
+ double width = 718, height = 480;
+ double params_CANON[] = { width*f/sx, // fx
+ height*f/sy, // fy
+ width/2, // cx
+ height/2}; // cy
+
+ /*
+ * Set up some basic colors
+ */
+ cv::Scalar red(0, 0, 255);
+ cv::Scalar green(0,255,0);
+ cv::Scalar blue(255,0,0);
+ cv::Scalar yellow(0,255,255);
+
+ /*
+ * CREATE MODEL REGISTRATION OBJECT
+ * CREATE OBJECT MESH
+ * CREATE OBJECT MODEL
+ * CREATE PNP OBJECT
+ */
+ Mesh mesh;
+ ModelRegistration registration;
+ PnPProblem pnp_verification_epnp(params_CANON);
+ PnPProblem pnp_verification_iter(params_CANON);
+ PnPProblem pnp_verification_p3p(params_CANON);
+ PnPProblem pnp_verification_dls(params_CANON);
+ PnPProblem pnp_verification_gt(params_CANON); // groud truth
+
+
+// Mouse events for model registration
+static void onMouseModelVerification( int event, int x, int y, int, void* )
+{
+ if ( event == cv::EVENT_LBUTTONUP )
+ {
+ int n_regist = registration.getNumRegist();
+ int n_vertex = pts[n_regist];
+
+ cv::Point2f point_2d = cv::Point2f(x,y);
+ cv::Point3f point_3d = mesh.getVertex(n_vertex-1);
+
+ bool is_registrable = registration.is_registrable();
+ if (is_registrable)
+ {
+ registration.registerPoint(point_2d, point_3d);
+ if( registration.getNumRegist() == registration.getNumMax() ) end_registration = true;
+ }
+ }
+}
+
+
+/*
+ * MAIN PROGRAM
+ *
+ */
+
+int main(int, char**)
+{
+
+ std::cout << "!!!Hello Verification!!!" << std::endl; // prints !!!Hello World!!!
+
+ // load a mesh given the *.ply file path
+ mesh.load(ply_read_path);
+
+ // load the 3D textured object model
+ Model model;
+ model.load(yml_read_path);
+
+ // set parameters
+ int numKeyPoints = 10000;
+
+ //Instantiate robust matcher: detector, extractor, matcher
+ RobustMatcher rmatcher;
+ cv::FeatureDetector * detector = new cv::OrbFeatureDetector(numKeyPoints);
+ rmatcher.setFeatureDetector(detector);
+ rmatcher.setRatio(0.80);
+
+ // RANSAC parameters
+ int iterationsCount = 500;
+ float reprojectionError = 2.0;
+ float confidence = 0.99;
+
+
+ /*
+ * GROUND TRUTH SECOND IMAGE
+ *
+ */
+
+ cv::Mat img_in, img_vis;
+
+ // Setup for new registration
+ registration.setNumMax(n);
+
+ // Create & Open Window
+ cv::namedWindow("MODEL GROUND TRUTH", CV_WINDOW_KEEPRATIO);
+
+ // Set up the mouse events
+ cv::setMouseCallback("MODEL GROUND TRUTH", onMouseModelVerification, 0 );
+
+ // Open the image to register
+ img_in = cv::imread(img_verification_path, cv::IMREAD_COLOR);
+
+ if (!img_in.data)
+ {
+ std::cout << "Could not open or find the image" << std::endl;
+ return -1;
+ }
+
+ std::cout << "Click the box corners ..." << std::endl;
+ std::cout << "Waiting ..." << std::endl;
+
+ // Loop until all the points are registered
+ while ( cv::waitKey(30) < 0 )
+ {
+ // Refresh debug image
+ img_vis = img_in.clone();
+
+ // Current registered points
+ std::vector<cv::Point2f> list_points2d = registration.get_points2d();
+ std::vector<cv::Point3f> list_points3d = registration.get_points3d();
+
+ // Draw current registered points
+ drawPoints(img_vis, list_points2d, list_points3d, red);
+
+ // If the registration is not finished, draw which 3D point we have to register.
+ // If the registration is finished, breaks the loop.
+ if (!end_registration)
+ {
+ // Draw debug text
+ int n_regist = registration.getNumRegist();
+ int n_vertex = pts[n_regist];
+ cv::Point3f current_poin3d = mesh.getVertex(n_vertex-1);
+
+ drawQuestion(img_vis, current_poin3d, green);
+ drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), red);
+ }
+ else
+ {
+ // Draw debug text
+ drawText(img_vis, "GROUND TRUTH", green);
+ drawCounter(img_vis, registration.getNumRegist(), registration.getNumMax(), green);
+ break;
+ }
+
+ // Show the image
+ cv::imshow("MODEL GROUND TRUTH", img_vis);
+ }
+
+ // The list of registered points
+ std::vector<cv::Point2f> list_points2d = registration.get_points2d();
+ std::vector<cv::Point3f> list_points3d = registration.get_points3d();
+
+ // Estimate pose given the registered points
+ bool is_correspondence = pnp_verification_gt.estimatePose(list_points3d, list_points2d, cv::ITERATIVE);
+
+ // Compute and draw all mesh object 2D points
+ std::vector<cv::Point2f> pts_2d_ground_truth = pnp_verification_gt.verify_points(&mesh);
+ draw2DPoints(img_vis, pts_2d_ground_truth, green);
+
+ // Draw the ground truth mesh
+ drawObjectMesh(img_vis, &mesh, &pnp_verification_gt, blue);
+
+ // Show the image
+ cv::imshow("MODEL GROUND TRUTH", img_vis);
+
+ // Show image until ESC pressed
+ cv::waitKey(0);
+
+
+ /*
+ * EXTRACT CORRRESPONDENCES
+ *
+ */
+
+ // refresh visualisation image
+ img_vis = img_in.clone();
+
+ // Get the MODEL INFO
+ std::vector<cv::Point2f> list_points2d_model = model.get_points2d_in();
+ std::vector<cv::Point3f> list_points3d_model = model.get_points3d();
+ std::vector<cv::KeyPoint> keypoints_model = model.get_keypoints();
+ cv::Mat descriptors_model = model.get_descriptors();
+
+ // -- Step 1: Robust matching between model descriptors and scene descriptors
+
+ std::vector<cv::DMatch> good_matches; // to obtain the 3D points of the model
+ std::vector<cv::KeyPoint> keypoints_scene; // to obtain the 2D points of the scene
+
+ //rmatcher.fastRobustMatch(frame, good_matches, keypoints_scene, descriptors_model);
+ rmatcher.robustMatch(img_vis, good_matches, keypoints_scene, descriptors_model);
+
+ cv::Mat inliers_idx;
+ std::vector<cv::DMatch> matches_inliers;
+ std::vector<cv::Point2f> list_points2d_inliers;
+ std::vector<cv::Point3f> list_points3d_inliers;
+
+ // -- Step 2: Find out the 2D/3D correspondences
+
+ std::vector<cv::Point3f> list_points3d_model_match; // container for the model 3D coordinates found in the scene
+ std::vector<cv::Point2f> list_points2d_scene_match; // container for the model 2D coordinates found in the scene
+
+ for(unsigned int match_index = 0; match_index < good_matches.size(); ++match_index)
+ {
+ cv::Point3f point3d_model = list_points3d_model[ good_matches[match_index].trainIdx ]; // 3D point from model
+ cv::Point2f point2d_scene = keypoints_scene[ good_matches[match_index].queryIdx ].pt; // 2D point from the scene
+ list_points3d_model_match.push_back(point3d_model); // add 3D point
+ list_points2d_scene_match.push_back(point2d_scene); // add 2D point
+ }
+
+ // Draw outliers
+ //draw2DPoints(img_vis, list_points2d_scene_match, red);
+
+ /*
+ * COMPUTE PNP ERRORS:
+ * Calculation of the rotation and translation error
+ *
+ */
+
+ pnp_verification_epnp.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::EPNP);
+ pnp_verification_iter.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::ITERATIVE);
+ //pnp_verification_p3p.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::P3P);
+ pnp_verification_dls.estimatePose( list_points3d_model_match, list_points2d_scene_match, cv::DLS);
+
+ // Draw mesh
+ drawObjectMesh(img_vis, &mesh, &pnp_verification_dls, green);
+ drawObjectMesh(img_vis, &mesh, &pnp_verification_gt, yellow);
+
+ cv::imshow("MODEL GROUND TRUTH", img_vis);
+
+ cv::Mat t_true = pnp_verification_gt.get_t_matrix();
+ cv::Mat t_epnp = pnp_verification_epnp.get_t_matrix();
+ cv::Mat t_iter = pnp_verification_iter.get_t_matrix();
+ cv::Mat t_p3p = pnp_verification_p3p.get_t_matrix();
+ cv::Mat t_dls = pnp_verification_dls.get_t_matrix();
+
+ cv::Mat R_true = pnp_verification_gt.get_R_matrix();
+ cv::Mat R_epnp = pnp_verification_epnp.get_R_matrix();
+ cv::Mat R_iter = pnp_verification_iter.get_R_matrix();
+ cv::Mat R_p3p = pnp_verification_p3p.get_R_matrix();
+ cv::Mat R_dls = pnp_verification_dls.get_R_matrix();
+
+ double error_trans_epnp = get_translation_error(t_true, t_epnp);
+ double error_rot_epnp = get_rotation_error(R_true, R_epnp)*180/CV_PI;
+
+ double error_trans_iter = get_translation_error(t_true, t_iter);
+ double error_rot_iter = get_rotation_error(R_true, R_iter)*180/CV_PI;
+
+ double error_trans_p3p = get_translation_error(t_true, t_p3p);
+ double error_rot_p3p = get_rotation_error(R_true, R_p3p)*180/CV_PI;
+
+ double error_trans_dls = get_translation_error(t_true, t_dls);
+ double error_rot_dls = get_rotation_error(R_true, R_dls)*180/CV_PI;
+
+
+ std::cout << std::endl << "**** EPNP ERRORS **** " << std::endl;
+
+ std::cout << "Translation error: " << error_trans_epnp << " m." << std::endl;
+ std::cout << "Rotation error: " << error_rot_epnp << " deg." << std::endl;
+
+
+ std::cout << std::endl << "**** ITERATIVE ERRORS **** " << std::endl;
+
+ std::cout << "Translation error: " << error_trans_iter << " m." << std::endl;
+ std::cout << "Rotation error: " << error_rot_iter << " deg." << std::endl;
+
+
+ std::cout << std::endl << "**** P3P ERRORS **** " << std::endl;
+
+ std::cout << "Translation error: " << error_trans_p3p << " m." << std::endl;
+ std::cout << "Rotation error: " << error_rot_p3p << " deg." << std::endl;
+
+
+ std::cout << std::endl << "**** DLS ERRORS **** " << std::endl;
+
+ std::cout << "Translation error: " << error_trans_dls << " m." << std::endl;
+ std::cout << "Rotation error: " << error_rot_dls << " deg." << std::endl;
+
+
+ // Show image until ESC pressed
+ cv::waitKey(0);
+
+ // Close and Destroy Window
+ cv::destroyWindow("MODEL GROUND TRUTH");
+
+ std::cout << "GOODBYE" << std::endl;
+
+}
--- /dev/null
+#include <opencv2/core/core.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/contrib/contrib.hpp>
+
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+using namespace cv;
+
+void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
+ -1, 5), Point3f pmax = Point3f(1, 1, 10))
+ {
+ const Point3f delta = pmax - pmin;
+ for (size_t i = 0; i < points.size(); i++)
+ {
+ Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
+ float(rand()) / RAND_MAX);
+ p.x *= delta.x;
+ p.y *= delta.y;
+ p.z *= delta.z;
+ p = p + pmin;
+ points[i] = p;
+ }
+ }
+
+void generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
+{
+ const double fcMinVal = 1e-3;
+ const double fcMaxVal = 100;
+ cameraMatrix.create(3, 3, CV_64FC1);
+ cameraMatrix.setTo(Scalar(0));
+ cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
+ cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
+ cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
+ cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
+ cameraMatrix.at<double>(2,2) = 1;
+}
+
+void generateDistCoeffs(Mat& distCoeffs, RNG& rng)
+{
+ distCoeffs = Mat::zeros(4, 1, CV_64FC1);
+ for (int i = 0; i < 3; i++)
+ distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6);
+}
+
+void generatePose(Mat& rvec, Mat& tvec, RNG& rng)
+{
+ const double minVal = 1.0e-3;
+ const double maxVal = 1.0;
+ rvec.create(3, 1, CV_64FC1);
+ tvec.create(3, 1, CV_64FC1);
+ for (int i = 0; i < 3; i++)
+ {
+ rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
+ tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
+ }
+}
+
+void data2file(const string& path, const vector<vector<double> >& data)
+{
+ std::fstream fs;
+ fs.open(path.c_str(), std::fstream::in | std::fstream::out | std::fstream::app);
+
+ for (int method = 0; method < data.size(); ++method)
+ {
+ for (int i = 0; i < data[method].size(); ++i)
+ {
+ fs << data[method][i] << " ";
+ }
+ fs << endl;
+ }
+
+ fs.close();
+}
+
+
+int main(int argc, char *argv[])
+{
+
+ RNG rng;
+ // TickMeter tm;
+ vector<vector<double> > error_trans(4), error_rot(4), comp_time(4);
+
+ int maxpoints = 2000;
+ for (int npoints = 10; npoints < maxpoints+10; ++npoints)
+ {
+ // generate 3D point cloud
+ vector<Point3f> points;
+ points.resize(npoints);
+ generate3DPointCloud(points);
+
+ // generate cameramatrix
+ Mat rvec, tvec;
+ Mat trueRvec, trueTvec;
+ Mat intrinsics, distCoeffs;
+ generateCameraMatrix(intrinsics, rng);
+
+ // generate distorsion coefficients
+ generateDistCoeffs(distCoeffs, rng);
+
+ // generate groud truth pose
+ generatePose(trueRvec, trueTvec, rng);
+
+ for (int method = 0; method < 4; ++method)
+ {
+ std::vector<Point3f> opoints;
+ if (method == 2)
+ {
+ opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
+ }
+ else
+ opoints = points;
+
+ vector<Point2f> projectedPoints;
+ projectedPoints.resize(opoints.size());
+ projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
+
+ //tm.reset(); tm.start();
+
+ solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec, false, method);
+
+ // tm.stop();
+
+ // double compTime = tm.getTimeMilli();
+ double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
+
+ error_rot[method].push_back(rvecDiff);
+ error_trans[method].push_back(tvecDiff);
+ //comp_time[method].push_back(compTime);
+
+ }
+
+ //system("clear");
+ cout << "Completed " << npoints+1 << "/" << maxpoints << endl;
+
+ }
+
+ data2file("translation_error.txt", error_trans);
+ data2file("rotation_error.txt", error_rot);
+ data2file("computation_time.txt", comp_time);
+
+}
+
projects / platform / upstream / opencv.git / commitdiff