int main(int argc, char *argv[])
{
int err = 0;
int i, j;
double *vecA, *matB, *vecC;
double s_time,e_time;
// Get cmndln args exit if error occurs
if((err = getArgs(argc, argv)))
errExit("Exiting app");
//Args.m = 3;
// Args.n = 3;
// Args.verbose = 1;
// Allocate memory for the matrix and vectors
if(! (vecA = (double*) malloc(Args.m * sizeof(double))))
errExit("Error: Failed to allocate memory for vecA\n");
if(! (matB = (double*) malloc(Args.m * Args.n * sizeof(double))))
errExit("Error: Failed to allocate memory for matB\n");
if(! (vecC = (double*) malloc(Args.n * sizeof(double))))
errExit("Error: Failed to allocate memory for vecC\n");
// Seed the random number generator (This provides a repeatable random series)
srand(1);
// Init matB as an array with random values between 1 and 10
for(i=0; i < Args.m; i++)
{
for(j=0; j < Args.n; j++)
matB[i * Args.n + j] = RNDRANGE(10.0);
}
// Init vecC as our vector which multiples matB*2
for(i=0; i < Args.n; i++)
vecC[i] = RNDRANGE(10.0);
// Perform the matrix-vector multiply
s_time=omp_get_wtime();
matvectMultiply(vecA, matB, vecC);
e_time=omp_get_wtime();
// If verbose then display the matrices
if(Args.verbose)
{
outputMat("Matrix B:", matB);
outputVec("Vector C:", vecC);
outputVec("Resultant Vector A:", vecA);
}
printf("%d %f\n",omp_get_max_threads(),e_time-s_time);
// Free memory
if(vecA) free(vecA);
if(matB) free(matB);
if(vecC) free(vecC);
//system("pause");
return err;
}
//--------------------------------------------------------------
void testApp::calibrate(){
ofxCv::Calibration cam1CalibBase, cam2CalibBase, cam1CalibRefined, cam2CalibRefined;
cam1CalibBase.load("depthCalibBase.yml");
cam2CalibBase.load("depthCalibBase.yml");
ofxRGBDepthCalibration calibrator;
// calibrator.refineDepthCalibration(cam1CalibBase, cam1CalibRefined, &front);
// calibrator.refineDepthCalibration(cam2CalibBase, cam2CalibRefined, &back);
vector<cv::Point2f> imagePoints1,imagePoints2;
for(int i = 0; i < 4; i++){
imagePoints1.push_back(ofxCv::toCv(cam1Calib[i]));
imagePoints2.push_back(ofxCv::toCv(cam2Calib[i]));
}
cout << "START CAM CALIB IS " << cam1Calib[0] << endl;
// vector<cv::Point2f> ; = ofxCv::toCv(cam2Calib);
vector<cv::Point3f> objectPoints1;
vector<cv::Point3f> objectPoints2;
cout << "object points " << endl;
for(int i = 0; i < 4; i++){
objectPoints1.push_back( ofxCv::toCv( front.getWorldCoordinateAt(imagePoints1[i].x, imagePoints1[i].y) ));
objectPoints2.push_back( ofxCv::toCv( back.getWorldCoordinateAt(imagePoints2[i].x, imagePoints2[i].y) ));
cout << " " << objectPoints1[i] << " vs " << objectPoints2[i] << endl;
}
cv::Mat camMatrix1,camMatrix2;
cv::Mat distCoeff1,distCoeff2;
cam1CalibRefined.getDistortedIntrinsics().getCameraMatrix().copyTo( camMatrix1 );
cam2CalibRefined.getDistortedIntrinsics().getCameraMatrix().copyTo( camMatrix2 );
cam1CalibRefined.getDistCoeffs().copyTo(distCoeff1);
cam2CalibRefined.getDistCoeffs().copyTo(distCoeff2);
//cv::Mat affine3d;
//cv::Mat inliers;
cv::Mat outputMat(3,4,CV_64F);
vector<uchar> inliers;
cv::estimateAffine3D(objectPoints1, objectPoints2, outputMat, inliers);
double* af = outputMat.ptr<double>(0);
calibrationMatrix = ofMatrix4x4(af[0], af[4], af[8], 0.0f,
af[1], af[5], af[9], 0.0f,
af[2], af[6], af[10], 0.0f,
af[3], af[7], af[11], 1.0f);
// calibrationMatrix = ofMatrix4x4(af[0], af[1], af[2], 0.0f,
// af[4], af[5], af[6], 0.0f,
// af[8], af[9], af[10], 0.0f,
// af[3], af[7], af[11], 1.0f);
// calibrationMatrix = ofMatrix4x4(1, 0, 0, 0.0f,
// 0, 1, 0, 0.0f,
// 0, 0, 1, 0.0f,
// af[3], af[7], af[11], 1.0f);
cout << "affine: " << endl << outputMat << endl;
// cout << "matrix: " << endl << inliers << endl;
// cam1CalibRefined.save("depthCalib1Refined.yml");
// cam2CalibRefined.save("depthCalib2Refined.yml");
// renderer1.setDepthOnly("depthCalib1Refined.yml");
// renderer2.setDepthOnly("depthCalib2Refined.yml");
saveCalibrationPoints();
// cv::Mat E,F;
// cv::stereoCalibrate(objectPoints, imagePoints1, imagePoints2,
// camMatrix1, distCoeff1,
// camMatrix2, distCoeff2,
// cv::Size(640,480), rvec, tvec, E, F);
// cv::solvePnPRansac(objectPoints, imagePoints2, camMatrix2, distCoeff2, rvec, tvec);
// ofxCv::saveMat(rvec, "rotation.yml");
// ofxCv::saveMat(tvec, "translation.yml");
// cv::Mat m3x3;
// cv::Rodrigues(rvec, m3x3);
// calibrationMatrix.set(1, 0, 0, tvec.at<double>(1,0),
// 0, 1, m3x3.at<double>(2,1), tvec.at<double>(2,0),
// 0, 0, m3x3.at<double>(2,2), tvec.at<double>(3,0),
// 0, 0, 0, 1);
//// calibrationMatrix.set(m3x3.at<double>(0,0), m3x3.at<double>(1,0), m3x3.at<double>(2,0), tvec.at<double>(1,0),
//// m3x3.at<double>(0,1), m3x3.at<double>(1,1), m3x3.at<double>(2,1), tvec.at<double>(2,0),
//// m3x3.at<double>(0,2), m3x3.at<double>(1,2), m3x3.at<double>(2,2), tvec.at<double>(3,0),
//// 0, 0, 0, 1);
// calibrationMatrix.getTransposedOf(calibrationMatrix);
// calibrationMatrix = ofxCv::makeMatrix(rvec, tvec);
cout << "calibrated!" << endl;
cout << "rotation: " << endl << rvec << endl;
cout << "translation: " << endl << tvec << endl;
cout << "START CAM CALIB IS " << cam1Calib[0] << endl;
// saveCalibrationPoints();
}
