void Features::serialize(cv::FileStorage& fs) const
{
fs << "{";
cvWriteComment(*fs, "Features class", 0);
// cv::write(fs,"keypoints",kpts_);
fs << "}";
}
void Images::serialize(cv::FileStorage& fs) const
{
if (!ondisk_ && persist_img_)
{
imwrite(path_ + "/" + fname_, src_);
}
fs << "{";
cvWriteComment(*fs, "Images class", 0);
fs << "fname" << fname_;
fs << "path" << path_;
fs << "ondisk" << (int)(ondisk_ || persist_img_);
fs << "persist" << (int)(persist_img_);
fs << "}";
}
//serialization
void Features2d::write(cv::FileStorage& fs) const
{
cvWriteComment(*fs, "Features2d", 0);
fs << "{";
fs << "image_name" << image_name;
fs << "mask_name" << mask_name;
fs << "camera";
camera.write(fs);
if (keypoints.size())
cv::write(fs, "keypoints", keypoints);
if (!descriptors.empty())
fs << "descriptors" << descriptors;
fs << "}";
}
void write_my_struct( CvFileStorage* fs, const char* name, my_struct* ms) {
cvWriteComment (fs, name, 0);
cvWriteInt(fs, "integer", ms->i);
cvStartWriteStruct(fs, "point", CV_NODE_SEQ);
cvWriteInt(fs, 0, ms->point.x);
cvWriteInt(fs, 0, ms->point.y);
cvEndWriteStruct(fs);
cvStartWriteStruct(fs, "rectangle", CV_NODE_SEQ);
cvWriteInt(fs, 0, ms->rect.x);
cvWriteInt(fs, 0, ms->rect.y);
cvWriteInt(fs, 0, ms->rect.width);
cvWriteInt(fs, 0, ms->rect.height);
cvEndWriteStruct(fs);
}
/* simple API for reading/writing data */
CV_IMPL void
cvSave( const char* filename, const void* struct_ptr,
const char* _name, const char* comment, CvAttrList attributes )
{
CvFileStorage* fs = 0;
if( !struct_ptr )
CV_Error( CV_StsNullPtr, "NULL object pointer" );
fs = cvOpenFileStorage( filename, 0, CV_STORAGE_WRITE );
if( !fs )
CV_Error( CV_StsError, "Could not open the file storage. Check the path and permissions" );
cv::String name = _name ? cv::String(_name) : cv::FileStorage::getDefaultObjectName(filename);
if( comment )
cvWriteComment( fs, comment, 0 );
cvWrite( fs, name.c_str(), struct_ptr, attributes );
cvReleaseFileStorage( &fs );
}
/*
* Sets up the WriteToDisk given the base of a filname.
* Creates a WriteOut Object.
*
* e.g. if you want to create a files named myexperiment.yaml and myexperiment.mov
* pass in the string "myexperiment"
*
*/
WriteOut* SetUpWriteToDisk(const char* dirfilename, const char* outfilename, CvMemStorage* Mem){
/** Create new instance of WriteOut object **/
WriteOut* DataWriter = CreateDataWriter();
/** Create Filenames **/
char* YAMLDataFileName = CreateFileName(dirfilename, outfilename, ".yaml");
/** Open YAML Data File for writing**/
DataWriter->fs=cvOpenFileStorage(YAMLDataFileName,Mem,CV_STORAGE_WRITE);
if (DataWriter->fs==0) {
printf("DataWriter->fs is zero! Could you have specified the wrong directory?\n");
--(DataWriter->error);
}
/** If there were errors, return immediately **/
if (DataWriter->error < 0) return DataWriter;
/** Write the header for the YAML data file **/
cvWriteComment(DataWriter->fs, "Remote Control Worm Experiment Data Log\nMade by OpticalMindControl software\[email protected]\n",0);
cvWriteString(DataWriter->fs, "gitHash", build_git_sha,0);
cvWriteString(DataWriter->fs, "gitBuildTime",build_git_time,0);
/** Write Out Current Time**/
struct tm *local;
time_t t;
t = time(NULL);
local = localtime(&t);
cvWriteString(DataWriter->fs, "ExperimentTime",asctime(local),0);
DataWriter->filename=YAMLDataFileName;
return DataWriter;
}
/*
* Write everything out to its own seperate YAML file
*
*/
void WriteProtocolToYAML(Protocol* myP){
/** Open file for writing **/
printf("We are about to write out a protocol. Here is some info about the protocol.:\n");
printf("The protocol has %d steps.\n",myP->Steps->total);
CvFileStorage* fs=cvOpenFileStorage(myP->Filename,myP->memory,CV_STORAGE_WRITE);
if (fs==0){
printf("fs is zero! Could you have specified the wrong directory?\n");
return;
}
printf("Writing to %s\n",myP->Filename);
/** Write out Generic comments **/
cvWriteComment(fs, "Illumination Protocol:",0);
cvWriteComment(fs, "Generated by the IlluminationWormProtocol Library \nmade by [email protected]",0);
cvWriteComment(fs, "\nSoftware Version Information:",0);
cvWriteComment(fs, build_git_sha,0);
cvWriteComment(fs, build_git_time,0);
cvWriteComment(fs, "\n",0);
printf("wrote comments\n");
WriteProtocol(myP,fs);
cvReleaseFileStorage(&fs);
}
// ------------------------------------------------------------------------------------------------------------
// Se hace un paso del EKF con prediccion, matching, ransac y update.
// ------------------------------------------------------------------------------------------------------------
void EKF::step(const cv::Mat &image)
{
_ekfSteps++;
if (_logFile.is_open())
{
_logFile << std::endl << std::endl;
_logFile << "~~~~~~~~~~~~ STEP " << _ekfSteps << " ~~~~~~~~~~~~" << std::endl;
}
//-------------------------------------------------------------------------------------------------------------
// Prediccion
Timer timer;
if (!_strOutputPath.empty())
{
std::stringstream frameName;
frameName << "Frame " << _ekfSteps;
_outputFileStorage << frameName.str() << "{";
cvWriteComment(_outputFileStorage.fs, "", 0);
cvWriteComment(_outputFileStorage.fs, "Running time (microseconds)", 0);
cvWriteComment(_outputFileStorage.fs, "", 0);
timer.start();
}
VectorImageFeaturePrediction predictedDistortedFeatures;
VectorMatd predictedFeatureJacobians;
SYSTEMTIME sys;
GetLocalTime( &sys );
int MileTs = sys.wSecond;int MileT = sys.wMilliseconds;
stateAndCovariancePrediction(state, stateCovarianceMatrix);
GetLocalTime( &sys );
int MileT2s = sys.wSecond;int MileT2 = sys.wMilliseconds;
int DetaT = MileT2 - MileT;
int DetaTs = MileT2s -MileTs;
//std::cout<< std::endl;std::cout<< "The CovariancePrediction time is :"<< DetaTs<<"S......................."<< std::endl;
//std::cout<< "CovariancePrediction time is :"<< DetaT<<"mS......................."<< std::endl; std::cout<< std::endl;
// se le pasa mapFeatureIndexes vacio para que sepa que la prediccion es para todos los features en el mapa
std::vector<int> mapFeatureIndexes;
VectorMapFeature unseenFeatures;
predictCameraMeasurements( state,
stateCovarianceMatrix,
state.mapFeatures,
mapFeatureIndexes,
predictedDistortedFeatures,
predictedFeatureJacobians,
unseenFeatures );
GetLocalTime( &sys );
MileT2s = sys.wSecond;MileT2 = sys.wMilliseconds;
DetaT = MileT2 - MileT;
DetaTs = MileT2s -MileTs;
//std::cout<< std::endl;std::cout<< "The predictCamera time is :"<< DetaTs<<"S......................."<< std::endl;
//std::cout<< "predictCamera time is :"<< DetaT<<"mS......................."<< std::endl; std::cout<< std::endl;
if (!_strOutputPath.empty())
{
double predictionTime = timer.getElapsedTimeInMicroSec();
timer.stop();
_outputFileStorage << "Prediction" << predictionTime;
}
if (!_strOutputPath.empty())
{
cv::Mat predictionImage;
drawPrediction(image, predictedDistortedFeatures, state.mapFeatures, predictionImage);
//Test!
cv::imshow("Predicciones", predictionImage);
cv::waitKey(2);
//std::stringstream imageFileName;
//imageFileName << _strOutputPath << std::setfill('0') << std::setw(5) << _ekfSteps << ".png";
//cv::imwrite(imageFileName.str().c_str(), predictionImage);
#ifndef ANDROID
// _outputVideoWriter.write(predictionImage);
#endif
#ifndef DEBUG_SHOW_IMAGES
}
#else
// Mostramos la imagen
cv::namedWindow("Predicciones");
cv::imshow("Predicciones", predictionImage);
cv::waitKey(2);
}
void CalibrateThread::saveCameraParams( const string& filename,
Size imageSize, Size boardSize,
float squareSize, float aspectRatio, int flags,
const Mat& cameraMatrix, const Mat& distCoeffs,
const vector<Mat>& rvecs, const vector<Mat>& tvecs,
const vector<float>& reprojErrs,
const vector<vector<Point2f> >& imagePoints,
double totalAvgErr )
{
FileStorage fs( filename, FileStorage::WRITE );
time_t t;
time( &t );
struct tm *t2 = localtime( &t );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
fs << "calibration_time" << buf;
if( !rvecs.empty() || !reprojErrs.empty() )
fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size());
fs << "image_width" << imageSize.width;
fs << "image_height" << imageSize.height;
fs << "board_width" << boardSize.width;
fs << "board_height" << boardSize.height;
fs << "square_size" << squareSize;
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
fs << "aspectRatio" << aspectRatio;
if( flags != 0 )
{
sprintf( buf, "flags: %s%s%s%s",
flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "",
flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" );
cvWriteComment( *fs, buf, 0 );
}
fs << "flags" << flags;
fs << "camera_matrix" << cameraMatrix;
fs << "distortion_coefficients" << distCoeffs;
fs << "avg_reprojection_error" << totalAvgErr;
if( !reprojErrs.empty() )
fs << "per_view_reprojection_errors" << Mat(reprojErrs);
if( !rvecs.empty() && !tvecs.empty() )
{
CV_Assert(rvecs[0].type() == tvecs[0].type());
Mat bigmat((int)rvecs.size(), 6, rvecs[0].type());
for( int i = 0; i < (int)rvecs.size(); i++ )
{
Mat r = bigmat(Range(i, i+1), Range(0,3));
Mat t = bigmat(Range(i, i+1), Range(3,6));
CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
//*.t() is MatExpr (not Mat) so we can use assignment operator
r = rvecs[i].t();
t = tvecs[i].t();
}
cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
fs << "extrinsic_parameters" << bigmat;
}
if( !imagePoints.empty() )
{
Mat imagePtMat((int)imagePoints.size(), imagePoints[0].size(), CV_32FC2);
for( int i = 0; i < (int)imagePoints.size(); i++ )
{
Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols);
Mat imgpti(imagePoints[i]);
imgpti.copyTo(r);
}
fs << "image_points" << imagePtMat;
}
}
void save_camera_params( const char* out_filename, int image_count, CvSize img_size,
CvSize board_size, float square_size,
float aspect_ratio, int flags,
const CvMat* camera_matrix, CvMat* dist_coeffs,
const CvMat* extr_params, const CvSeq* image_points_seq,
const CvMat* reproj_errs, double avg_reproj_err )
{
CvFileStorage* fs = cvOpenFileStorage( out_filename, 0, CV_STORAGE_WRITE );
time_t t;
time( &t );
struct tm *t2 = localtime( &t );
char buf[1024];
strftime( buf, sizeof(buf)-1, "%c", t2 );
cvWriteString( fs, "calibration_time", buf );
cvWriteInt( fs, "image_count", image_count );
cvWriteInt( fs, "image_width", img_size.width );
cvWriteInt( fs, "image_height", img_size.height );
cvWriteInt( fs, "board_width", board_size.width );
cvWriteInt( fs, "board_height", board_size.height );
cvWriteReal( fs, "square_size", square_size );
if( flags & CV_CALIB_FIX_ASPECT_RATIO )
cvWriteReal( fs, "aspect_ratio", aspect_ratio );
if( flags != 0 )
{
sprintf( buf, "flags: %s%s%s%s",
flags & CV_CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
flags & CV_CALIB_FIX_ASPECT_RATIO ? "+fix_aspect_ratio" : "",
flags & CV_CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
flags & CV_CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" );
cvWriteComment( fs, buf, 0 );
}
cvWriteInt( fs, "flags", flags );
cvWrite( fs, "camera_matrix", camera_matrix );
cvWrite( fs, "distortion_coefficients", dist_coeffs );
cvWriteReal( fs, "avg_reprojection_error", avg_reproj_err );
if( reproj_errs )
cvWrite( fs, "per_view_reprojection_errors", reproj_errs );
if( extr_params )
{
cvWriteComment( fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 );
cvWrite( fs, "extrinsic_parameters", extr_params );
}
if( image_points_seq )
{
cvWriteComment( fs, "the array of board corners projections used for calibration", 0 );
assert( image_points_seq->total == image_count );
CvMat* image_points = cvCreateMat( 1, image_count*board_size.width*board_size.height, CV_32FC2 );
cvCvtSeqToArray( image_points_seq, image_points->data.fl );
cvWrite( fs, "image_points", image_points );
cvReleaseMat( &image_points );
}
cvReleaseFileStorage( &fs );
}
