Image center in the calibration matrix

Question

I have a camera and a 3D object, I have calculated the camera matrix as given in this. The image captured using the camera is having size 1600x1200. But in the camera matrix I am not getting the value of image centers properly. Instead of 800, 600 I am getting some other values. What will be the possible reasons for this.

  #include <iostream>
    #include "opencv2/imgproc/imgproc.hpp"
    #include "opencv2/highgui/highgui.hpp"
    #include <math.h> 
    using namespace std;
    using namespace cv;
    int main()
    {
      int numberofpoints=30;
      float p2d[30][2] =
  {{72, 169}, {72, 184}, {71, 264}, {96, 168}, {94, 261}, {94, 276}, {257, 133},
    {254, 322}, {254, 337}, {278, 132}, {278, 146}, {275, 321}, {439, 228},
    {439, 243}, {437, 328}, {435, 431}, {461, 226}, {459, 326}, {459, 342},
    {457, 427}, {457, 444}, {612, 196}, {609, 291}, {605, 392}, {605, 406},
    {634, 191}, {633, 206}, {630, 288}, {630, 303}, {627, 390}};



      float p3d[30][3] = 
{{0, 0, 98},  {0, 0, 94}, {0, 0, 75}, {4, 4, 98}, {4, 4, 75}, {4, 4, 71},
{0, 96, 75}, {0, 96, 25}, {0, 96, 21}, {4, 100, 75},
{4, 100, 71}, {4, 100, 25}, {96, 0, 98}, {96, 0, 94},
{96, 0, 75}, {96, 0, 50}, {100, 4, 98}, {100, 4, 75},
{100, 4, 71}, {100, 4, 50}, {100, 4, 46}, {96, 96, 75},
{96, 96, 50}, {96, 96, 25}, {96, 96, 21}, {100, 100, 75},
{100, 100, 71}, {100, 100, 50}, {100, 100, 46}, {100, 100, 25}};


      Mat Matrix2D= Mat(30, 2, CV_64FC1, &p2d);
      Mat Matrix3D= Mat(30, 3, CV_64FC1, &p3d);
      Mat X_3D=Matrix3D.col(0);
      Mat Y_3D=Matrix3D.col(1);
      Mat Z_3D=Matrix3D.col(2);
      Mat X_2D=Matrix2D.col(0);
      Mat Y_2D=Matrix2D.col(1);
      Mat z = Mat::zeros(numberofpoints,4, CV_64FC1);
      Mat o = Mat::ones(numberofpoints,1, CV_64FC1);
      Mat temp,temp1,C,D,AoddRows;
      hconcat( X_3D, Y_3D,temp); 
      hconcat(Z_3D ,o,temp1);
      hconcat(z, -X_2D.mul(X_3D),C);
      hconcat(-X_2D.mul(Y_3D),-X_2D.mul(Z_3D),D);
      hconcat(temp,temp1,AoddRows);
      hconcat(AoddRows,C,AoddRows);
      hconcat(AoddRows,D,AoddRows);
      hconcat(AoddRows,-X_2D,AoddRows);
      Mat A1,B1,C1,AevenRows;
      hconcat(z,Matrix3D,A1); 
      hconcat(o,-Y_2D.mul(X_3D) ,B1);
      hconcat(-Y_2D.mul(Y_3D),-Y_2D.mul(Z_3D),C1);
      hconcat(A1,B1,AevenRows);
      hconcat(AevenRows,C1,AevenRows);
      hconcat(AevenRows,-Y_2D,AevenRows);
      Mat A;                                      
      vconcat(AoddRows,AevenRows,A);              
      Mat  w, u, EigenVectors;
      SVD::compute(A, w, u, EigenVectors);
      Mat EigenVector_SmallestEigenvalue=EigenVectors.row(EigenVectors.rows-1);
      Mat P=Mat::zeros(3,4, CV_64FC1);
      int k=0;
      for(int i=0;i<P.rows;i++)
      {
        for(int j=0;j<P.cols;j++)
        {
          P.at<double>(i, j) = EigenVector_SmallestEigenvalue.at<double>(0,k);
          k++;
        }
      }
      double abs_lambda = sqrt(P.at<double>(2,0) * P.at<double>(2,0)  + P.at<double>(2,1) * P.at<double>(2,1) + P.at<double>(2,2) * P.at<double>(2,2));
      P = P / abs_lambda;
      Mat ProjectionMatrix = P;    
      Mat T= Mat::zeros(3,1, CV_64FC1);
      Mat R = Mat::zeros(3,3, CV_64FC1);
      Mat B = Mat::zeros(3,3, CV_64FC1);
      Mat b = Mat::zeros(3,1,CV_64FC1);
      P.row(0).colRange(0,3).copyTo(B.row(0));
      P.row(1).colRange(0,3).copyTo(B.row(1));
      P.row(2).colRange(0,3).copyTo(B.row(2));
      P.col(3).copyTo(b.col(0));
      Mat K=B*B.t();
      double Center_x = K.at<double>(0,2);
      double Center_y = K.at<double>(1,2);
      double beta = sqrt(K.at<double>(1,1)-Center_y*Center_y);
      double gemma = (K.at<double>(0,1)-Center_x*Center_y)/beta;
      double alpha = sqrt(K.at<double>(0,0)-Center_x*Center_x-gemma*gemma);

      Mat CameraMatrix=Mat::zeros(3,3, CV_64FC1);
      CameraMatrix.at<double>(0,0)=alpha;
      CameraMatrix.at<double>(0,1)=gemma;
      CameraMatrix.at<double>(0,2)=Center_x;
      CameraMatrix.at<double>(1,1)=beta;
      CameraMatrix.at<double>(1,2)=Center_y;
      CameraMatrix.at<double>(2,2)=1;
      R = CameraMatrix.inv()*B;
      T = CameraMatrix.inv()*b;
      Mat newMatrix2D=Mat::zeros(numberofpoints,2, CV_64FC1);
      for(int i=0;i<numberofpoints;i++)
      {
        double num_u =  P.at<double>(0,0) * Matrix3D.at<double>(i,0) 
          + P.at<double>(0,1) * Matrix3D.at<double>(i,1) 
          + P.at<double>(0,2) * Matrix3D.at<double>(i,2) 
          + P.at<double>(0,3);

        double num_v =  P.at<double>(1,0) * Matrix3D.at<double>(i,0) 
          + P.at<double>(1,1) * Matrix3D.at<double>(i,1) 
          + P.at<double>(1,2) * Matrix3D.at<double>(i,2) 
          + P.at<double>(1,3);

        double den =    P.at<double>(2,0) * Matrix3D.at<double>(i,0) 
          + P.at<double>(2,1) * Matrix3D.at<double>(i,1) 
          + P.at<double>(2,2) * Matrix3D.at<double>(i,2) 
          + P.at<double>(2,3);

        newMatrix2D.at<double>(i,0)=num_u/den;
        newMatrix2D.at<double>(i,1)=num_v/den;
      } 

      Mat reprojMatrix=newMatrix2D;
      Mat errorDiff=reprojMatrix-Matrix2D;
      int size=errorDiff.rows;
      double sum=0;
      for(int i=0;i<errorDiff.rows;i++)
      {
        sum=sum + sqrt(errorDiff.at<double>(i,0) * errorDiff.at<double>(i,0)
                       + errorDiff.at<double>(i,1) * errorDiff.at<double>(i,1));
      }
      sum=sum/size;
      cout<<"Average Error="<<sum<<endl;
      cout<<"Translation Matrix="<<T<<endl<<endl;
      cout<<"Rotational Matrix="<<R<<endl<<endl;
      cout<<"Camera Matrix="<<CameraMatrix<<endl;
      return 0;
    }

---

SAMPLE 1:

  const int numberofpoints = 30;
  float p2d[numberofpoints][2] =
    {{72, 169}, {72,  184}, {71, 264},  {96, 168},  {94, 261},  {94, 276}, {257, 133},
    {254, 322}, {254, 337}, {278, 132}, {278, 146}, {275, 321}, {439, 228},
    {439, 243}, {437, 328}, {435, 431}, {461, 226}, {459, 326}, {459, 342},
    {457, 427}, {457, 444}, {612, 196}, {609, 291}, {605, 392}, {605, 406},
    {634, 191}, {633, 206}, {630, 288}, {630, 303}, {627, 390}};

  float p3d[numberofpoints][3] =
    {{0, 0, 98},  {0, 0, 94},   {0, 0, 75},   {4, 4, 98}, {4, 4, 75}, {4, 4, 71},
    {0, 96, 75},  {0, 96, 25},  {0, 96, 21},  {4, 100, 75},
    {4, 100, 71}, {4, 100, 25}, {96, 0, 98},  {96, 0, 94},
    {96, 0, 75},  {96, 0, 50},  {100, 4, 98}, {100, 4, 75},
    {100, 4, 71}, {100, 4, 50}, {100, 4, 46}, {96, 96, 75},
    {96, 96, 50}, {96, 96, 25}, {96, 96, 21}, {100, 100, 75},
    {100, 100, 71}, {100, 100, 50}, {100, 100, 46}, {100, 100, 25}};

---

SAMPLE 2:

 const int numberofpoints = 24;
  float p2d[24][2] =
  {{41, 291}, {41, 494},  {41, 509}, {64, 285}, {64, 303},
     {64, 491}, {195, 146},  {216, 144}, {216, 160}, {431, 337},
     {431, 441}, {431, 543}, {431, 558}, {452, 336}, {452, 349},
     {452, 438}, {452, 457}, {452, 539}, {568, 195}, {566, 291},
     {588, 190}, {587, 209}, {586, 289}, {586, 302}};
  float p3d[24][3] =
  {{0, 0, 75}, {0, 0, 25}, {0, 0, 21}, {4, 4, 75}, {4, 4, 71}, {4, 4, 25},
    {0, 96, 75 }, {4, 100, 75}, {4, 100, 71}, {96, 0, 75}, {96, 0, 50}, {96, 0, 25},
  {96, 0, 21}, {100, 4, 75}, {100, 4, 71}, {100, 4, 50}, {100, 4, 46}, {100, 4, 25}, {96,  96, 75 },  {96,  96, 50 }, {100, 100, 75}, {100, 100, 71}, {100, 100, 50}, {100, 100, 46}};

---

SAMPLE 3:

 const int numberofpoints = 33;
  float p2d[numberofpoints][2] =
  {{45, 310},   {43, 412}, {41, 513}, {41, 530}, {68, 305},
    {68, 321}, {66, 405}, {66, 423}, {64, 509}, {201, 70}, {201, 84},
    {200, 155}, {198, 257}, {218, 259}, {218, 271}, {441, 364},
    {439, 464}, {437, 569}, {437, 586}, {462, 361}, {462, 376},
    {460, 462}, {460, 479}, {459, 565}, {583, 117}, {583, 131},
    {580, 215}, {576, 313}, {603, 113}, {600, 214}, {599, 229},
    {596, 311}, {596, 323}};

    float p3d[numberofpoints][3] =
   {{0, 0, 75},   {0, 0, 50}, {0, 0, 25}, {0, 0, 21}, {4, 4, 75}, {4, 4, 71},
    {4, 4, 50}, {4, 4, 46}, {4, 4, 25}, {0, 96, 98}, {0, 96, 94}, {0, 96, 75},
    {0, 96, 50}, {4, 100,75}, {4, 100,71}, {96, 0, 75}, {96, 0, 50}, {96, 0, 25},
    {96, 0, 21}, {100, 4,75}, {100, 4,71}, {100, 4,50}, {100, 4,46}, {100, 4,25},{96, 96,98}, {96, 96,94}, {96, 96,75}, {96, 96,50}, {100, 100, 98}, {100, 100, 75},{100, 100, 71}, {100, 100, 50}, {100, 100, 46}};

---

Answer 1

EDIT:

I've taken one of the 3D coordinate point sets you've provided, and then tried to project them on a virtual camera (to prevent any errors with 3d-2d point correspondences). The alpha, beta and gamma are estimated correctly in this case, but u and v (that you are looking for) were for some reason negative in all of my experiments. Perhaps there's a bug in my code, but maybe this is due to inaccuracy of OpenCV solveZ implementation (I've had problems with OpenCV's eigenvalue/vector computation before and try to use OpenBLAS when high accuracy is needed).

Another possible reason - as authors state, this approach performs algebraic minimization, which may produce physically meaningless results. Perhaps you should check remaining part of paper or try a different approach.

Here's the updated code:

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

int main()
{
  // generate random 3d points
  const int numberofpoints = 33;

  float p3d_[numberofpoints][3] =
      {{0, 0, 75},   {0, 0, 50}, {0, 0, 25}, {0, 0, 21}, {4, 4, 75}, {4, 4, 71},
       {4, 4, 50}, {4, 4, 46}, {4, 4, 25}, {0, 96, 98}, {0, 96, 94}, {0, 96, 75},
       {0, 96, 50}, {4, 100,75}, {4, 100,71}, {96, 0, 75}, {96, 0, 50}, {96, 0, 25},
       {96, 0, 21}, {100, 4,75}, {100, 4,71}, {100, 4,50}, {100, 4,46}, {100, 4,25},{96, 96,98}, {96, 96,94}, {96, 96,75}, {96, 96,50}, {100, 100, 98}, {100, 100, 75},{100, 100, 71}, {100, 100, 50}, {100, 100, 46}};

  std::vector<cv::Point3d> p3d;

  for (int i = 0; i < numberofpoints; i++) {
    cv::Point3d X(p3d_[i][0], p3d_[i][1], p3d_[i][2]);
    p3d.push_back(X);
  }

  // set up virtual camera
  const cv::Size imgSize(1600, 1200);
  const double fx = 100;
  const double fy = 120;

  cv::Mat1d K = (cv::Mat1d(3, 3) << fx, 0, imgSize.width/2,
      0, fy, imgSize.height/2,
      0, 0, 1);

  std::cout << "K = " << std::endl;
  std::cout << K << std::endl << std::endl;

  cv::Mat1d t = (cv::Mat1d(3, 1) << 10, 20, 20);
  cv::Mat1d rvecDeg = (cv::Mat1d(3, 1) << 10, 20, 30);

  // project point on camera
  std::vector<cv::Point2d> p2d;

  cv::projectPoints(p3d,
                    rvecDeg*CV_PI/180,
                    t,
                    K,
                    cv::Mat(),
                    p2d);

  // estimate projection
  cv::Mat1d G = cv::Mat1d::zeros(numberofpoints*2, 12);

  for (int i = 0; i < numberofpoints; i++) {
    const double X = p3d[i].x;
    const double Y = p3d[i].y;
    const double Z = p3d[i].z;

    G(i*2 + 0, 0) = X;
    G(i*2 + 0, 1) = Y;
    G(i*2 + 0, 2) = Z;
    G(i*2 + 0, 3) = 1;

    G(i*2 + 1, 4) = X;
    G(i*2 + 1, 5) = Y;
    G(i*2 + 1, 6) = Z;
    G(i*2 + 1, 7) = 1;

    const double u = p2d[i].x;
    const double v = p2d[i].y;

    G(i*2 + 0, 8) = u*X;
    G(i*2 + 0, 9) = u*Y;
    G(i*2 + 0, 10) = u*Z;
    G(i*2 + 0, 11) = u;

    G(i*2 + 1, 8) = v*X;
    G(i*2 + 1, 9) = v*Y;
    G(i*2 + 1, 10) = v*Z;
    G(i*2 + 1, 11) = v;
  }

  std::cout << "G = " << std::endl;
  std::cout << G << std::endl << std::endl;

  cv::Mat1d p;
  cv::SVD::solveZ(G, p);

  cv::Mat1d P = p.reshape(0, 3).clone();
  P /= P(2, 3);

  std::cout << "p = " << std::endl;
  std::cout << p << std::endl << std::endl;

  std::cout << "P = " << std::endl;
  std::cout << P << std::endl << std::endl;

  cv::Mat1d B(3, 3);
  cv::Mat1d b(3, 1);

  P.colRange(0, 3).copyTo(B);
  P.col(3).copyTo(b);

  std::cout << "B = " << std::endl;
  std::cout << B << std::endl << std::endl;

  std::cout << "b = " << std::endl;
  std::cout << b << std::endl << std::endl;

  cv::Mat1d K_ = B*B.t();
  K_ /= K_(2, 2);

  std::cout << "K_ = " << std::endl;
  std::cout << K_ << std::endl << std::endl;

  const double u = K_(0, 2);
  const double v = K_(1, 2);
  const double ku = K_(0, 0);
  const double kv = K_(1, 1);
  const double kc = K_(0, 1);

  const double beta = sqrt(kv - v*v);
  const double gamma = (kc - u*v)/beta;
  const double alpha = sqrt(ku - u*u - gamma*gamma);

  cv::Mat1d A = (cv::Mat1d(3, 3) << alpha, gamma, u,
                                        0,  beta, v,
                                        0,     0, 1);

  std::cout << "A = " << std::endl;
  std::cout << A << std::endl << std::endl;

  return 0;
}

And output:

K =
[100, 0, 800;
0, 120, 600;
0, 0, 1]

G =
[0, 0, 75, 1, 0, 0, 0, 0, 0, 0, 63156.71331987197, 842.0895109316264;
0, 0, 0, 0, 0, 0, 75, 1, 0, 0, 46451.70410142483, 619.3560546856644;
0, 0, 50, 1, 0, 0, 0, 0, 0, 0, 42144.23132613153, 842.8846265226306;
0, 0, 0, 0, 0, 0, 50, 1, 0, 0, 31473.60945095979, 629.4721890191959;
0, 0, 25, 1, 0, 0, 0, 0, 0, 0, 21113.32803626328, 844.5331214505311;
0, 0, 0, 0, 0, 0, 25, 1, 0, 0, 16261.14346086196, 650.4457384344785;
0, 0, 21, 1, 0, 0, 0, 0, 0, 0, 17744.50634900177, 844.9764928096083;
0, 0, 0, 0, 0, 0, 21, 1, 0, 0, 13777.82037617329, 656.0866845796805;
4, 4, 75, 1, 0, 0, 0, 0, 3374.871998456306, 3374.871998456306, 63278.84997105574, 843.7179996140766;
0, 0, 0, 0, 4, 4, 75, 1, 2506.953106676701, 2506.953106676701, 47005.37075018814, 626.7382766691752;
4, 4, 71, 1, 0, 0, 0, 0, 3375.547822963469, 3375.547822963469, 59915.97385760157, 843.8869557408673;
0, 0, 0, 0, 4, 4, 71, 1, 2513.243523511581, 2513.243523511581, 44610.07254233056, 628.3108808778952;
4, 4, 50, 1, 0, 0, 0, 0, 3380.337247599766, 3380.337247599766, 42254.21559499707, 845.0843118999414;
0, 0, 0, 0, 4, 4, 50, 1, 2557.822370597248, 2557.822370597248, 31972.7796324656, 639.4555926493119;
4, 4, 46, 1, 0, 0, 0, 0, 3381.587616222724, 3381.587616222724, 38888.25758656133, 845.396904055681;
0, 0, 0, 0, 4, 4, 46, 1, 2569.460510014068, 2569.460510014068, 29548.79586516178, 642.365127503517;
4, 4, 25, 1, 0, 0, 0, 0, 3391.684639092943, 3391.684639092943, 21198.02899433089, 847.9211597732357;
0, 0, 0, 0, 4, 4, 25, 1, 2663.441243136111, 2663.441243136111, 16646.50776960069, 665.8603107840277;
0, 96, 98, 1, 0, 0, 0, 0, 0, 76956.82972653268, 78560.09701250211, 801.6336429847155;
0, 0, 0, 0, 0, 96, 98, 1, 0, 65682.8899983677, 67051.28354000035, 684.1967708163302;
0, 96, 94, 1, 0, 0, 0, 0, 0, 76853.25603860538, 75252.14653780111, 800.5547504021395;
0, 0, 0, 0, 0, 96, 94, 1, 0, 65937.37528926283, 64563.67997073651, 686.8476592631544;
0, 96, 75, 1, 0, 0, 0, 0, 0, 76268.94727818707, 59585.11506108365, 794.4682008144487;
0, 0, 0, 0, 0, 96, 75, 1, 0, 67373.04865228917, 52635.19425960092, 701.8025901280123;
0, 96, 50, 1, 0, 0, 0, 0, 0, 75153.33695072016, 39142.36299516675, 782.8472599033349;
0, 0, 0, 0, 0, 96, 50, 1, 0, 70114.15427855898, 36517.78868674947, 730.3557737349894;
4, 100, 75, 1, 0, 0, 0, 0, 3182.802966382433, 79570.07415956081, 59677.55561967062, 795.7007415956082;
0, 0, 0, 0, 4, 100, 75, 1, 2830.826944396773, 70770.67360991932, 53078.00520743949, 707.7067360991932;
4, 100, 71, 1, 0, 0, 0, 0, 3176.842851499092, 79421.07128747732, 56388.96061410889, 794.2107128747731;
0, 0, 0, 0, 4, 100, 71, 1, 2846.678125949429, 71166.95314873573, 50528.53673560237, 711.6695314873573;
96, 0, 75, 1, 0, 0, 0, 0, 94501.57967461165, 0, 73829.35912079035, 984.3914549438714;
0, 0, 0, 0, 96, 0, 75, 1, 69392.97525695754, 0, 54213.26191949808, 722.8434922599744;
96, 0, 50, 1, 0, 0, 0, 0, 102665.427189569, 0, 53471.57666123386, 1069.431533224677;
0, 0, 0, 0, 96, 0, 50, 1, 76872.21110081286, 0, 40037.60994834002, 800.7521989668005;
96, 0, 25, 1, 0, 0, 0, 0, 134150.4060603281, 0, 34935.00157821043, 1397.400063128417;
0, 0, 0, 0, 96, 0, 25, 1, 105716.8927391909, 0, 27530.44081749762, 1101.217632699905;
96, 0, 21, 1, 0, 0, 0, 0, 150007.0124893856, 0, 32814.03398205309, 1562.573046764433;
0, 0, 0, 0, 96, 0, 21, 1, 120243.7808209231, 0, 26303.32705457694, 1252.539383551283;
100, 4, 75, 1, 0, 0, 0, 0, 98699.75231231008, 3947.990092492403, 74024.81423423256, 986.9975231231008;
0, 0, 0, 0, 100, 4, 75, 1, 73361.21263523313, 2934.448505409325, 55020.90947642484, 733.6121263523312;
100, 4, 71, 1, 0, 0, 0, 0, 99628.75712124966, 3985.150284849986, 70736.41755608725, 996.2875712124966;
0, 0, 0, 0, 100, 4, 71, 1, 74265.21217550985, 2970.608487020394, 52728.30064461199, 742.6521217550985;
100, 4, 50, 1, 0, 0, 0, 0, 107383.6098967354, 4295.344395869415, 53691.80494836769, 1073.836098967354;
0, 0, 0, 0, 100, 4, 50, 1, 81811.33387099921, 3272.453354839968, 40905.6669354996, 818.113338709992;
100, 4, 46, 1, 0, 0, 0, 0, 109823.0621321851, 4392.922485287403, 50518.60858080514, 1098.230621321851;
0, 0, 0, 0, 100, 4, 46, 1, 84185.12534995917, 3367.405013998367, 38725.15766098122, 841.8512534995917;
100, 4, 25, 1, 0, 0, 0, 0, 141059.7182762867, 5642.388731051467, 35264.92956907167, 1410.597182762867;
0, 0, 0, 0, 100, 4, 25, 1, 114581.0097655446, 4583.240390621784, 28645.25244138615, 1145.810097655446;
96, 96, 98, 1, 0, 0, 0, 0, 83904.83905262669, 83904.83905262669, 85652.85653288974, 874.0087401315279;
0, 0, 0, 0, 96, 96, 98, 1, 72995.44277461393, 72995.44277461393, 74516.18116575171, 760.3691955688951;
96, 96, 94, 1, 0, 0, 0, 0, 84021.59669072446, 84021.59669072446, 82271.1467596677, 875.2249655283798;
0, 0, 0, 0, 96, 96, 94, 1, 73575.81721996517, 73575.81721996517, 72042.98769454923, 766.4147627079706;
96, 96, 75, 1, 0, 0, 0, 0, 84712.67045349024, 84712.67045349024, 66181.77379178925, 882.4236505571899;
0, 0, 0, 0, 96, 96, 75, 1, 77010.98050603706, 77010.98050603706, 60164.82852034145, 802.1977136045526;
96, 96, 50, 1, 0, 0, 0, 0, 86206.34878960505, 86206.34878960505, 44899.13999458597, 897.9827998917193;
0, 0, 0, 0, 96, 96, 50, 1, 84435.70020728504, 84435.70020728504, 43976.92719129429, 879.5385438258859;
100, 100, 98, 1, 0, 0, 0, 0, 87539.46210370047, 87539.46210370047, 85788.67286162646, 875.3946210370046;
0, 0, 0, 0, 100, 100, 98, 1, 76664.75192364832, 76664.75192364832, 75131.45688517536, 766.6475192364833;
100, 100, 75, 1, 0, 0, 0, 0, 88416.27638616599, 88416.27638616599, 66312.20728962449, 884.16276386166;
0, 0, 0, 0, 100, 100, 75, 1, 81008.14162095707, 81008.14162095707, 60756.10621571781, 810.0814162095708;
100, 100, 71, 1, 0, 0, 0, 0, 88614.85267838663, 88614.85267838663, 62916.5454016545, 886.1485267838663;
0, 0, 0, 0, 100, 100, 71, 1, 81991.80970097007, 81991.80970097007, 58214.18488768875, 819.9180970097007;
100, 100, 50, 1, 0, 0, 0, 0, 90038.77512167525, 90038.77512167525, 45019.38756083763, 900.3877512167526;
0, 0, 0, 0, 100, 100, 50, 1, 89045.35592350175, 89045.35592350175, 44522.67796175087, 890.4535592350175;
100, 100, 46, 1, 0, 0, 0, 0, 90415.3917433265, 90415.3917433265, 41591.08020193018, 904.153917433265;
0, 0, 0, 0, 100, 100, 46, 1, 90910.96508045508, 90910.96508045508, 41819.04393700934, 909.1096508045508]

p =
[0.006441365659365744;
-0.006935698812720837;
-0.03488896901596035;
-0.7622591852949104;
0.004771957238156334;
-0.01133107207303337;
-0.02452697177582621;
-0.6456783687203944;
-1.258567018901194e-05;
1.123537587555102e-05;
4.154374841821949e-05;
0.0008967755121116598]

P =
[7.182807260423624, -7.734041261217285, -38.90490824599617, -849.9999999999995;
5.321239455925471, -12.63534956072988, -27.35018011148848, -719.9999999999993;
-0.0140343597913107, 0.01252863813051139, 0.04632569451009583, 1]

B =
[7.182807260423624, -7.734041261217285, -38.90490824599617;
5.321239455925471, -12.63534956072988, -27.35018011148848;
-0.0140343597913107, 0.01252863813051139, 0.04632569451009583]

b =
[-849.9999999999995;
-719.9999999999993;
1]

K_ =
[649999.9999999985, 479999.9999999995, -799.9999999999992;
479999.9999999995, 374400.0000000002, -600.0000000000002;
-799.9999999999992, -600.0000000000002, 1]

A =
[99.99999999999883, -1.940255363782255e-12, -799.9999999999992;
0, 119.9999999999995, -600.0000000000002;
0, 0, 1]