//Because of Unknow Error, so I have to use 6 XML files to save those matrixs
void myPCA::saveModel()
{
FileStorage fs1("mean.xml", FileStorage::WRITE);
FileStorage fs2("sample.xml", FileStorage::WRITE);
FileStorage fs3("eigen.xml", FileStorage::WRITE);
FileStorage fs4("proj.xml", FileStorage::WRITE);
FileStorage fs5("ID.xml", FileStorage::WRITE);
FileStorage fs6("sampleOri.xml", FileStorage::WRITE);
fs1 << "MeanFace" << meanFace;
fs2 << "SampleMatrix" << sampleMatrix ;
fs3 << "EigenVectors" << eigenVectors;
fs4 << "ProjFaces" << projFaces;
fs5 << "TrainImageID" << trainImageID;
fs6 << "SampleMatrixOri" << sampleMatrixOri ;
}
void myPCA::loadModel()
{
FileStorage fs1("mean.xml", FileStorage::READ);
FileStorage fs2("sample.xml", FileStorage::READ);
FileStorage fs3("eigen.xml", FileStorage::READ);
FileStorage fs4("proj.xml", FileStorage::READ);
FileStorage fs5("ID.xml", FileStorage::READ);
FileStorage fs6("sampleOri.xml", FileStorage::READ);
fs1["MeanFace"] >> meanFace;
fs2["SampleMatrix"] >> sampleMatrix ;
fs3["EigenVectors"] >> eigenVectors;
fs4["ProjFaces"] >> projFaces;
fs5["TrainImageID"] >> trainImageID;
fs6["SampleMatrixOri"] >> sampleMatrixOri ;
}
//評価用
void Agi::writeprojection(){
//保存場所とファイル名を考慮
const string d = data->dataname.at(data->dataid);
string dir = "../data/" + d +"/"+ d+ "-projection/projection_" +to_string(writenum)+ ".dat";
int dim = data->dim;
double projection[dim*2];
int j = 0;
for (prj p : ee){
projection[2*j] = p.first;
projection[2*j+1] = p.second;
j++;
}
ofstream fs2(dir,ios::out | ios::binary);
for(int i = 0; i< dim *2 ;i++ ){
fs2.write((char*) &projection[i],sizeof(double));
}
fs2.close();
writenum++;
}
void MasterTimer_Test::restart()
{
MasterTimer* mt = m_doc->masterTimer();
mt->start();
Function_Stub fs1(m_doc);
fs1.start(mt, FunctionParent::master());
Function_Stub fs2(m_doc);
fs2.start(mt, FunctionParent::master());
Function_Stub fs3(m_doc);
fs3.start(mt, FunctionParent::master());
QTest::qWait(60);
QVERIFY(mt->runningFunctions() == 3);
mt->stop();
QTest::qWait(60);
QVERIFY(mt->runningFunctions() == 0);
QVERIFY(mt->m_functionList.size() == 0);
QVERIFY(mt->m_functionListMutex.tryLock() == true);
mt->m_functionListMutex.unlock();
// QVERIFY(mt->m_running == false);
QVERIFY(mt->m_stopAllFunctions == false);
mt->start();
QVERIFY(mt->runningFunctions() == 0);
QVERIFY(mt->m_functionList.size() == 0);
QVERIFY(mt->m_functionListMutex.tryLock() == true);
mt->m_functionListMutex.unlock();
// QVERIFY(mt->m_running == true);
QVERIFY(mt->m_stopAllFunctions == false);
fs1.start(mt, FunctionParent::master());
fs2.start(mt, FunctionParent::master());
fs3.start(mt, FunctionParent::master());
QTest::qWait(60);
QVERIFY(mt->runningFunctions() == 3);
mt->stopAllFunctions();
}
int main3(int argc, const char** argv)
{
try
{
e1.assign(expression_text);
e2.assign(pre_expression);
for(int i = 1; i < argc; ++i)
{
std::cout << "Processing file " << argv[i] << std::endl;
std::ifstream fs(argv[i]);
std::string in;
std::wstring inw2;
std::wifstream fs2(argv[i]);
load_file2(inw2, fs2);
fs2.close();
load_file1(in, fs); //从文件内获取内容
fs.close();
std::string out_name = std::string(argv[i]) + std::string(".htm");
std::ofstream os(out_name.c_str()); //创建个保存文件
os << header_text; //写入html头
// strip '<' and '>' first by outputting to a
// temporary string stream
std::ostringstream t(std::ios::out | std::ios::binary);
std::ostream_iterator<char> oi(t);
boost::regex_merge(oi, in.begin(), in.end(), e2, pre_format, boost::match_default | boost::format_all);
// then output to final output stream
// adding syntax highlighting:
std::string s(t.str());
std::ostream_iterator<char> out(os);
boost::regex_merge(out, s.begin(), s.end(), e1, format_string, boost::match_default | boost::format_all);
os << footer_text;
os.close();
}
}
catch(...)
{
return -1;
}
return 0;
}
// private
void WebcamDevice::calibrateCam(cv::FileStorage *fs)
{
bool stop = false;
do
{
QMessageBox::StandardButton button = QMessageBox::critical(0, tr("Erreur"),
tr("Impossible de trouver la matrice de calibration.\nDisposez-vous deja d'une matrice camera ?"),
QMessageBox::Yes | QMessageBox::No);
switch(button)
{
case QMessageBox::No:
CalibrateDialog calibrateDialog(_frame);
calibrateDialog.exec();
break;
case QMessageBox::Yes:
{
cv::Mat test1, test2;
QString fsPath = QFileDialog::getOpenFileName(0, "Ouvrir la matrice camera", "../rsc/", "FileStorage (*.yml)");
if(fsPath != "")
fs->open(fsPath.toStdString(), cv::FileStorage::READ);
(*fs)["cameraMatrix"] >> test1;
(*fs)["distCoeffs"] >> test2;
if(test1.empty() || test2.empty())
QMessageBox::warning(0, tr("matrice incorrecte"), tr("La matrice indiquee n'est pas correcte"));
else
{
stop = true;
cv::FileStorage fs2("../rsc/intrinsicMatrix.yml", cv::FileStorage::WRITE);
fs2 << "cameraMatrix" << test1 << "distCoeffs" << test2;
fs2.release();
}
break;
}
default:
emit shutdownSignal();
break;
}
}while(!stop);
}
void MasterTimer_Test::stop()
{
MasterTimer* mt = m_doc->masterTimer();
mt->start();
Function_Stub fs1(m_doc);
fs1.start(mt, FunctionParent::master());
Function_Stub fs2(m_doc);
fs2.start(mt, FunctionParent::master());
Function_Stub fs3(m_doc);
fs3.start(mt, FunctionParent::master());
QTest::qWait(60);
QVERIFY(mt->runningFunctions() == 3);
mt->stop();
QTest::qWait(60);
QVERIFY(mt->runningFunctions() == 0);
// QVERIFY(mt->m_running == false);
}
// muestra el sistema, matriz A y vector b en archivos separado
void Sistema_Lineal::mostrar_sistema(Matriz *a, Vector *x, Vector *b)
{
unsigned long long n,i,j; // n = cant filas = cant columnas
n = a->Columnas();
cout<<"tamaño"<<n<<"\n\n";
//cout<<".....................................................................::::::::::::::::::::::::::: tamaño"<<n<<"\n\n";
ofstream fs1("/home/usuario/Escritorio/carpeta/matriz.dat"); //crear archivo de salida con la matriz
ofstream fs2("/home/usuario/Escritorio/carpeta/vector.dat"); //crear archivo de salida con el vector
for(i=0; i<n; i++)
{
for(j=0; j<n; j++)
{
fs1 << a->Retorna(i,j) <<" ";//enviar la matriz al archivo de salida
//cout << a->Retorna(i,j) <<" \t";
}
fs1 << "\n";
}
for(i=0; i<n; i++)
{
fs2 << b->Retorna(i)<<" \n"; //enviar el vector al archivo de salida
}
fs1.close(); //cerrar archivo de salida
fs2.close();
}
Main()
{
mScale = 0.1f;
mOffset = 1.0f;
mDataOffset = 0;
isClicked = false;
screen = Vec2<unsigned int>(800, 600);
std::setlocale(LC_ALL, "en_US.UTF-8");
glfwInit();
glfwWindowHint(GLFW_CLIENT_API, GLFW_OPENGL_API);
glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
//glfwWindowHint(GLFW_OPENGL_DEBUG_CONTEXT, GL_TRUE);
glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 0);
GLFWwindow* window = glfwCreateWindow(screen.x, screen.y, "test", nullptr, nullptr);
if (window == nullptr)
{
printf("cant create window");
return;
}
glfwSetWindowSizeCallback(window, windowSizeCallback);
glfwSetKeyCallback(window, keyCallback);
glfwSetMouseButtonCallback(window, clickCallback);
glfwSetCursorPosCallback(window, mouseCallback);
glfwMakeContextCurrent(window);
glewExperimental = true;
glewInit();
int tmp;
glGetIntegerv(GL_MAX_ELEMENTS_VERTICES , &tmp);
std::cout << "GL_MAX_ELEMENTS_VERTICES: " << tmp << std::endl;
int err = Pa_Initialize();
if (err != paNoError)
printf("error");
int num = Pa_GetDeviceCount();
const PaDeviceInfo* devInfo;
const PaHostApiInfo* apiInfo;
for (int i = 0; i < num; ++i) {
devInfo = Pa_GetDeviceInfo(i);
apiInfo = Pa_GetHostApiInfo(devInfo->hostApi);
printf("%i, %s on %s\n", i, devInfo->name, apiInfo->name);
}
float sampleRate = 44100.0f;
double t = glfwGetTime();
Kern k(sampleRate, 12, 4 * 16.352f, sampleRate / 2);
BlockMatrix<std::complex<double>> b(k.K, k.mN0, k.mB, 0.01);
mAudioData = new double[b.getWidth()];
mAudioLength = b.getWidth();
for (unsigned int i = 0; i < mAudioLength; ++i) {
mAudioData[i] = wave(55, sampleRate, i) + wave(110, sampleRate, i) + wave(220, sampleRate, i)
+ wave(440, sampleRate, i) + wave(880, sampleRate, i) + wave(1760, sampleRate, i)
+ wave(3520, sampleRate, i) + wave(7040, sampleRate, i);
}
printf("kernel time:%f\n", glfwGetTime() - t);
float drawArray[k.mB * 2];
std::complex<double> out[k.mB];
CQT::transform(mAudioData, out, b, mAudioLength);
t = glfwGetTime();
printf("transform time:%f\n", glfwGetTime() - t);
//glEnable(GL_DEBUG_OUTPUT_SYNCHRONOUS);
glDebugMessageCallback(debugCallback, nullptr);
//glEnable(GL_DEBUG_OUTPUT);
printf("%s\n", glGetString(GL_VERSION));
Shader fs("res/shader/fragment.c", true, GL_FRAGMENT_SHADER);
Shader vs("res/shader/vertex.c", true, GL_VERTEX_SHADER);
Program* p = new Program();
p->attach(fs);
p->attach(vs);
p->build();
p->use();
Program p2;
Shader fs2("res/shader/fragment2.c", true, GL_FRAGMENT_SHADER);
Shader vs2("res/shader/vertex2.c", true, GL_VERTEX_SHADER);
p2.attach(fs2);
p2.attach(vs2);
p2.build();
p2.use();
int uniformData = p2.getUniformLocation("data");
unsigned int waterfallSize = 512;
tm = new TextureManager();
unsigned int waterfallTexture;
unsigned int waterfallId = tm->getFreeTexture();
glGenTextures(1, &waterfallTexture);
glActiveTexture(GL_TEXTURE0 + waterfallId);
glBindTexture( GL_TEXTURE0, waterfallTexture );
glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D_ARRAY, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
unsigned char* textureTmp = new unsigned char[waterfallSize * b.getWidth()];
glTexImage2D( GL_TEXTURE_2D_ARRAY, 0, GL_R8, b.getWidth(), waterfallSize, 0, GL_RED, GL_UNSIGNED_BYTE, textureTmp);
delete textureTmp;
float max = 0;
for (unsigned int i = 0; i < k.mB; ++i) {
drawArray[2 * i + 0] = (float)i / k.mB * 2.0f - 1.0f;
float tmp = std::abs(out[i]);
drawArray[2 * i + 1] = tmp;
max = std::max(tmp, max);
}
font = new Font(512, "res/font/DroidSans.woff", 32, tm);
print = new Print(font);
//print.set(&font, "res/shader/fontVertex.c", "res/shader/fontFragment.c");
print->setScreenSize(screen);
glm::vec2* vert = new glm::vec2[1024];
glm::vec2* debug = new glm::vec2[b.getWidth()];
for (unsigned int i = 0; i < b.getWidth(); ++i) {
debug[i].x = (float)i / b.getWidth() * 2.0f - 1.0f;
}
uint32_t vao;
glGenVertexArrays(1, &vao);
glBindVertexArray(vao);
uint32_t vbo[2];
glGenBuffers(1, vbo);
glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
glEnableVertexAttribArray(0);
glBufferData(GL_ARRAY_BUFFER, k.mB * sizeof(glm::vec2), drawArray, GL_DYNAMIC_DRAW);
glVertexAttribPointer(0, 2, GL_FLOAT, GL_FALSE, 0, 0);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glEnable(GL_BLEND);
glfwSetWindowUserPointer(window, this);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
double time, timeo;
glfwSwapInterval(1);
PaStream* stream;
PaStreamParameters params;
params.device = 21;
params.channelCount = 1;
params.sampleFormat = paFloat32;
params.hostApiSpecificStreamInfo = nullptr;
params.suggestedLatency = 0.5;
err = Pa_OpenStream(&stream, ¶ms, nullptr, sampleRate, paFramesPerBufferUnspecified, 0, paCallback, this);
if (err != paNoError)
printf("error %i", err);
Pa_StartStream(stream);
while(!glfwWindowShouldClose(window))
{
timeo = time;
time = glfwGetTime();
CQT::transform(mAudioData, out, b, mAudioLength);
max = 0.0f;
for (unsigned int i = 0; i < k.mB; ++i) {
drawArray[2 * i + 0] = (float)i / k.mB * 2.0f - 1.0f;
float tmp = std::abs(out[i]);
drawArray[2 * i + 1] = tmp;
max = std::max(tmp, max);
}
for (unsigned int i = 0; i < k.mB; ++i) {
drawArray[2 * i + 1] = std::log(drawArray[2 * i +1]) * mScale + mOffset;
}
//printf("%f\n", drawArray[1]);
glBindVertexArray(vao);
glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
glBufferData(GL_ARRAY_BUFFER, k.mB * sizeof(glm::vec2), drawArray, GL_DYNAMIC_DRAW);
p->use();
glDrawArrays(GL_LINE_STRIP, 0, k.mB);
for (unsigned int i = 0; i < b.getWidth(); ++i) {
debug[i].y = mAudioData[i] / 15.0;
}
glBufferData(GL_ARRAY_BUFFER, b.getWidth() * sizeof(glm::vec2), debug, GL_DYNAMIC_DRAW);
glDrawArrays(GL_LINE_STRIP, 0, b.getWidth());
print->printfAt(-300.0f, 100.0f, 16.0f, 16.0f, u8"Fps:%03.3f", 1/(time-timeo));
glfwSwapBuffers(window);
glClear(GL_COLOR_BUFFER_BIT);
glfwPollEvents();
}
Pa_StopStream(stream);
Pa_CloseStream(stream);
Pa_Terminate();
std::cout << "Hello World. I'm Peach." << std::endl;
}
/// Calibrates the extrinsic parameters of the setup and saves it to an XML file
/// Press'r' to retreive chessboard corners
/// 's' to save and exit
/// 'c' to exit without saving
/// In: inputCapture1: video feed of camera 1
/// inputCapture2: video feed of camera 2
void CalibrateEnvironment(VideoCapture& inputCapture1, VideoCapture& inputCapture2)
{
Size boardSize;
boardSize.width = BOARD_WIDTH;
boardSize.height = BOARD_HEIGHT;
const string fileName1 = "CameraIntrinsics1.xml";
const string fileName2 = "CameraIntrinsics2.xml";
cerr << "Attempting to open configuration files" << endl;
FileStorage fs1(fileName1, FileStorage::READ);
FileStorage fs2(fileName2, FileStorage::READ);
Mat cameraMatrix1, cameraMatrix2;
Mat distCoeffs1, distCoeffs2;
fs1["Camera_Matrix"] >> cameraMatrix1;
fs1["Distortion_Coefficients"] >> distCoeffs1;
fs2["Camera_Matrix"] >> cameraMatrix2;
fs2["Distortion_Coefficients"] >> distCoeffs2;
if (cameraMatrix1.data == NULL || distCoeffs1.data == NULL ||
cameraMatrix2.data == NULL || distCoeffs2.data == NULL)
{
cerr << "Could not load camera intrinsics\n" << endl;
}
else{
cerr << "Loaded intrinsics\n" << endl;
cerr << "Camera Matrix1: " << cameraMatrix1 << endl;
cerr << "Camera Matrix2: " << cameraMatrix2 << endl;
}
Mat translation;
Mat image1, image2;
Mat mapX1, mapX2, mapY1, mapY2;
inputCapture1.read(image1);
Size imageSize = image1.size();
bool rotationCalibrated = false;
while(inputCapture1.isOpened() && inputCapture2.isOpened())
{
inputCapture1.read(image1);
inputCapture2.read(image2);
if (rotationCalibrated)
{
Mat t1 = image1.clone();
Mat t2 = image2.clone();
remap(t1, image1, mapX1, mapY1, INTER_LINEAR);
remap(t2, image2, mapX2, mapY2, INTER_LINEAR);
t1.release();
t2.release();
}
char c = waitKey(15);
if (c == 'c')
{
cerr << "Cancelling..." << endl;
return;
}
else if(c == 's' && rotationCalibrated)
{
cerr << "Saving..." << endl;
const string fileName = "EnvironmentCalibration.xml";
FileStorage fs(fileName, FileStorage::WRITE);
fs << "Camera_Matrix_1" << getOptimalNewCameraMatrix(cameraMatrix1, distCoeffs1, imageSize, 1,imageSize, 0);
fs << "Camera_Matrix_2" << getOptimalNewCameraMatrix(cameraMatrix2, distCoeffs2, imageSize, 1, imageSize, 0);
fs << "Mapping_X_1" << mapX1;
fs << "Mapping_Y_1" << mapY1;
fs << "Mapping_X_2" << mapX2;
fs << "Mapping_Y_2" << mapY2;
fs << "Translation" << translation;
cerr << "Exiting..." << endl;
destroyAllWindows();
return;
}
else if(c == 's' && !rotationCalibrated)
{
cerr << "Exiting..." << endl;
destroyAllWindows();
return;
}
else if (c == 'r')
{
BoardSettings s;
s.boardSize.width = BOARD_WIDTH;
s.boardSize.height = BOARD_HEIGHT;
s.cornerNum = s.boardSize.width * s.boardSize.height;
s.squareSize = (float)SQUARE_SIZE;
vector<Point3f> objectPoints;
vector<vector<Point2f> > imagePoints1, imagePoints2;
if (RetrieveChessboardCorners(imagePoints1, imagePoints2, s, inputCapture1, inputCapture2, ITERATIONS))
{
vector<vector<Point3f> > objectPoints(1);
CalcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0]);
objectPoints.resize(imagePoints1.size(),objectPoints[0]);
Mat R, T, E, F;
Mat rmat1, rmat2, rvec;
double rms = stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F,
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 1000, 0.01),
CV_CALIB_FIX_INTRINSIC);
cerr << "Original translation: " << T << endl;
cerr << "Reprojection error reported by camera: " << rms << endl;
// convert to rotation vector and then remove 90 degree offset
Rodrigues(R, rvec);
rvec.at<double>(1,0) -= 1.570796327;
// equal rotation applied to each image...not necessarily needed
rvec = rvec/2;
Rodrigues(rvec, rmat1);
invert(rmat1,rmat2);
initUndistortRectifyMap(cameraMatrix1, distCoeffs1, rmat1,
getOptimalNewCameraMatrix(cameraMatrix1, distCoeffs1, imageSize, 1,imageSize, 0), imageSize, CV_32FC1, mapX1, mapY1);
initUndistortRectifyMap(cameraMatrix2, distCoeffs2, rmat2,
getOptimalNewCameraMatrix(cameraMatrix2, distCoeffs2, imageSize, 1, imageSize, 0), imageSize, CV_32FC1, mapX2, mapY2);
// reproject points in camera 1 since its rotation has been changed
// need to find the translation between cameras based on the new camera 1 orientation
for (int i = 0; i < imagePoints1.size(); i++)
{
Mat pointsMat1 = Mat(imagePoints1[i]);
Mat pointsMat2 = Mat(imagePoints2[i]);
undistortPoints(pointsMat1, imagePoints1[i], cameraMatrix1, distCoeffs1, rmat1,getOptimalNewCameraMatrix(cameraMatrix1, distCoeffs1, imageSize, 1, imageSize, 0));
undistortPoints(pointsMat2, imagePoints2[i], cameraMatrix2, distCoeffs2, rmat2,getOptimalNewCameraMatrix(cameraMatrix2, distCoeffs2, imageSize, 1, imageSize, 0));
pointsMat1.release();
pointsMat2.release();
}
Mat temp1, temp2;
R.release();
T.release();
E.release();
F.release();
// TODO: remove this
// CalcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0]);
// objectPoints.resize(imagePoints1.size(),objectPoints[0]);
stereoCalibrate(objectPoints, imagePoints1, imagePoints2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T, E, F,
TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 1000, 0.01),
CV_CALIB_FIX_INTRINSIC);
// need to alter translation matrix so
// [0] = distance in X direction (right from perspective of camera 1 is positive)
// [1] = distance in Y direction (away from camera 1 is positive)
// [2] = distance in Z direction (up is positive)
translation = T;
double temp = -translation.at<double>(0,0);
translation.at<double>(0,0) = translation.at<double>(2,0);
translation.at<double>(2,0) = temp;
cerr << "Translation reproj: " << translation << endl;
Rodrigues(R, rvec);
cerr << "Reprojected rvec: " << rvec << endl;
imagePoints1.clear();
imagePoints2.clear();
rvec.release();
rmat1.release();
rmat2.release();
R.release();
T.release();
E.release();
F.release();
rotationCalibrated = true;
}
}
imshow("Image View1", image1);
imshow("Image View2", image2);
}
}
// キャリブレーション
__declspec(dllexport) void calcCalibration( double srcPoint2dx[], double srcPoint2dy[], double srcPoint3dx[], double srcPoint3dy[], double srcPoint3dz[], int srcCorrespond, int proWidth, int proHeight, double projectionMatrix[], double externalMatrix[], double& reprojectionResult)
{
cv::Mat point2dx(1, srcCorrespond, CV_64F, srcPoint2dx);
cv::Mat point2dy(1, srcCorrespond, CV_64F, srcPoint2dy);
cv::Mat point3dx(1, srcCorrespond, CV_64F, srcPoint3dx);
cv::Mat point3dy(1, srcCorrespond, CV_64F, srcPoint3dy);
cv::Mat point3dz(1, srcCorrespond, CV_64F, srcPoint3dz);
// 対応点
cv::vector<cv::Point2d> imagePoints;
cv::vector<cv::Point3d> worldPoints;
for(int i = 0; i < srcCorrespond; ++i)
{
cv::Point2d point2d(point2dx.at<double>(i), point2dy.at<double>(i));
cv::Point3d point3d(point3dx.at<double>(i), point3dy.at<double>(i), point3dz.at<double>(i));
imagePoints.push_back(point2d);
worldPoints.push_back(point3d);
}
/////////////////// 透視投影変換行列の推定 /////////////////////////////////
cv::Mat perspectiveMat; // 透視投影変換行列
// 透視投影変換行列の推定
calcProjectionMatrix(worldPoints, imagePoints, perspectiveMat);
// 再投影誤差
reprojectionResult = inspection_error_value(perspectiveMat, worldPoints, imagePoints);
perspectiveMat = perspectiveMat/perspectiveMat.at<double>(11);
cv::Mat cameraMat;
cv::Mat rotation;
cv::Mat translate;
cv::Mat translate2 = cv::Mat::eye(3,1,CV_64F);
cv::Mat worldTranslate = cv::Mat::eye(3,1,CV_64F);
cv::decomposeProjectionMatrix(perspectiveMat, cameraMat, rotation, translate); // 透視投影変換行列の分解→あやしい
// OpenGL用内部パラメータ
cameraMat = -cameraMat / cameraMat.at<double>(8);
double far = 1000.0;
double near = 0.05;
cv::Mat cameraMatrix0 = cv::Mat::zeros(4, 4, CV_64F);
cv::Mat cameraMatrix = cv::Mat::zeros(4, 4, CV_64F);
cameraMatrix0.at<double>(0) = cameraMat.at<double>(0);
cameraMatrix0.at<double>(1) = cameraMat.at<double>(1);
cameraMatrix0.at<double>(2) = cameraMat.at<double>(2);
cameraMatrix0.at<double>(5) = cameraMat.at<double>(4);
cameraMatrix0.at<double>(6) = cameraMat.at<double>(5);
cameraMatrix0.at<double>(10) = -(far+near) / (far-near);
cameraMatrix0.at<double>(11) = -2*far*near / (far-near);
cameraMatrix0.at<double>(14) = cameraMat.at<double>(8);
cv::Mat M = cv::Mat::eye(4, 4, CV_64F);
M.at<double>(0) = 2.0 / (double)proWidth;
M.at<double>(3) = -1;
M.at<double>(5) = -2.0 / (double)proHeight;
M.at<double>(7) = 1;
cameraMatrix = M * cameraMatrix0;
cameraMatrix.at<double>(5) = -cameraMatrix.at<double>(5);
// 外部パラメータ
translate2.at<double>(0) = translate.at<double>(0)/translate.at<double>(3);
translate2.at<double>(1) = translate.at<double>(1)/translate.at<double>(3);
translate2.at<double>(2) = translate.at<double>(2)/translate.at<double>(3);
wornldTranslate = rotation * translate2; //decomposeProjectionMatrix()のせい？
//decomposeProjectionMatrix()のせいで-色々つけてる？
cv::Mat externalMat = cv::Mat::eye(4,4,CV_64F);
externalMat.at<double>(0) = rotation.at<double>(0);
externalMat.at<double>(1) = rotation.at<double>(1);
externalMat.at<double>(2) = rotation.at<double>(2);
externalMat.at<double>(3) = -worldTranslate.at<double>(0);
externalMat.at<double>(4) = -rotation.at<double>(3);
externalMat.at<double>(5) = -rotation.at<double>(4);
externalMat.at<double>(6) = -rotation.at<double>(5);
externalMat.at<double>(7) = worldTranslate.at<double>(1);
externalMat.at<double>(8) = rotation.at<double>(6);
externalMat.at<double>(9) = rotation.at<double>(7);
externalMat.at<double>(10) = rotation.at<double>(8);
externalMat.at<double>(11) = -worldTranslate.at<double>(2);
// 結果の保存
cv::FileStorage fs2("Calibration/calibration.xml", cv::FileStorage::WRITE);
cv::write(fs2,"reprojectionError", reprojectionResult);
cv::write(fs2,"projectorCalibration", perspectiveMat);
cv::write(fs2,"internalMatrix", cameraMat);
cv::write(fs2,"cameraMatrix", cameraMatrix);
cv::write(fs2,"externalMatrix", externalMat);
for(int i = 0; i < 16; ++i)
{
projectionMatrix[i] = cameraMatrix.at<double>(i);
externalMatrix[i] = externalMat.at<double>(i);
}
}
int Depth_and_Disparity::stereo_match_and_disparity_init(int argc, char** argv, Size img_size)
{
if(argc < 3)
{
return 0;
}
//alg = STEREO_SGBM;
alg = STEREO_BM;
desired_param_set = 3; // called at the end of this function.
minDisparityToCut = 35; // for the threshold cut //default here. runover later in ParamFunction.
SADWindowSize = 0;
numberOfDisparities = 0;
no_display = false;
scale = 1.f;
target_image_size = img_size;
last_disparity_depth= 0;
for( int i = 1+2; i < argc; i++ )
{
//if( argv[i][0] != '-' )
//{
// /* if( !img1_filename )
// img1_filename = argv[i];
// else
// img2_filename = argv[i];*/
//}
//else
if( strncmp(argv[i], algorithm_opt, strlen(algorithm_opt)) == 0 )
{
char* _alg = argv[i] + strlen(algorithm_opt);
alg = strcmp(_alg, "bm") == 0 ? STEREO_BM :
strcmp(_alg, "sgbm") == 0 ? STEREO_SGBM :
strcmp(_alg, "hh") == 0 ? STEREO_HH :
strcmp(_alg, "var") == 0 ? STEREO_VAR : -1;
if( alg < 0 )
{
printf("Command-line parameter error: Unknown stereo algorithm\n\n");
//print_help();
return -1;
}
}
else if( strncmp(argv[i], maxdisp_opt, strlen(maxdisp_opt)) == 0 )
{
if( sscanf( argv[i] + strlen(maxdisp_opt), "%d", &numberOfDisparities ) != 1 ||
numberOfDisparities < 1 || numberOfDisparities % 16 != 0 )
{
printf("Command-line parameter error: The max disparity (--maxdisparity=<...>) must be a positive integer divisible by 16\n");
//print_help();
return -1;
}
}
else if( strncmp(argv[i], blocksize_opt, strlen(blocksize_opt)) == 0 )
{
if( sscanf( argv[i] + strlen(blocksize_opt), "%d", &SADWindowSize ) != 1 ||
SADWindowSize < 1 || SADWindowSize % 2 != 1 )
{
printf("Command-line parameter error: The block size (--blocksize=<...>) must be a positive odd number\n");
return -1;
}
}
else if( strncmp(argv[i], scale_opt, strlen(scale_opt)) == 0 )
{
if( sscanf( argv[i] + strlen(scale_opt), "%f", &scale ) != 1 || scale < 0 )
{
printf("Command-line parameter error: The scale factor (--scale=<...>) must be a positive floating-point number\n");
return -1;
}
}
else if( strcmp(argv[i], nodisplay_opt) == 0 )
no_display = true;
else if( strcmp(argv[i], "-i" ) == 0 )
intrinsic_filename = argv[++i];
else if( strcmp(argv[i], "-e" ) == 0 )
extrinsic_filename = argv[++i];
else if( strcmp(argv[i], "-o" ) == 0 )
disparity_filename = argv[++i];
else if( strcmp(argv[i], "-p" ) == 0 )
point_cloud_filename = argv[++i];
else
{
printf("Command-line parameter error: unknown option %d %s\n", i, argv[i]);
//return -1;
}
}
if( (intrinsic_filename != 0) ^ (extrinsic_filename != 0) )
{
printf("Command-line parameter error: either both intrinsic and extrinsic parameters must be specified, or none of them (when the stereo pair is already rectified)\n");
return -1;
}
if( extrinsic_filename == 0 && point_cloud_filename )
{
printf("Command-line parameter error: extrinsic and intrinsic parameters must be specified to compute the point cloud\n");
return -1;
}
if( intrinsic_filename )
{
// reading intrinsic parameters
FileStorage fs(intrinsic_filename, FileStorage::READ);
if(!fs.isOpened())
{
printf("Failed to open file %s\n", intrinsic_filename);
return -1;
}
fs["M1"] >> M1;
fs["D1"] >> D1;
fs["M2"] >> M2;
fs["D2"] >> D2;
M1 *= scale;
M2 *= scale;
fs.release();
FileStorage fs2(extrinsic_filename, FileStorage::READ);
///fs2.open(extrinsic_filename, FileStorage::READ);
if(!fs2.isOpened())
{
printf("Failed to open file %s\n", extrinsic_filename);
return -1;
}
fs2["R"] >> R;
fs2["T"] >> T;
fs2.release();
/* initialize rectification mapping */
/*
when calibrated and saving matrices - i calibrated
Left camera as img1 ,
Right camera as img2
*/
Size img_ORG_size = Size(320,240); // the images size when calibrated the cameras
stereoRectify( M1, D1, M2, D2, img_ORG_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );
initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map11, map12);
initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map21, map22);
}
//// set algorithm and parameters :
if (alg == STEREO_SGBM)
{
if (desired_param_set==1)
set_SGBM_params_options_1();
else
set_SGBM_params_options_2();
}
else if (alg == STEREO_BM)
{
switch (desired_param_set)
{
case 1:
set_BM_params_options_1(); break;
case 2:
set_BM_params_options_2(); break;
case 3:
set_BM_params_options_3(); break;
default:
break;
}
}
else ; //ERROR about alg type
return 0;
}
void TestStores4() {
B(s30, Memo storage, 0)W(s30a);
{
c4_Bytes hi("hi", 2);
c4_Bytes gday("gday", 4);
c4_Bytes hello("hello", 5);
c4_MemoProp p1("p1");
c4_Storage s1("s30a", 1);
s1.SetStructure("a[p1:B]");
c4_View v1 = s1.View("a");
v1.Add(p1[hi]);
A(p1(v1[0]) == hi);
v1.Add(p1[hello]);
A(p1(v1[0]) == hi);
A(p1(v1[1]) == hello);
v1.InsertAt(1, p1[gday]);
A(p1(v1[0]) == hi);
A(p1(v1[1]) == gday);
A(p1(v1[2]) == hello);
s1.Commit();
A(p1(v1[0]) == hi);
A(p1(v1[1]) == gday);
A(p1(v1[2]) == hello);
}
D(s30a);
R(s30a);
E;
// this failed in the unbuffered 1.8.5a interim release in Mk4tcl 1.0.5
B(s31, Check sort buffer use, 0)W(s31a);
{
c4_IntProp p1("p1");
c4_Storage s1("s31a", 1);
s1.SetStructure("a[p1:I]");
c4_View v1 = s1.View("a");
v1.Add(p1[3]);
v1.Add(p1[1]);
v1.Add(p1[2]);
s1.Commit();
c4_View v2 = v1.SortOn(p1);
A(v2.GetSize() == 3);
A(p1(v2[0]) == 1);
A(p1(v2[1]) == 2);
A(p1(v2[2]) == 3);
}
D(s31a);
R(s31a);
E;
// this failed in 1.8.6, fixed 19990828
B(s32, Set memo empty or same size, 0)W(s32a);
{
c4_Bytes empty;
c4_Bytes full("full", 4);
c4_Bytes more("more", 4);
c4_MemoProp p1("p1");
c4_Storage s1("s32a", 1);
s1.SetStructure("a[p1:B]");
c4_View v1 = s1.View("a");
v1.Add(p1[full]);
A(p1(v1[0]) == full);
s1.Commit();
A(p1(v1[0]) == full);
p1(v1[0]) = empty;
A(p1(v1[0]) == empty);
s1.Commit();
A(p1(v1[0]) == empty);
p1(v1[0]) = more;
A(p1(v1[0]) == more);
s1.Commit();
A(p1(v1[0]) == more);
p1(v1[0]) = full;
A(p1(v1[0]) == full);
s1.Commit();
A(p1(v1[0]) == full);
}
D(s32a);
R(s32a);
E;
// this failed in 1.8.6, fixed 19990828
B(s33, Serialize memo fields, 0)W(s33a);
W(s33b);
W(s33c);
{
c4_Bytes hi("hi", 2);
c4_Bytes gday("gday", 4);
c4_Bytes hello("hello", 5);
c4_MemoProp p1("p1");
c4_Storage s1("s33a", 1);
s1.SetStructure("a[p1:B]");
c4_View v1 = s1.View("a");
v1.Add(p1[hi]);
v1.Add(p1[gday]);
v1.Add(p1[hello]);
A(p1(v1[0]) == hi);
A(p1(v1[1]) == gday);
A(p1(v1[2]) == hello);
s1.Commit();
A(p1(v1[0]) == hi);
A(p1(v1[1]) == gday);
A(p1(v1[2]) == hello);
{
c4_FileStream fs1(fopen("s33b", "wb"), true);
s1.SaveTo(fs1);
}
c4_Storage s2("s33c", 1);
c4_FileStream fs2(fopen("s33b", "rb"), true);
s2.LoadFrom(fs2);
c4_View v2 = s2.View("a");
A(p1(v2[0]) == hi);
A(p1(v2[1]) == gday);
A(p1(v2[2]) == hello);
s2.Commit();
A(p1(v2[0]) == hi);
A(p1(v2[1]) == gday);
A(p1(v2[2]) == hello);
s2.Commit();
A(p1(v2[0]) == hi);
A(p1(v2[1]) == gday);
A(p1(v2[2]) == hello);
}
D(s33a);
D(s33b);
D(s33c);
R(s33a);
R(s33b);
R(s33c);
E;
// check smarter commit and commit failure on r/o
B(s34, Smart and failed commits, 0)W(s34a);
{
c4_IntProp p1("p1");
{
c4_Storage s1("s34a", 1);
s1.SetStructure("a[p1:I]");
c4_View v1 = s1.View("a");
v1.Add(p1[111]);
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
bool f1 = s1.Commit();
A(f1);
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
bool f2 = s1.Commit();
A(f2); // succeeds, but should not write anything
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
}
{
c4_Storage s1("s34a", 0);
c4_View v1 = s1.View("a");
v1.Add(p1[222]);
A(v1.GetSize() == 2);
A(p1(v1[0]) == 111);
A(p1(v1[1]) == 222);
bool f1 = s1.Commit();
A(!f1);
A(v1.GetSize() == 2);
A(p1(v1[0]) == 111);
A(p1(v1[1]) == 222);
}
}
D(s34a);
R(s34a);
E;
B(s35, Datafile with preamble, 0)W(s35a);
{
{
c4_FileStream fs1(fopen("s35a", "wb"), true);
fs1.Write("abc", 3);
}
c4_IntProp p1("p1");
{
c4_Storage s1("s35a", 1);
s1.SetStructure("a[p1:I]");
c4_View v1 = s1.View("a");
v1.Add(p1[111]);
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
bool f1 = s1.Commit();
A(f1);
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
bool f2 = s1.Commit();
A(f2); // succeeds, but should not write anything
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
}
{
c4_FileStream fs1(fopen("s35a", "rb"), true);
char buffer[10];
int n1 = fs1.Read(buffer, 3);
A(n1 == 3);
A(c4_String(buffer, 3) == "abc");
}
{
c4_Storage s1("s35a", 0);
c4_View v1 = s1.View("a");
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
v1.Add(p1[222]);
A(v1.GetSize() == 2);
A(p1(v1[0]) == 111);
A(p1(v1[1]) == 222);
bool f1 = s1.Commit();
A(!f1);
A(v1.GetSize() == 2);
A(p1(v1[0]) == 111);
A(p1(v1[1]) == 222);
}
}
D(s35a);
R(s35a);
E;
B(s36, Commit after load, 0)W(s36a);
W(s36b);
{
c4_IntProp p1("p1");
c4_Storage s1("s36a", 1);
s1.SetStructure("a[p1:I]");
c4_View v1 = s1.View("a");
v1.Add(p1[111]);
A(v1.GetSize() == 1);
A(p1(v1[0]) == 111);
{
c4_FileStream fs1(fopen("s36b", "wb"), true);
s1.SaveTo(fs1);
}
p1(v1[0]) = 222;
v1.Add(p1[333]);
bool f1 = s1.Commit();
A(f1);
A(v1.GetSize() == 2);
A(p1(v1[0]) == 222);
A(p1(v1[1]) == 333);
c4_FileStream fs2(fopen("s36b", "rb"), true);
s1.LoadFrom(fs2);
//A(v1.GetSize() == 0); // should be detached, but it's still 2
c4_View v2 = s1.View("a");
A(v2.GetSize() == 1);
A(p1(v2[0]) == 111);
// this fails in 2.4.0, reported by James Lupo, August 2001
bool f2 = s1.Commit();
A(f2);
}
D(s36a);
D(s36b);
R(s36a);
R(s36b);
E;
// fails in 2.4.1, reported Oct 31. 2001 by Steve Baxter
B(s37, Change short partial fields, 0)W(s37a);
{
c4_BytesProp p1("p1");
c4_Storage s1("s37a", true);
c4_View v1 = s1.GetAs("v1[key:I,p1:B]");
v1.Add(p1[c4_Bytes("12345", 6)]);
A(v1.GetSize() == 1);
s1.Commit();
c4_Bytes buf = p1(v1[0]);
A(buf.Size() == 6);
A(buf == c4_Bytes("12345", 6));
buf = p1(v1[0]).Access(1, 3);
A(buf == c4_Bytes("234", 3));
p1(v1[0]).Modify(c4_Bytes("ab", 2), 2, 0);
s1.Commit();
buf = p1(v1[0]);
A(buf == c4_Bytes("12ab5", 6));
}
D(s37a);
R(s37a);
E;
// Gross memory use (but no leaks), January 2002, Murat Berk
B(s38, Lots of empty subviews, 0)W(s38a);
{
c4_BytesProp p1("p1");
{
c4_Storage s1("s38a", true);
c4_View v = s1.GetAs("v[v1[p1:S]]");
v.SetSize(100000);
s1.Commit();
}
{
c4_Storage s2("s38a", true);
c4_View v2 = s2.View("v");
// this should not materialize all the empty subviews
v2.SetSize(v2.GetSize() + 1);
// nor should this
s2.Commit();
}
{
c4_Storage s3("s38a", true);
c4_View v3 = s3.View("v");
v3.RemoveAt(1, v3.GetSize() - 2);
A(v3.GetSize() == 2);
s3.Commit();
}
}
D(s38a);
R(s38a);
E;
// Fix bug introduced on 7-2-2002, as reported by M. Berk
B(s39, Do not detach empty top-level views, 0)W(s39a);
{
c4_IntProp p1("p1");
c4_Storage s1("s39a", true);
c4_View v1 = s1.GetAs("v1[p1:I]");
s1.Commit();
A(v1.GetSize() == 0);
v1.Add(p1[123]);
A(v1.GetSize() == 1);
s1.Commit();
c4_View v2 = s1.View("v1");
A(v2.GetSize() == 1); // fails with 0 due to recent bug
}
D(s39a);
R(s39a);
E;
}
int main(int argc, char **argv){
if(argc<3){
std::cout<<"Need to specify the calibration and plane files"<<std::endl;
return 1;
}
//reating the disparity map for the empty truck
cv::Mat img1,img2, disparity;
TwinCamera twin(0,1);
if(argc!=5){
twin.getDoubleImages(img1,img2);
}else{
img1 = cv::imread(argv[3]);
img2 = cv::imread(argv[4]);
}
twin.loadCameraParameters(argv[1], img1, img2);
StereoDepth stereoDepth;
twin.rectifyForStereo(img1, img2);
stereoDepth.setImage1(img1);
stereoDepth.setImage2(img2);
if(stereoDepth.doDepth()){
disparity = stereoDepth.getDisparity();
imwrite("left.png",img1);
imwrite("right.png",img2);
imwrite("diparity.png", disparity);
}else{
std::cout<<"Error when computing the stereo image."<<std::endl;
return 1;
}
//creating the computePlanes object with 5 planes to calculate
pclGetPlanes computePlanes(5);
pclView viewtest;
pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr points_full;
cv::Mat Q;
cv::FileStorage fs(argv[1], cv::FileStorage::READ);
fs["Q"] >> Q;
fs.release();
cv::FileStorage fs2(argv[2], cv::FileStorage::READ);
std::vector<double> left_plane;
std::vector<double> right_plane;
std::vector<double> top_plane;
std::vector<double> bottom_plane;
std::vector<double> back_plane;
for(int i=0; i<4;i++){
double val;
char c = (i+'a');
fs2[std::string("left_plane_").append(1u,c)] >> val;
left_plane.push_back(val);
fs2[std::string("right_plane_").append(1u,c)] >> val;
right_plane.push_back(val);
fs2[std::string("top_plane_").append(1u,c)] >> val;
top_plane.push_back(val);
fs2[std::string("bottom_plane_").append(1u,c)] >> val;
bottom_plane.push_back(val);
fs2[std::string("back_plane_").append(1u,c)] >> val;
back_plane.push_back(val);
}
try{
computePlanes.setCoeficientsForIndex(left_plane, pclGetPlanes::LEFT_CUT);
computePlanes.setCoeficientsForIndex(right_plane, pclGetPlanes::RIGHT_CUT);
computePlanes.setCoeficientsForIndex(top_plane, pclGetPlanes::TOP_CUT);
computePlanes.setCoeficientsForIndex(bottom_plane, pclGetPlanes::BOTTOM_CUT);
computePlanes.setCoeficientsForIndex(back_plane, pclGetPlanes::BACK_CUT);
}catch(std::runtime_error &exp){
std::cerr<<exp.what()<<std::endl;
return 0;
}
points_full = viewtest.convertToPointCloudNoColor(disparity, Q);
for(int i=0; i<points_full->points.size(); i++){
if( (computePlanes.isOnPlane(points_full->points[i], pclGetPlanes::LEFT_CUT)) ||
(computePlanes.isOnPlane(points_full->points[i], pclGetPlanes::RIGHT_CUT)) ||
(computePlanes.isOnPlane(points_full->points[i], pclGetPlanes::TOP_CUT)) ||
(computePlanes.isOnPlane(points_full->points[i], pclGetPlanes::BACK_CUT)) ||
(computePlanes.isOnPlane(points_full->points[i], pclGetPlanes::BOTTOM_CUT))) {
continue;
}
points->points.push_back(points_full->points[i]);
}
if(points->points.size() == 0){
std::cout<<"Percentage: 0.0%"<<std::endl;
}
std::cout<<"Valid points: "<<points->points.size()<<std::endl;
double total_occupied_volume = 0.0;
pcl::PointXYZ origin;
origin.x=origin.y=origin.z=0;
double back_plane_distance = computePlanes.distanceFromPlane(origin, pclGetPlanes::BACK_CUT);
for(int i=0; i<points->points.size(); i++){
total_occupied_volume += computePlanes.distanceFromPlane(points->points[i], pclGetPlanes::BACK_CUT)/back_plane_distance;
}
std::cout<<"Total occupied volume(voxels): "<<total_occupied_volume <<std::endl;
std::cout<<"Total occupied volume(cm^3): "<<(430*total_occupied_volume) <<std::endl;
std::cout<<"Percentage: "<<(total_occupied_volume/(640*480) *100)<<"%"<<std::endl;
return 0;
}
int main()
{
std::pair<std::string, std::string> temp_files = get_temp_file_names();
std::string& temp1 = temp_files.first;
std::string& temp2 = temp_files.second;
assert(temp1 != temp2);
{
std::ofstream fs1(temp1.c_str());
std::ofstream fs2(temp2.c_str());
fs1 << 3.25;
fs2 << 4.5;
swap(fs1, fs2);
fs1 << ' ' << 3.25;
fs2 << ' ' << 4.5;
}
{
std::ifstream fs(temp1.c_str());
double x = 0;
fs >> x;
assert(x == 3.25);
fs >> x;
assert(x == 4.5);
}
std::remove(temp1.c_str());
{
std::ifstream fs(temp2.c_str());
double x = 0;
fs >> x;
assert(x == 4.5);
fs >> x;
assert(x == 3.25);
}
std::remove(temp2.c_str());
{
std::wofstream fs1(temp1.c_str());
std::wofstream fs2(temp2.c_str());
fs1 << 3.25;
fs2 << 4.5;
swap(fs1, fs2);
fs1 << ' ' << 3.25;
fs2 << ' ' << 4.5;
}
{
std::wifstream fs(temp1.c_str());
double x = 0;
fs >> x;
assert(x == 3.25);
fs >> x;
assert(x == 4.5);
}
std::remove(temp1.c_str());
{
std::wifstream fs(temp2.c_str());
double x = 0;
fs >> x;
assert(x == 4.5);
fs >> x;
assert(x == 3.25);
}
std::remove(temp2.c_str());
}
nmethod* SICompiler::compile() {
EventMarker em("SIC-compiling %#lx %#lx", L->selector(), NULL);
ShowCompileInMonitor sc(L->selector(), "SIC", recompilee != NULL);
// cannot recompile uncommon branches in DI nmethods & top nmethod yet
FlagSetting fs2(SICDeferUncommonBranches,
SICDeferUncommonBranches &&
diLink == NULL && L->adeps->length() == 0 &&
L->selector() != VMString[DO_IT]);
// don't use uncommon traps when recompiling because of trap
useUncommonTraps =
SICDeferUncommonBranches && !currentProcess->isUncommon();
// don't inline into doIt
FlagSetting fs3(Inline, Inline && L->selector() != VMString[DO_IT]);
# if TARGET_ARCH != I386_ARCH // no FastMapTest possible on I386
// don't use fast map loads if this nmethod trapped a lot
FlagSetting fs4(FastMapTest, FastMapTest &&
(recompilee == NULL ||
recompilee->flags.trapCount < MapLoadTrapLimit));
# endif
FlagSetting fs5(PrintCompilation, PrintCompilation || PrintSICCompilation);
timer t;
FlagSetting fs6(verifyOften, SICDebug || CheckAssertions);
if(PrintCompilation || PrintLongCompilation ||
PrintCompilationStatistics || VMSICLongProfiling) {
t.start();
}
if (PrintCompilation || PrintSICCode) {
lprintf("*SIC-%s%scompiling %s%s: (SICCompilationCount=%d)",
currentProcess->isUncommon() ? "uncommon-" : "",
recompilee ? "re" : "",
sprintName( (methodMap*) method()->map(), L->selector()),
sprintValueMethod( L->receiver ),
(void*)SICCompilationCount);
}
topScope->genCode();
buildBBs();
if (verifyOften) bbIterator->verify(false);
bbIterator->eliminateUnreachableNodes(); // needed for removeUptoMerge to work
// compute exposed blocks and up-level accessed vars
bbIterator->computeExposedBlocks();
bbIterator->computeUplevelAccesses();
// make defs & uses and insert flush nodes for uplevel-accessed vars
bbIterator->makeUses();
// added verify here cause want to catch unreachable merge preds
// before elimination -- dmu
if (verifyOften) bbIterator->verify();
if (SICLocalCopyPropagate) {
bbIterator->localCopyPropagate();
if (verifyOften) bbIterator->verify();
}
if (SICGlobalCopyPropagate) {
bbIterator->globalCopyPropagate();
if (verifyOften) bbIterator->verify();
}
if (SICEliminateUnneededNodes) {
bbIterator->eliminateUnneededResults();
if (verifyOften) bbIterator->verify();
}
// do after CP to explot common type test source regs
if (SICOptimizeTypeTests) {
bbIterator->computeDominators();
bbIterator->optimizeTypeTests();
if (verifyOften) bbIterator->verify();
}
// allocate the temp (i.e. volatile) registers
bbIterator->allocateTempRegisters();
// allocate the callee-saved (i.e. non-volatile) registers
SICAllocator* a = theAllocator;
a->allocate(bbIterator->globals, topScope->incoming);
stackLocCount = a->stackTemps;
// make sure frame size is aligned properly
int32 frame_size_so_far = frameSize();
stackLocCount += roundTo(frame_size_so_far, frame_word_alignment) - frame_size_so_far;
// compute the register masks for inline caches
bbIterator->computeMasks(stackLocCount, nonRegisterArgCount());
topScope->computeMasks(regStringToMask(topScope->incoming),
stackLocCount, nonRegisterArgCount());
if (PrintSICCode) {
print_code(false);
lprintf("\n\n");
}
topScope->describe(); // must come before gen to set scopeInfo
genHelper = new SICGenHelper;
bbIterator->gen();
assert(theAssembler->verifyLabels(), "undefined labels");
rec->generate();
topScope->fixupBlocks(); // must be after rec->gen to know offsets
if (vscopes) computeMarkers(); // ditto
nmethod* nm = new_nmethod(this, false);
if (theAssembler->lastBackpatch >= theAssembler->instsEnd)
fatal("dangling branch");
em.event.args[1] = nm;
fint ms = IntervalTimer::dont_use_any_timer ? 0 : t.millisecs();
if (PrintCompilation || PrintLongCompilation) {
if (!PrintCompilation && PrintLongCompilation && ms >= MaxCompilePause) {
lprintf("*SIC-%s%scompiling ",
currentProcess->isUncommon() ? "uncommon-" : "",
recompilee ? "re" : "");
methodMap* mm = method() ? (methodMap*) method()->map() : NULL;
printName(mm, L->selector());
lprintf(": %#lx (%ld ms; level %ld)\n", nm, (void*)ms, (void*)nm->level());
} else if (PrintCompilation) {
lprintf(": %#lx (%ld ms; level %ld v%d)\n", (void*)nm, (void*)ms,
(void*)nm->level(), (void*)nm->version());
}
}
if (SICDebug && estimatedSize() > inlineLimit[NmInstrLimit]) {
float rat = (float)estimatedSize() / (float)nm->instsLen();
lprintf("*est. size = %ld, true size = %ld, ratio = %4.2f\n",
(void*)estimatedSize(), (void*)nm->instsLen(),
*(void**)&rat);
}
if (PrintCompilationStatistics) {
static fint counter = 0;
lprintf("\n*SIC-time= |%ld| ms; to/co/sc/lo/de= |%ld|%ld|%ld|%ld|%ld| %ld|%ld|%ld| %ld |",
(void*)ms,
(void*) (nm->instsLen() + nm->scopes->length() +
nm->locsLen() + nm->depsLen),
(void*)nm->instsLen(),
(void*)nm->scopes->length(),
(void*)nm->locsLen(),
(void*)nm->depsLen,
(void*)BasicNode::currentID,
(void*)bbIterator->bbCount,
(void*)ncodes,
(void*)counter++);
}
# if GENERATE_DEBUGGING_AIDS
if (CheckAssertions) {
// nm->verify();
}
# endif
return nm;
}
void testVocCreation(const BoWFeatures &features,const BoWFeatures &featuresrgb)
{
// branching factor and depth levels
const int k = 10;
const int L = VOC;
const ScoringType score = L1_NORM;
const WeightingType weight = TF_IDF;
NarfVocabulary voc;
Surf128Vocabulary voc2;
string nomefile_3d = filename_voc_3d;
string nomefile_rgb = filename_voc_rgb;
cv::FileStorage fs(nomefile_3d.c_str(), cv::FileStorage::READ);
cv::FileStorage fs2(nomefile_rgb.c_str(), cv::FileStorage::READ);
if ((!fs.isOpened()) || (!fs2.isOpened()))
{
NarfVocabulary voc(k, L, weight, score);
BoWFeatures tmp,tmp2;
for(int yy=0; yy<features.size(); yy+=3){
tmp.push_back(features[yy]);
}
for(int yy=0; yy<featuresrgb.size(); yy+=3){
tmp2.push_back(featuresrgb[yy]);
}
voc.create(tmp);
Surf128Vocabulary voc2(k, L, weight, score);
voc2.create(tmp2);
cout << "Creazione " << k << "^" << L << " vocabolario 3D terminata." << endl;
cout << "Vocabolario 3D: " << endl << voc << endl << endl;
cout << "Creazione " << k << "^" << L << " vocabolario RGB terminata." << endl;
cout << "Vocabolario RGB: " << endl << voc2 << endl << endl;
voc.save(nomefile_3d);
voc2.save(nomefile_rgb);
}
else
{
if(flag_loop || flag_l3d){
voc.load(nomefile_3d);
cout << "Vocabolario 3D: " << endl << voc << endl << endl;
}
if(flag_loop || flag_lrgb){
voc2.load(nomefile_rgb);
cout << "Vocabolario RGB: " << endl << voc2 << endl << endl;
}
}
// BowVector v1, v2;
// ofstream bown("bow.txt");
// const static int taglia = (pow(k,L)*2) + 1; //il +1 si riferisce all'etichetta del luogo.
// const static int offset = taglia/2;
// for(int i ; i < files_list_3d.size(); i++)
// {
// voc.transform(features[i], v1);
// voc2.transform(featuresrgb[i], v2);
// double bbow[taglia];
// for(int jay = 0; jay < taglia;jay++){
// bbow[jay] = 0;
// }
// //RGB
// for(BowVector::iterator vit = v2.begin(); vit != v2.end(); vit++){
// int pivot = vit->first;
// bbow[pivot] = vit->second;
// }
// //3D
// for(BowVector::iterator vit = v1.begin(); vit != v1.end(); vit++){
// int pivot = offset + vit->first;
// bbow[pivot] = vit->second;
// }
// //inserimento etichetta luogo per generare il file che verrà inviato all'apprendimento.
// string luogo = registro_aux.at(i);
// vector<string> a;
// boost::split(a, registro_aux.at(i), boost::is_any_of("=>"));
// a.erase( remove_if( a.begin(), a.end(), boost::bind( & string::empty, _1 ) ), a.end());
// StringFunctions::trim(a[1]);
// bown << a[1] << ",";
// for(int jay = 0; jay < taglia;jay++){
// bown << bbow[jay];
// if (jay < taglia - 1){ bown << ","; }
// }
// bown << endl;
// }
// bown.close();
}
bool ShapeMatch::importThresholds() {
if(!ShapeMatch::THRESH_IMPORTED) {
String folderName = "Thresholds/";
String filename = folderName+"shape_shifting_rules.csv";
String filename2 = folderName+"shape_penalties.csv";
String filename3 = folderName+"shape_weights.csv";
assert(fs::exists(filename)==true);
assert(fs::exists(filename2)==true);
assert(fs::exists(filename3)==true);
fstream fs(filename);
fstream fs2(filename2);
fstream fs3(filename3);
if(fs.is_open() && fs2.is_open() && fs3.is_open()) {
String temp;
vector<String> vec;
vector<String> vec2;
vector<float> vec3;
while(getline(fs,temp)) {
ip::getSubstr(temp,',',vec);
for(unsigned int i=0; i<vec.size(); i++) {
if(vec.at(i)!="") {
if(i>0) {
vec2.push_back(vec.at(i));
}
}
}
ShapeMatch::shiftingRules.push_back(vec2);
vec2.clear();
}
getline(fs2,temp);
while(getline(fs2,temp)) {
ip::getSubstr(temp,',',vec);
for(unsigned int i=0; i<vec.size(); i++) {
if(i>0) {
vec3.push_back(atof(vec.at(i).c_str()));
}
}
ShapeMatch::shiftingPenalties.push_back(vec3);
vec3.clear();
}
getline(fs3,temp);
while(getline(fs3,temp)) {
ip::getSubstr(temp,',',vec);
ShapeMatch::shapeWeightsVec.push_back(atof(vec.at(1).c_str()));
ShapeMatch::shapeWeightsVec2.push_back(atof(vec.at(2).c_str()));
}
assert(ShapeMatch::shapeNames.size()==ShapeMatch::shiftingPenalties.size());
assert(ShapeMatch::shapeNames.size()==ShapeMatch::shiftingRules.size());
assert(ShapeMatch::shapeNames.size()==ShapeMatch::shapeWeightsVec.size());
assert(ShapeMatch::shapeNames.size()==ShapeMatch::shapeWeightsVec2.size());
fs.close();
fs2.close();
fs3.close();
return true;
} else {
cout << "Importing ShapeMatch Thresholds failed!" << endl;
return false;
}
}
return true;
}
