bool CalibrateThread::runAndSave(const string& outputFilename,
const vector<vector<Point2f> >& imagePoints,
Size imageSize, Size boardSize, Pattern patternType, float squareSize,
float aspectRatio, int flags, Mat& cameraMatrix,
Mat& distCoeffs, bool writeExtrinsics, bool writePoints )
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
printf("%s. avg reprojection error = %.2f\n",
ok ? "Calibration succeeded" : "Calibration failed",
totalAvgErr);
if( ok )
saveCameraParams( outputFilename, imageSize,
boardSize, squareSize, aspectRatio,
flags, cameraMatrix, distCoeffs,
writeExtrinsics ? rvecs : vector<Mat>(),
writeExtrinsics ? tvecs : vector<Mat>(),
writeExtrinsics ? reprojErrs : vector<float>(),
writePoints ? imagePoints : vector<vector<Point2f> >(),
totalAvgErr );
return ok;
}
bool CameraCalibrationAlgorithm::calibrateCamera(
const VectorOfVectorOf2DPoints& gridCorners,
const cv::Size imageSize,
cv::Mat& cameraMatrix,
cv::Mat& distCoeffs
) const
{
VectorOfMat rvecs;
VectorOfMat tvecs;
std::vector<float> reprojErrs;
double totalAvgErr;
return runCalibration(gridCorners,
imageSize,
m_patternSize,
m_pattern,
1,
0,
0,
cameraMatrix,
distCoeffs,
rvecs,
tvecs,
reprojErrs,
totalAvgErr);
}
/*!
* \brief Recovery after emergency stop or power supply failure
*/
bool MapDemonCtrl::recover()
{
std::ostringstream errorMsg;
sd_->PutString("R0\n"); /// shut reposition messages from the robot
if( !sd_->checkIfStillThere() )
{
errorMsg << "COB3DMD not detected anymore.";
sd_->closePort();
error_message_ = errorMsg.str();
return false;
}
else if( !runCalibration() )
{
errorMsg << "COB3DMD recalibration failed.";
error_message_ = errorMsg.str();
return false;
}
else
{
sd_->PutString("R1\n");
return true;
}
}
bool CameraCalibration::runCalibrationAndSave( Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,vector<vector<Point2f> > imagePoints )
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
bool ok = runCalibration( s,imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs, totalAvgErr);
cout << ( ok ? "Calibration succeeded" : "Calibration failed" ) << ". avg re projection error = " << totalAvgErr << endl;
if ( ok ) {
saveCameraParams( s, imageSize, cameraMatrix, distCoeffs, rvecs ,tvecs, reprojErrs, imagePoints, totalAvgErr );
}
return( ok );
}
bool Camera::prepareandRunCalibration(const vector<vector<Point2f> >& imagePoints,
Size imageSize, Size boardSize, float squareSize,
float aspectRatio, int flags, Mat& cameraMatrix,
Mat& distCoeffs)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
bool ok = runCalibration(imagePoints, imageSize, boardSize, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
printf("%s. avg reprojection error = %.2f\n",
ok ? "Calibration succeeded" : "Calibration failed",
totalAvgErr);
return ok;
}
void imuMenu::parseInput(int16_t user_input){
switch(user_input){
case 'c': //perform callibration
runCalibration();
displayOffsets();
break;
case 'd': //print offsets
displayOffsets();
break;
case 'l': //perform leveling operation
runLevel();
displayOffsets();
break;
case 't': //perform test
runTest();
break;
default: //print menu
printMenu();
break;
}
//clear input buffer
_Console->flush();
_Console->clearWriteError();
_Console->println("\nparseInput :: press any key to continue");
//clear user input buffer
while(!_Console->available() ) {
delay(20);
}
//end
}
/** Runs and saves file. -------------------------------------------------------------------------*/
bool runAndSave(const string& outputFilename,
const vector<vector<Point2f> >& imagePoints,
Size imageSize, Size boardSize, float squareSize,
float aspectRatio, int flags, Mat& cameraMatrix,
Mat& distCoeffs, bool writeExtrinsics, bool writePoints, int qr_size_px )
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
// QR code dependant parameters
int qr_size_mm;
int known_distance;
cout << "QR code size (mm): ";
cin >> qr_size_mm;
cout << "QR code distance from source (mm): ";
cin >> known_distance;
int scale_factor = known_distance * qr_size_px / qr_size_mm;
bool ok = runCalibration(imagePoints, imageSize, boardSize, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
printf("%s. avg reprojection error = %.2f\n",
ok ? "Calibration succeeded" : "Calibration failed",
totalAvgErr);
if(ok)
saveCameraParams( outputFilename, imageSize,
boardSize, squareSize, aspectRatio,
flags, cameraMatrix, distCoeffs,
writeExtrinsics ? rvecs : vector<Mat>(),
writeExtrinsics ? tvecs : vector<Mat>(),
writeExtrinsics ? reprojErrs : vector<float>(),
writePoints ? imagePoints : vector<vector<Point2f> >(),
totalAvgErr, scale_factor, qr_size_mm );
return ok;
}
void CalibrationEnv::run()
{
while(!stopthread)
{
if(getCalibrationState()==CALIBRATIONINIT)
{
Logger(Standard) << "Calibration started";
try{
QTime time;
time.start();
runCalibration();
setCalibrationState(CALIBRATIONNOTRUNNING);
int elapsed = time.elapsed();
Logger(Standard) << "Samples/Sec: " << QString::number(calibration->getNumOfComplete()/(elapsed/(double)1000));
Logger(Standard) << "Time elapsed: " << QString::number(elapsed) << "ms";
Logger(Standard) << "Calibration stopped";
}
catch(PythonException &exception)
{
Logger(Error) << exception.exceptionmsg;
Logger(Error) << exception.type;
Logger(Error) << exception.value;
Logger(Error) << exception.traceback;
setCalibrationState(CALIBRATIONNOTRUNNING);
Logger(Standard) << "Calibration stopped";
}
catch(CalimeroException &exception)
{
Logger(Error) << exception.exceptionmsg;
setCalibrationState(CALIBRATIONNOTRUNNING);
Logger(Standard) << "Calibration stopped";
}
}
msleep(10);
}
}
/*!
* \brief Initializing
*/
bool MapDemonCtrl::init(DemonstratorParams * params)
{
/// get serial port configurable parameters
std::string SerialDeviceName = params_->getSerialDevice();
int SerialBaudrate = params_->getBaudRate();
std::ostringstream errorMsg;
int DOF = params_->getDOF();
std::vector<std::string> JointNames = params_->getJointNames();
std::vector<double> MaxVel = params_->getMaxVel();
std::vector<double> FixedVel = params_->getVels();
std::vector<double> Offsets = params_->getOffsets();
std::vector<double> LowerLimits = params_->getLowerLimits();
std::vector<double> UpperLimits = params_->getUpperLimits();
positions_.resize(DOF);
positions_[0] = 0;
positions_[1] = 0;
old_positions_.resize(DOF);
old_positions_[0] = 0;
old_positions_[1] = 0;
velocities_.resize(DOF);
velocities_[0] = 0;
velocities_[1] = 0;
std::cout << "============================================================================== " << std::endl;
std::cout << "Mapping Demonstrator Init: Trying to initialize with the following parameters: " << std::endl;
std::cout << std::endl << "Joint Names:\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << JointNames[i] << "\t";
}
std::cout << std::endl << "maxVel :\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << MaxVel[i] << "\t";
}
std::cout << std::endl << "fixedVel :\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << FixedVel[i] << "\t";
}
std::cout << std::endl << "upperLimits:\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << UpperLimits[i] << "\t";
}
std::cout << std::endl << "lowerLimits:\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << LowerLimits[i] << "\t";
}
std::cout << std::endl << "offsets :\t\t";
for (int i = 0; i < DOF; i++)
{
std::cout << Offsets[i] << "\t";
}
std::cout << std::endl << "============================================================================== " << std::endl;
/// now open serial port
int spres = sd_->openPort(SerialDeviceName.c_str(), SerialBaudrate, 2, 0);
if(spres == -1)
{
errorMsg << "Could not open device " << SerialDeviceName;
error_message_ = errorMsg.str();
return false;
}
/// this is for security. in case the robot is already outputing data, shut it...
sd_->PutString("R0\n");
usleep(200000);
if( !sd_->FlushInBuffer() )
return false; /// ...and flush serial port input buffer
usleep(200000);
std::cout << "Detecting the robot..." << std::endl;
//m_sd->PutString("N\n");
//usleep(200000);
std::string str("");
//m_sd->GetString(str);
sd_->PutString("N\n");
unsigned int ctr=0;
while(str.find("COB3DMD") == std::string::npos)
{
str.clear();
sd_->GetString(str);
std::cout << "return 1: " << str.c_str() << std::endl;
//str.resize(30); // Resize just in case weird name is too long
ctr++;
if(ctr>=100)
{
errorMsg << "Unknown robot. ID: """ << str.c_str() << std::endl ;
error_message_ = errorMsg.str();
return false;
}
}
std::cout << "Robot successfuly detected. ID: """ << str.c_str() << """" << std::endl;
serial_device_opened_ = true;
std::cout << "Now running calibration" << std::endl;
/// run pan calibration
if(!runCalibration())
{
errorMsg << "Calibration failed.";
error_message_ = errorMsg.str();
return false;
}
sd_->PutString("R1\n");
initialized_ = true;
return true;
}
int main_camera(Parameter *pParam)
{
Size boardSize(8,6);
Size imageSize;
int flags = CV_CALIB_FIX_ASPECT_RATIO;
float squareSize = pParam->square_size;
float aspectRatio = 1.f;
Mat cameraMatrix;
Mat distCoeffs;
Mat frame;
VideoCapture video;
int flag_finish = 0;
int result = 0;
// read frames from data file
vector<vector<Point2f> > imagePointsSet;
vector<Point2f> imagePoints;
vector<string> fileNames;
fileNames.clear();
imagePointsSet.clear();
video.open(pParam->camera_index);
if(video.isOpened() != true)
{
printf("fail to open camera %d\n", pParam->camera_index);
video.open(-1);
if(video.isOpened() != true)
{
printf("fail to open the default camera, please make sure an accessible camera is connected \n");
return -1;
}
else
{
printf("open the default camera\n");
}
}
while(flag_finish == 0)
{
Mat framecv;
int found = 0;
video >> frame;
cvtColor(frame, framecv, CV_RGB2GRAY);
imshow("framecv", framecv); // for oberserving input
waitKey(10);
if(framecv.cols <= 0 || framecv.rows <= 0 || framecv.data == NULL )
{
printf("finish chess board detection \n");
break;
}
imagePoints.clear();
imageSize.width = framecv.cols;
imageSize.height = framecv.rows;
found = findChessboardCorners(
framecv,
boardSize,
imagePoints,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
cornerSubPix(
framecv,
imagePoints,
Size(11,11),
Size(-1,-1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if(found)
{
char key = 0;
drawChessboardCorners( framecv, boardSize, Mat(imagePoints), found);
imshow("framecv_xx", framecv);
key = waitKey(0);
if(key == 'c' || key == 'C') // not correct
continue;
else if(key == 'q' || key == 'Q')
return 0;
else if(key == 's' || key == 'S')
flag_finish = 1;
}
printf("get a new chess board input\n");
imagePointsSet.push_back(imagePoints);
}
else
{
printf("no found usable chess board\n");
}
}
// calibrate cameras
if(1)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
result = runCalibration(imagePointsSet, imageSize, boardSize, CHESSBOARD, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
}
// test calibrate
if(1)
{
Mat view, rview, map1, map2;
int i;
Size imageSize2;
imageSize2.width = 2 * imageSize.width;
imageSize2.height = 2 * imageSize.height;
initUndistortRectifyMap(cameraMatrix,
distCoeffs,
Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
while(1)
{
char key = 0;
video>>view;
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("rview", rview);
key = waitKey(0);
if(key == 's')
break;
else if(key == 'q')
break;
}
}
int main_file(Parameter *pParam)
{
char *file_list = pParam->image_list;//"/home/sean/Pictures/calib_test1/dir.txt";
Size boardSize(8,6);
Size imageSize;
int flags = CV_CALIB_FIX_ASPECT_RATIO;
float squareSize = pParam->square_size;
float aspectRatio = 1.f;
Mat cameraMatrix;
Mat distCoeffs;
int result = 0;
// read frames from data file
vector<vector<Point2f> > imagePointsSet;
vector<Point2f> imagePoints;
vector<string> fileNames;
fileNames.clear();
imagePointsSet.clear();
getFileName(file_list, fileNames);
for(unsigned int i = 0; i < fileNames.size(); i++)
{
Mat framecv;
int found = 0;
framecv = imread(fileNames[i].c_str(), 0);
if(framecv.cols <= 0 || framecv.rows <= 0 || framecv.data == NULL )
{
printf("finish chess board detection \n");
break;
}
imagePoints.clear();
imageSize.width = framecv.cols;
imageSize.height = framecv.rows;
found = findChessboardCorners(
framecv,
boardSize,
imagePoints,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
if(found)
{
cornerSubPix(
framecv,
imagePoints,
Size(11,11),
Size(-1,-1),
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
if(found)
{
drawChessboardCorners( framecv, boardSize, Mat(imagePoints), found);
imshow("framecv_xx", framecv);
waitKey(10);
}
imagePointsSet.push_back(imagePoints);
}
}
// calibrate cameras
if(1)
{
vector<Mat> rvecs, tvecs;
vector<float> reprojErrs;
double totalAvgErr = 0;
result = runCalibration(imagePointsSet, imageSize, boardSize, CHESSBOARD, squareSize,
aspectRatio, flags, cameraMatrix, distCoeffs,
rvecs, tvecs, reprojErrs, totalAvgErr);
}
// test calibrate
if(1)
{
Mat view, rview, map1, map2;
int i;
Size imageSize2;
imageSize2.width = 2 * imageSize.width;
imageSize2.height = 2 * imageSize.height;
initUndistortRectifyMap(cameraMatrix,
distCoeffs,
Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0),
imageSize, CV_16SC2, map1, map2);
for(i = 0; i < fileNames.size(); i++)
{
view = imread(fileNames[i].c_str());
remap(view, rview, map1, map2, INTER_LINEAR);
imshow("rview", rview);
waitKey(0);
}
}
// save
if(result == 0 )
{
save_result(pParam->output_path, cameraMatrix, distCoeffs);
}
}
int NativeMiscService::runSensorCalibration(int index)
{
return runCalibration(index);
}
void OnStartCalib(class Fl_Button* b, void*)
{
CalibUI* ui = reinterpret_cast<CalibUI*>(b->window()->user_data());
if (ui != nullptr)
{
ui->calibImageBox->SetImagePoints(nullptr);
cv::Size imageSize;
cv::Size boardSize(static_cast<int>(ui->vertCornersInput->value()),
static_cast<int>(ui->horizCornersInput->value()));
ui->appData->imagesPoints.clear();
ui->appData->imagesPointsMap.clear();
ui->calibTextResult->buffer(ui->appData->resultTextBuff.get());
ui->appData->resultTextBuff->text("Calibration in progress ...");
ui->calibTextResult->redraw();
Fl::wait();
for (int i = 1; i <= ui->calibImageBrowser->size(); ++i)
{
Fl::wait();
const char* fileName = ui->calibImageBrowser->text(i);
Fl_Shared_Image *img = Fl_Shared_Image::get(fileName);
cv::Mat mat = FLImageToMat(img);
imageSize = mat.size();
std::vector<cv::Point2f> pointbuf;
bool found = cv::findChessboardCorners(mat, boardSize, pointbuf, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
Fl::wait();
cv::Mat matGray;
cv::cvtColor(mat, matGray, CV_BGR2GRAY);
if(found)
{
cv::cornerSubPix(matGray, pointbuf, cv::Size(11,11),
cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
ui->appData->imagesPoints.push_back(pointbuf);
ui->appData->imagesPointsMap.insert(std::make_pair(fileName, ui->appData->imagesPoints.size() - 1));
}
img->release();
}
if (!ui->appData->imagesPoints.empty())
{
double squareSize = ui->cellSizeInput->value();
bool ok = runCalibration(ui->appData->imagesPoints, imageSize, boardSize, squareSize,
ui->appData->cameraMatrix, ui->appData->distCoeffs);
if (ok)
{
std::stringstream buf;
double pixelSize = 0;
buf << "Focal length X (pixel rel. units) : " << ui->appData->cameraMatrix.at<double>(0,0) << "\n";
buf << "Focal length Y (pixel rel. units) : " << ui->appData->cameraMatrix.at<double>(1,1) << "\n";
buf << "Principal point x : " << ui->appData->cameraMatrix.at<double>(0,2) << "\n";
buf << "Principal point y : " << ui->appData->cameraMatrix.at<double>(1,2) << "\n";
buf << "K1 : " << ui->appData->distCoeffs.at<double>(0, 0) << "\n";
buf << "K2 : " << ui->appData->distCoeffs.at<double>(1, 0) << "\n";
buf << "K3 : " << ui->appData->distCoeffs.at<double>(4, 0) << "\n";
buf << "P1 : " << ui->appData->distCoeffs.at<double>(2, 0) << "\n";
buf << "P2 : " << ui->appData->distCoeffs.at<double>(3, 0) << "\n";
ui->appData->resultTextBuff->text(buf.str().c_str());
OnCalibImageSelected(ui->calibImageBrowser, nullptr);
return;
}
}
}
ui->appData->resultTextBuff->text("Calibration failed!");
}
