Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "opencv2/core/core.hpp"
- #include "opencv2/imgproc/imgproc.hpp"
- #include "opencv2/calib3d/calib3d.hpp"
- #include "opencv2/highgui/highgui.hpp"
- #include <iostream>
- using namespace cv;
- using namespace std;
- #define EPSILON 1E-5
- cv::Point2f center(0, 0);
- cv::Point2f computeIntersect(cv::Vec4i a,
- cv::Vec4i b)
- {
- int x1 = a[0], y1 = a[1], x2 = a[2], y2 = a[3], x3 = b[0], y3 = b[1], x4 = b[2], y4 = b[3];
- float denom;
- if (float d = ((float)(x1 - x2) * (y3 - y4)) - ((y1 - y2) * (x3 - x4)))
- {
- cv::Point2f pt;
- pt.x = ((x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4)) / d;
- pt.y = ((x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4)) / d;
- return pt;
- }
- else
- return cv::Point2f(-1, -1);
- }
- void sortCorners(std::vector<cv::Point2f>& corners,
- cv::Point2f center)
- {
- std::vector<cv::Point2f> top, bot;
- for (int i = 0; i < corners.size(); i++)
- {
- if (corners[i].y < center.y)
- top.push_back(corners[i]);
- else
- bot.push_back(corners[i]);
- }
- corners.clear();
- if (top.size() == 2 && bot.size() == 2) {
- cv::Point2f tl = top[0].x > top[1].x ? top[1] : top[0];
- cv::Point2f tr = top[0].x > top[1].x ? top[0] : top[1];
- cv::Point2f bl = bot[0].x > bot[1].x ? bot[1] : bot[0];
- cv::Point2f br = bot[0].x > bot[1].x ? bot[0] : bot[1];
- corners.push_back(tl);
- corners.push_back(tr);
- corners.push_back(br);
- corners.push_back(bl);
- }
- }
- struct Ray
- {
- cv::Vec3f origin;
- cv::Vec3f direction;
- };
- std::pair<bool, double> linePlaneIntersection(Vec3f& contact, Vec3f direction, Vec3f rayOrigin,
- Vec3f normal, Vec3f coord) {
- double denom = normalize(normal).dot(direction);
- double d = -(normalize(normal).dot(coord));
- if (std::abs(d) < std::numeric_limits<double>::epsilon())
- {
- cout << "point" << contact;
- }
- if (std::abs(denom) < std::numeric_limits<double>::epsilon())
- {
- // Parallel
- return std::pair<bool, double>(false, (double)0);
- }
- else
- {
- double nom = normalize(normal).dot(normalize(rayOrigin)) + d;
- double t = -(nom / denom);
- return std::pair<bool, double>(t >= 0, (double)t);
- }
- }
- cv::Point2f snapPoint;
- bool isClicked = false;
- void onmouse(int event, int x, int y, int flags, void* param)
- {
- if (event == CV_EVENT_LBUTTONDOWN)
- {
- snapPoint.x = x;
- snapPoint.y = y;
- //Mat &img = *((Mat*)(param)); // 1st cast it back, then deref
- //circle(img, Point(x, y), 50, Scalar(0, 255, 0), 1);
- isClicked = true;
- }
- }
- std::vector<cv::Point3d> Generate3DPoints()
- {
- std::vector<cv::Point3d> points;
- double x, y, z;
- x = .5; y = .5; z = -.5;
- points.push_back(cv::Point3d(x, y, z));
- x = .5; y = .5; z = .5;
- points.push_back(cv::Point3d(x, y, z));
- x = -.5; y = .5; z = .5;
- points.push_back(cv::Point3d(x, y, z));
- x = -.5; y = .5; z = -.5;
- points.push_back(cv::Point3d(x, y, z));
- x = .5; y = -.5; z = -.5;
- points.push_back(cv::Point3d(x, y, z));
- x = -.5; y = -.5; z = -.5;
- points.push_back(cv::Point3d(x, y, z));
- x = -.5; y = -.5; z = .5;
- points.push_back(cv::Point3d(x, y, z));
- /*
- for(unsigned int i = 0; i < points.size(); ++i)
- {
- std::cout << points[i] << std::endl;
- }
- */
- return points;
- }
- std::vector<cv::Point2d> Generate2DPoints()
- {
- std::vector<cv::Point2d> points;
- float x, y;
- x = 282; y = 274;
- points.push_back(cv::Point2d(x, y));
- x = 397; y = 227;
- points.push_back(cv::Point2d(x, y));
- x = 577; y = 271;
- points.push_back(cv::Point2d(x, y));
- x = 462; y = 318;
- points.push_back(cv::Point2d(x, y));
- x = 270; y = 479;
- points.push_back(cv::Point2d(x, y));
- x = 450; y = 523;
- points.push_back(cv::Point2d(x, y));
- x = 566; y = 475;
- points.push_back(cv::Point2d(x, y));
- for (unsigned int i = 0; i < points.size(); ++i)
- {
- std::cout << points[i] << std::endl;
- }
- return points;
- }
- int main(int argc, char** argv)
- {
- Mat src, src_copy, edges, dst;
- src = imread("freezeFrame__1508152029892.png", 0);
- namedWindow("My window", 1);
- imshow("My window", src);
- while (1 && isClicked == false)
- {
- setMouseCallback("My window", onmouse, &src); // pass address of img here
- cv::waitKey(10);
- while (isClicked != false)
- {
- isClicked = false;
- std::vector< cv::Point2f > corners;
- // maxCorners – The maximum number of corners to return. If there are more corners
- // than that will be found, the strongest of them will be returned
- int maxCorners = 10;
- // qualityLevel – Characterizes the minimal accepted quality of image corners;
- // the value of the parameter is multiplied by the by the best corner quality
- // measure (which is the min eigenvalue, see cornerMinEigenVal() ,
- // or the Harris function response, see cornerHarris() ).
- // The corners, which quality measure is less than the product, will be rejected.
- // For example, if the best corner has the quality measure = 1500,
- // and the qualityLevel=0.01 , then all the corners which quality measure is
- // less than 15 will be rejected.
- double qualityLevel = 0.01;
- // minDistance – The minimum possible Euclidean distance between the returned corners
- double minDistance = 20.;
- // mask – The optional region of interest. If the image is not empty (then it
- // needs to have the type CV_8UC1 and the same size as image ), it will specify
- // the region in which the corners are detected
- cv::Mat mask;
- // blockSize – Size of the averaging block for computing derivative covariation
- // matrix over each pixel neighborhood, see cornerEigenValsAndVecs()
- int blockSize = 3;
- // useHarrisDetector – Indicates, whether to use operator or cornerMinEigenVal()
- bool useHarrisDetector = false;
- // k – Free parameter of Harris detector
- double k = 0.04;
- cv::goodFeaturesToTrack(src, corners, maxCorners, qualityLevel, minDistance, mask, blockSize, useHarrisDetector, k);
- for (size_t i = 0; i < corners.size(); i++)
- {
- cv::circle(src, corners[i], 10, cv::Scalar(0, 255, 255), -1);
- }
- for (size_t i = 0; i < corners.size(); i++)
- {
- cv::Point2f pt1 = corners[i];
- for (int j = 0; j < corners.size(); j++)
- {
- cv::Point2f pt2 = corners[j];
- line(src, pt1, pt2, cv::Scalar(0, 255, 0));
- }
- }
- corners.push_back(snapPoint);
- Mat cameraIntrinsics(3, 3, CV_64FC1);
- cameraIntrinsics.at<double>(0, 0) = 1600;
- cameraIntrinsics.at<double>(0, 1) = 0;
- cameraIntrinsics.at<double>(0, 2) = src.cols/2;
- cameraIntrinsics.at<double>(1, 0) = 0;
- cameraIntrinsics.at<double>(1, 1) = 1600;
- cameraIntrinsics.at<double>(1, 2) = src.rows/2;
- cameraIntrinsics.at<double>(2, 0) = 0;
- cameraIntrinsics.at<double>(2, 1) = 0;
- cameraIntrinsics.at<double>(2, 2) = 1;
- Mat invCameraIntrinsics = cameraIntrinsics.inv();
- cout << "inv" << invCameraIntrinsics;
- std::vector<cv::Vec3d> pointsTransformed3D;
- std::vector<cv::Vec3d> points3D;
- for (int i = 0; i < corners.size(); i++)
- {
- cv::Vec3d pt;
- pt[2] = 1.0f;
- pt[0] = (corners[i].y );
- pt[1] = (corners[i].x );
- points3D.push_back(pt);
- }
- cv::transform(points3D, pointsTransformed3D, invCameraIntrinsics);
- /*
- std::vector<Ray> rays;
- for (int i = 0; i < pointsTransformed3D.size(); i++)
- {
- Ray ray;
- ray.origin = Vec3f(0, 0, 0);
- ray.direction = Vec3f(pointsTransformed3D[i][0], pointsTransformed3D[i][1], pointsTransformed3D[i][2]);
- rays.push_back(ray);
- }
- std::vector<cv::Vec3f> contacts;
- for (int i = 0; i < pointsTransformed3D.size(); i++)
- {
- Vec3f pt(pointsTransformed3D[i][0], pointsTransformed3D[i][1], pointsTransformed3D[i][2]);
- cv::Vec3f contact;
- std::pair<bool, double> test = linePlaneIntersection(contact, rays[i].direction, rays[i].origin, Vec3f(0, 1, 0), pt);
- if (test.first == true)
- {
- cv::Vec3f contact(rays[i].origin + (rays[i].direction) * test.second);
- contacts.push_back(contact);
- }
- }
- */
- for (int i = 0; i < pointsTransformed3D.size(); i++)
- {
- double d = -(normalize(Vec3d(0,1,0)).dot(pointsTransformed3D[i]));
- if (std::abs(d) < std::numeric_limits<double>::epsilon())
- {
- cout << "point" << pointsTransformed3D[i];
- }
- }
- Mat rotationMatrix(3, 3, CV_64FC1);
- rotationMatrix.at<double>(0, 0) = 0.9115078799790896;
- rotationMatrix.at<double>(0, 1) = -0.1883612409043686;
- rotationMatrix.at<double>(0, 2) = -0.3656137684237178;
- rotationMatrix.at<double>(1, 0) = -0.3046835686704949;
- rotationMatrix.at<double>(1, 1) = 0.2878667580409447;
- rotationMatrix.at<double>(1, 2) = -0.9079100465339108;
- rotationMatrix.at<double>(2, 0) = 0.2762631132059388;
- rotationMatrix.at<double>(2, 1) = 0.9389636694462479;
- rotationMatrix.at<double>(2, 2) = 0.2050022432604093;
- cv::Mat rVec(3, 1, CV_64FC1); // Rotation vector
- Rodrigues(rotationMatrix, rVec);
- cout << "rVec" << rVec << endl;
- double norm = cv::norm(rVec);
- double theta = (double)(sqrt(rVec.at<double>(0)*rVec.at<double>(0) + rVec.at<double>(1)*rVec.at<double>(1) + rVec.at<double>(2)*rVec.at<double>(2)) * 180 / 3.14567898726);
- cv::Mat tVec(3, 1, CV_64FC1); // Translation vector
- tVec.at<double>(1) = 21.408294677734375;
- tVec.at<double>(2) = 531.1319580078125; //
- tVec.at<double>(0) = 705.74224853515625;
- cv::Mat distCoeffs(5, 1, CV_64FC1); // Distortion vector
- distCoeffs.at<double>(0) = 0;
- distCoeffs.at<double>(1) = 0;
- distCoeffs.at<double>(2) = 0;
- distCoeffs.at<double>(3) = 0;
- distCoeffs.at<double>(4) = 0;
- std::vector<cv::Point2d> projectedPoints;
- std::vector < cv::Point3d> ContactPoints;
- for (int i = 0; i < pointsTransformed3D.size(); i++)
- {
- cv::Point3d pt;
- pt.x = pointsTransformed3D[i][0];
- pt.y = pointsTransformed3D[i][1];
- pt.z = pointsTransformed3D[i][2];
- ContactPoints.push_back(pt);
- }
- //ContactPoints = Generate3DPoints();
- cv::projectPoints(ContactPoints, rVec, tVec, cameraIntrinsics, distCoeffs, projectedPoints);
- for (size_t i = 0; i < projectedPoints.size(); i++)
- {
- cv::Point2d pt;
- pt.x = projectedPoints[i].y;
- pt.y = projectedPoints[i].x;
- cv::circle(src, pt, 10, cv::Scalar(255, 0, 255), -1);
- }
- imshow("My window", src);
- }
- }
- cv:waitKey(0);
- return 0;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement