never-ever's profile - activity

So I write the most important part of my code (not all because program is large but I think it will be enough)

Images
From left: left images
From right: right images

Intrinsic and extrinsic parameteres for left

cameraMat1 = 
    5.327949e+02      0        3.089701e+02 
         0       5.333043e+02  2.523206e+02 
         0            0            1 

distCoeffs1 = 
    8.571497e-02   
    -2.059629e-01 
    5.049960e-03 
    -3.563517e-03

rotation1 = 
     9.986447e-01    5.134855e-02     -8.494029e-03 
    -5.165607e-02    9.978162e-01     -4.116318e-02 
     6.361810e-03    4.154616e-02      9.991163e-01

projMat1 = 
     4.204745e+02        0         3.185035e+02   0 
           0        4.204745e+02   2.475893e+02   0 
           0             0              1         0

Intrinsic and extrinsic parameteres for right

cameraMat2 = 
    5.339174e+02      0        3.001284e+02 
         0       5.343352e+02  2.333189e+02 
         0            0            1 

distCoeffs2 = 
    7.593528e-02   
    -1.829142e-01 
    7.050366e-03 
    -4.569196e-03

rotation2 = 
     9.976312e-01    5.243198e-02     -4.452930e-02 
    -5.054383e-02    9.978163e-01     4.251988e-02 
     4.666146e-02   -4.016848e-02     9.981028e-01

projMat2 = 
     4.204745e+02        0         3.185035e+02   -2.708721e+03 
           0        4.204745e+02   2.475893e+02      0 
           0             0              1            0

1. Here is startOffline() method that starts all calculation:

void TrackSystem::startOffline()
{
  try
  {
    saveData(true); //it only sets up boolean variables to know what type of data should be saved

    for(int i = 0; i < m_imageList1.size(); i++)
    {
      cv::Mat* tempMat1 = &m_imageList1[i];
      cv::Mat* tempMat2 = &m_imageList2[i];

      IplImage* image1 = cvCloneImage(&(IplImage)(*tempMat1));
      IplImage* image2 = cvCloneImage(&(IplImage)(*tempMat2));

      stereovideocallback(image1, image2);  //see 2.
    }

    calcTriangulation2TEST(); //see first post
    saveData(false);
  }
  catch(const std::exception &e)
 {
    qDebug()<<"Exception: " + QString(e.what());
 }
 catch(...){}
}

2. Here is stereovideocallback() method that allows to detect markers and save data to m_history.

void TrackSystem::stereovideocallback(IplImage* image1, IplImage* image2)
{
  static IplImage *rgba;
  bool flip_image1 = (image1->origin?true:false);
  if(flip_image1)
  {
    cvFlip(image1);
    image1->origin = !image1->origin;
  }

  bool flip_image2 = (image2->origin?true:false);
  if(flip_image2)
  {
    cvFlip(image2);
    image2->origin = !image2->origin;
  }

  // Loading camera calibration
  if(m_init)
  {
    bool calibLoaded1 = loadCalibration(image1, true); //see 3.
    bool calibLoaded2 = loadCalibration(image2, false);
    if(!calibLoaded1 || !calibLoaded2)
      emit calibLoadFailure();
  }

  // Finding and processing markers 
  static MarkerDetector<MarkerData> marker_detector1; //Alvar library
  static MarkerDetector<MarkerData> marker_detector2;

  marker_detector1.SetMarkerSize(m_markerSize);
  marker_detector2.SetMarkerSize(m_markerSize);

  QString timeNow;
  if(m_isDataSaved)
  {
    timeNow = m_timeObj->getTimeString();
  }

  bool found1 = processMarkers(&marker_detector1, image1, 0); // see 4.
  bool found2 = processMarkers(&marker_detector2, image2, 1);

  if(found1 && found2)
  {
    calcStereoMarkers(image1, 0, &marker_detector1, timeNow); // see 5.
    calcStereoMarkers(image2, 1, &marker_detector2, timeNow);
  }

  if(m_init)
    m_init = false;

  if(flip_image1)
  {
    cvFlip(image1);
    image1->origin = !image1->origin;
  }

  if(flip_image2)
  {
    cvFlip(image2);
    image2->origin = !image2->origin;
  }
}

3. Here is loadCalibration() function and readCameraParameters()

bool Tracksystem::loadCalibration(IplImage *image, bool firstCamera)
{
  bool isFileOpened = readCameraParameters(firstCamera);

  QString tempFile;
  alvar::Camera* cam;

  if(firstCamera)
  {
    tempFile = m_calibrationFile1;
    cam = &m_cam1;
  }
  else
  {
    tempFile = m_calibrationFile2;
    cam = &m_cam2;
  }

  bool calibSetted = cam->SetCalib(tempFile.toStdString().c_str(), image->width, image->height);

  if(!calibSetted)
    cam->SetRes(image->width, image->height);

  return calibSetted ;
}


bool TrackSystem::readCameraParameters(bool firstCam)
{
  QString tempFile;
  Mat* tempMat;
  Mat* tempCoeffs;
  Mat* tempProj;
  Mat* tempTransl;
  Mat* tempRot;

  if(firstCam)
  {
    tempFile = m_calibrationFile1;
    tempMat = &m_cameraMatrix1;
    tempCoeffs = &m_distortionCoeffs1;
    tempProj = &m_projectionMat1;
    tempRot = &m_rotationMat1;
    tempTransl = &m_translationMat1;
  }
  else
  {
    tempFile = m_calibrationFile2 ...

Hi,
I have images from two cameras (left and right). First I calibrate cameras and I get cameras matrices, distortion coefficients and projection matrices.
On images I detect marker and save it positions on left and right image. Next I use cv::triangulatePoints but I get really strange result (I use other library that also allows to get that 3D coords and it shows more likely results).

Here is my code:

int size0 = m_history.getSize(0);
int size1 = m_history.getSize(1);

if(size0 != size1)
{
    setInfo(tr("Cannot calculate triangulation: number of marker positions is different on left and right camera"));
    return false;
}
if(size0 <= 0)
{
    setInfo(tr("No marker postion saved. Cannot calc triangulation."));
    return false;
 }

cv::Mat pointsMat1(2, 1, CV_64F);
cv::Mat pointsMat2(2, 1, CV_64F);

for(int i = 0; i < size0; i++)
{
    cv::Point2f pt1 = m_history.getOriginalPoint(0, i);   //before edit, here was cv::Point not cv::Point2f
    cv::Point2f pt2 = m_history.getOriginalPoint(1, i);   //before edit, here was cv::Point not cv::Point2f

    pointsMat1.at<double>(0,0) = pt1.y;
    pointsMat1.at<double>(1,0) = pt1.y;
    pointsMat2.at<double>(0,0) = pt2.x;
    pointsMat2.at<double>(1,0) = pt2.y;

    cv::Mat pnts3D(4, 1, CV_32F);

    cv::triangulatePoints(m_projectionMat1, m_projectionMat2, pointsMat1, pointsMat2, pnts3D);

    CvPoint3D64f point3D;
    point3D.x = pnts3D.at<double>(0, 0);
    point3D.y = pnts3D.at<double>(1, 0);
    point3D.z = pnts3D.at<double>(2, 0);

    point3D.x = point3D.x/pnts3D.at<double>(3, 0);
    point3D.y = point3D.y/pnts3D.at<double>(3, 0);
    point3D.z = point3D.z/pnts3D.at<double>(3, 0);
    m_history.addTriangulatedPoint(point3D);
}

Where I do mistake?

Results from triangulation:

 3D_X              3D_Y              3D_Z

1.20151e+24    7.70359e+23     4.41239e+24
-2.23198e+23    -3.11741e+22    -7.98741e+23
3.79697e+22    -1.45599e+22    1.31405e+23
-7.27922e+22    6.40761e+22    -2.45849e+23
7.11722e+22    -9.67023e+22    2.32894e+23
2.14094e+22    -2.26098e+22    7.11648e+22
4.95297e+22    -2.77331e+22    1.70013e+23
2.21597e+23    -1.22193e+22    7.79736e+23
-3.94602e+23    -1.85898e+23    -1.43658e+24
2.56816e+24    2.13037e+24     1.22021e+25

And from other library (it seems quite correct):

  3D_X              3D_Y              3D_Z

8.33399      4.74962       62.6119
8.33243       -1.62743       60.8669
8.40008       -8.43666       59.5643
8.47287       -15.1735       58.3184
8.52143       -21.964       57.0896
8.4673       -17.6672       57.6249
8.31039       -10.9452       58.6376
8.39497       -4.27203       60.6766
8.37444       2.56077       62.3287
5.41351       4.73769       62.7424

My projection matrices:

P1 = 
    4.484533e+02         0             3.073146e+02    0 
           0           4.484533e+02    2.473914e+02    0 
           0                  0             1          0

P2 = 
    4.484533e+02         0             3.073146e+02     -8.275082e+02 
           0           4.484533e+02    2.473914e+02          0 
           0                  0              1               0

EDIT:

After correct cv::Point to cv::Point2f and with cv::undistortPoints I get that: (I pass to cv::undistortPoints only ... (more)