include <opencv2 opencv.hpp="">

include <iostream>

include <map>

include <fstream>

#include <opencv2/opencv.hpp> 
#include <iostream>
#include <map>
#include <fstream>

using namespace cv;
using namespace std;
 
std;

double PI = acos(-1.0);
 
acos(-1.0);

int main(int argc, char** argv)
{
    RNG rng(0xFFFFFFFF);
    vector< Mat > point(8);
    vector< Point3d > p3d(point.size()); // Real point in world coordinate
    vector< Point2d > p2d(point.size()); // point in image coordinate p2d=A[R|t]p3d  http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
 
http://docs.opencv.org/doc/tutorials/calib3d/camera_calibration/camera_calibration.html

// Real point randomly place in cube center at (10.5,10.5,10.5) with width 1
    for (int i = 0; i < point.size(); i++)
    {
 
{


    Mat_<double> v(4, 1);
    v(0, 0) = 10 + rng.uniform((double)0, (double)1);
    v(1, 0) = 10 + rng.uniform((double)0, (double)1);
    v(2, 0) = 10 + rng.uniform((double)0, (double)1);
    v(3,0) = 1;
    point[i] = v;
    p3d[i].x = v(0, 0);
    p3d[i].y = v(1, 0);
    p3d[i].z = v(2, 0);
}
Mat_<double> mInt(3, 3);
// Intrinsic parameter of webcam c270
mInt(0, 0) = 4520; mInt(0, 1) = 0; mInt(0, 2) = 600;
mInt(1, 0) = 0; mInt(1, 1) = 5490; mInt(1, 2) = 440;
mInt(2, 0) = 0; mInt(2, 1) = 0; mInt(2, 2) = 1;
// Camera rotation  (1.2rd,0.9rd,0.7rd)
Mat_<double> angle(3, 1);
angle(0, 0) = 1.2; angle(1, 0) = 0.9; angle(2, 0) = 0.7;
Mat r;
Mat rt = Mat::eye(3, 4, CV_64F);
// Camera translation  (2.4,4.8,0)
rt.at<double>(0, 3) = 2.4;
rt.at<double>(1, 3) = 4.8;
rt.at<double>(2, 3) = 0;
// make matrix rotation
Rodrigues(angle, r);

for (int i = 0; i < 3; i++)
    for (int j = 0; j < 3; j++)
        rt.at<double>(i, j) = r.at<double>(i, j);

Mat art;
// Mat product intrinsic and extrinsic parameter
gemm(mInt,rt,1,Mat(),0,art);
cout << art<<endl;
vector< Mat > pImg(point.size());
for (int i = 0; i < point.size(); i++)
{
    gemm(art, point[i], 1, Mat(), 0, pImg[i]);
    // Homogenous coordinate
    pImg[i]=pImg[i] / pImg[i].at<double>(2, 0);
    p2d[i] = Point2d(pImg[i].at<double>(0, 0), pImg[i].at<double>(1, 0));
    cout << point[i] << "\t" << pImg[i]  << endl;
}
// p3d real point- p2d image point what are values of camera orientation and translation 
Mat rvec, tvec;
solvePnP(p3d, p2d, mInt, Mat(), rvec, tvec);
cout << "Rvec camera = " << rvec << endl;
cout << "Tvec camera= " << tvec << endl;
return 0;
 
}