예제 #1
0
void SLTrackerWorker::setup(){

    // Initialize the tracker object
    tracker = new TrackerICP();
    CalibrationData calibration = CalibrationData();
    calibration.load("calibration.xml");
    Eigen::Matrix3f Kc;
    Kc << calibration.Kc(0,0), calibration.Kc(0,1), calibration.Kc(0,2),
          calibration.Kc(1,0), calibration.Kc(1,1), calibration.Kc(1,2),
          calibration.Kc(2,0), calibration.Kc(2,1), calibration.Kc(2,2);
    tracker->setCameraMatrix(Kc);

    QSettings settings("SLStudio");
    writeToDisk = settings.value("writeToDisk/tracking",false).toBool();
    if(writeToDisk){

        // Tracking file
        QString fileName = QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss");
        fileName.append(".track");
        ofStream = new std::ofstream;
        ofStream->open(fileName.toLocal8Bit().data(), std::ofstream::out);
        (*ofStream) << "#V1.1 SLStudio Tracking File" << std::endl << "t_ms,Tx,Ty,Tz,Q0,Qx,Qy,Qz,RMS" << std::endl;

        trackingTime.start();
    }
}
예제 #2
0
void SLStudio::onActionLoadCalibration(){
    QString fileName = QFileDialog::getOpenFileName(this, "Choose calibration file", QString(), "*.xml");
    if(!(fileName.length() == 0)){
        CalibrationData calibration;
        calibration.load(fileName);
        calibration.save("calibration.xml");
    }
}
int main(int argc, char const ** argv)
{
  char const * keys =
    "{    h | help   | false            | show help message}"
    "{    f | file   | calibration.yaml | calibration yaml file}"
    "{    c | camera | 0                | camera number}"
    ;

  cv::CommandLineParser parser(argc, argv, keys);

  if (parser.get<bool>("help"))
  {
    parser.printParams();
    return 0;
  }
  
  int capnum = parser.get<int>("camera");
  cv::VideoCapture cap(capnum);

  CalibrationData calibData;
  calibData.load(parser.get<std::string>("file"));

  cv::Mat map1, map2;
  cv::initUndistortRectifyMap(calibData.cameraMatrix, calibData.distCoeffs, cv::Mat(), 
      getOptimalNewCameraMatrix(calibData.cameraMatrix, calibData.distCoeffs, calibData.imageSize, 1, calibData.imageSize, 0),
      calibData.imageSize, CV_16SC2, map1, map2);

  cv::namedWindow("undistorted");
  while(true)
  {
    cv::Mat frame, undistFrame;
    cap >> frame;

    if(frame.size() != calibData.imageSize)
    {
      std::cerr << "Frame size does not match calibration image size" << std::endl;
      exit(-1);
    }

    cv::remap(frame, undistFrame, map1, map2, cv::INTER_LINEAR);

    cv::imshow("image", frame);
    cv::imshow("undistorted", undistFrame);

    cv::waitKey(10);
  }

  return 0;
}
예제 #4
0
void SLStudio::onActionExportCalibration() {
    CalibrationData calibration;
    calibration.load("calibration.xml");
//  Non native file dialog
//    QFileDialog saveFileDialog(this, "Export Calibration", QString(), "*.slcalib;;*.xml;;*.m");
//    saveFileDialog.setDefaultSuffix("slcalib");
//    saveFileDialog.exec();
//    QString fileName = saveFileDialog.selectedFiles().first();
//  Native file dialog
    QString selectedFilter;
    QString fileName = QFileDialog::getSaveFileName(this, "Export Calibration", QString(), "*.slcalib;;*.xml;;*.m", &selectedFilter);
    QFileInfo info(fileName);
    QString type = info.suffix();
    if(type == "")
        fileName.append(selectedFilter.remove(0,1));
    calibration.save(fileName);
}
예제 #5
0
void SLPointCloudWidget::updateCalibration(){

    CalibrationData calibration;
    bool load_result = calibration.load("calibration.xml");
	if (!load_result)
		return;
    // Camera coordinate system
    visualizer->addCoordinateSystem(50, "camera", 0);

    // Projector coordinate system
    cv::Mat TransformPCV(4, 4, CV_32F, 0.0);
    cv::Mat(calibration.Rp).copyTo(TransformPCV.colRange(0, 3).rowRange(0, 3));
    cv::Mat(calibration.Tp).copyTo(TransformPCV.col(3).rowRange(0, 3));
    TransformPCV.at<float>(3, 3) = 1.0; // make it homogeneous 
    Eigen::Affine3f TransformP;
    cv::cv2eigen(TransformPCV, TransformP.matrix());

    visualizer->addCoordinateSystem(50, TransformP.inverse(), "projector", 0);
}