int StereoVision::calibrationError() {

    cv::Size imageSize;

    int Nimg, Npts;

    vector<vector<cv::Point3f> > objectPoints;

    vector<vector<cv::Point2f> >imagePoints;

    cv::FileStorage f("calib_debug.yml",cv::FileStorage::READ);

    cv::FileNodeIterator it = f["img_sz"].begin(); it >> imageSize.width >> imageSize.height;

      Nimg = (int) f ["NofImages"];

      Npts = (int) f["NofPoints"];

      for (int i=0; i<Nimg;i++) {

          std::stringstream imagename; imagename << "image" << i;

          cv::FileNode img = f[imagename.str()];

          vector <cv::Point3f> ov;

          vector <cv::Point2f> iv;

          for (int j=0; j<Npts; j++) {

              std::stringstream nodename; nodename << "node" << j;

              cv::FileNode pnt = img[nodename.str()];

              cv::Point3f op;

              cv::Point2f ip;

              cv::FileNodeIterator ot = pnt["objPnt"].begin(); ot >> op.x >> op.y >> op.z;

              cv::FileNodeIterator it = pnt["imgPnt"].begin(); it >> ip.x >> ip.y;

              iv.push_back(ip);

              ov.push_back(op);

          }

          imagePoints.push_back(iv);

          objectPoints.push_back(ov);

      }

      cv::Mat M,D;

      vector<cv::Mat> R,T;

      cv::calibrateCamera(objectPoints, imagePoints, imageSize, M, D,R,T,

          CV_CALIB_FIX_ASPECT_RATIO + 1*CV_CALIB_FIX_K3 + 1*CV_CALIB_ZERO_TANGENT_DIST);

      cv::FileStorage fo("calib_output.yml",cv::FileStorage::WRITE);

      fo << "M" << M;

}