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();
}
}
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;
}
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);
}
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);
}