// ------------------------------------------------------
// TestMatrixs
// ------------------------------------------------------
void TestMatrixTemplate()
{
CTicTac tictac;
CMatrixDouble M,Z,D,RES;
// --------------------------------------
M.loadFromTextFile(myDataDir+string("matrixA.txt"));
cout << M << "\n";
M.eigenVectors(Z,D);
// cout << "Z:\n" << Z << "D:\n" << D;
int I,N = 1000;
tictac.Tic();
for (I =0;I<N;I++) RES= Z * D * (~Z);
printf("Operation 'RES= Z * D * (~Z)' done in %.03fus\n",1e6*tictac.Tac()/N);
// cout << "RES (1):\n" << RES;
tictac.Tic();
for (I =0;I<N;I++) Z.multiply_HCHt(D,RES);
printf("Operation 'Z.multiply_HCHt(D,RES)' done in %.03fus\n",1e6f*tictac.Tac()/N);
// cout << "RES (2):\n" << RES;
CMatrixDouble Q(M);
// cout << "A:\n" << Q;
}
// ------------------------------------------------------
// TestFFT_2D_complex
// ------------------------------------------------------
void TestFFT_2D_complex()
{
CMatrix DATA_R,DATA_I, RES_R,RES_I,B_R,B_I,D_R,D_I;
CTicTac tictac;
printf("Loading matrix from file...");
DATA_R.loadFromTextFile(myDataDir+string("complex_fft2_test_real.txt"));
DATA_I.loadFromTextFile(myDataDir+string("complex_fft2_test_imag.txt"));
printf("ok\n");
printf("Computing 2D complex FFT of %ux%u...",(unsigned int)DATA_R.getRowCount(),(unsigned int)DATA_R.getColCount());
tictac.Tic();
math::dft2_complex(DATA_R,DATA_I,RES_R,RES_I);
printf(" Done,%.06fms\n",tictac.Tac()*1000.0f);
SAVE_MATRIX(RES_R);
SAVE_MATRIX(RES_I);
printf("Computing 2D complex IFFT of %ux%u...",(unsigned int)DATA_R.getRowCount(),(unsigned int)DATA_R.getColCount());
tictac.Tic();
math::idft2_complex(RES_R,RES_I,B_R,B_I);
printf(" Done,%.06fms\n",tictac.Tac()*1000.0f);
D_R = B_R - DATA_R;
D_I = B_I - DATA_I;
float maxError_R,maxError_I;
size_t u,v;
D_R.find_index_max_value(u,v,maxError_R);
D_I.find_index_max_value(u,v,maxError_I);
printf("Maximum error between 'A' and 'IFFT(FFT(A))'=%e\n",maxError_R);
printf("Maximum error between 'A' and 'IFFT(FFT(A))'=%e\n",maxError_I);
}
// ------------------------------------------------------
// TestFFT_2D_real
// ------------------------------------------------------
void TestFFT_2D_real()
{
CMatrix A, RES_R,RES_I,B,D;
CTicTac tictac;
printf("Loading matrix from file...");
A.loadFromTextFile(myDataDir+string("fft2_test.txt"));
printf("ok\n");
printf("Computing 2D FFT of %ux%u...",(unsigned int)A.getRowCount(),(unsigned int)A.getColCount());
tictac.Tic();
math::dft2_real(A,RES_R,RES_I);
printf(" Done,%.06fms\n",tictac.Tac()*1000.0f);
RES_R.saveToTextFile("_out_fft2_real.txt");
RES_I.saveToTextFile("_out_fft2_imag.txt");
printf("Computing 2D IFFT of %ux%u...",(unsigned int)A.getRowCount(),(unsigned int)A.getColCount());
tictac.Tic();
math::idft2_real(RES_R,RES_I,B);
printf(" Done,%.06fms\n",tictac.Tac()*1000.0f);
// B.saveToTextFile("_out_ifft2.txt");
D = B - A;
// D.saveToTextFile("_out_fft2_error_diffs.txt");
float maxError;
size_t u,v;
D.find_index_max_value(u,v,maxError);
printf("Maximum error between 'A' and 'IFFT(FFT(A))'=%e\n",maxError);
}
double pointmap_test_1(int a1, int a2)
{
// test 1: insert scan
// ----------------------------------------
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) );
scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
CSimplePointsMap pt_map;
pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03;
CPose3D pose;
CTicTac tictac;
for (long i=0;i<a1;i++)
{
pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02);
pt_map.insertObservation(&scan1, &pose);
}
const unsigned N_REPS = 25;
if (a2==0)
return tictac.Tac();
else if (a2==1)
{ // 2d kd-tree
float x,y, dist2;
tictac.Tic();
for (unsigned k=N_REPS;k!=0;--k)
/*size_t idx = */ pt_map.kdTreeClosestPoint2D(5.0, 6.0, x,y, dist2);
return tictac.Tac()/N_REPS;
}
else
{ // 3d kd-tree
float x,y,z, dist2;
tictac.Tic();
for (unsigned k=N_REPS;k!=0;--k)
/*size_t idx =*/ pt_map.kdTreeClosestPoint3D(5.0, 6.0, 1.0, x,y,z, dist2);
return tictac.Tac()/N_REPS;
}
}
// ------------------------------------------------------
// TestCapture
// ------------------------------------------------------
void TestExtractFeaturesTile()
{
CDisplayWindow wind1,wind2;
CFeatureExtraction fExt;
CFeatureList featsHarris;
CImage img;
string the_img = myDataDir+string("test_image.jpg");
if (!img.loadFromFile(the_img ))
{
cerr << "Cannot load " << the_img << endl;
return;
}
cout << "Loaded test image: " << the_img << endl;
CTicTac tictac;
cout << "Extracting Harris features (tiled)... [f_harris_tiled.txt]";
fExt.options.featsType = featHarris;
fExt.options.harrisOptions.tile_image = true;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
cout << "Detected " << featsHarris.size() << " features in " << endl;
featsHarris.saveToTextFile("f_harris_tiled.txt");
wind1.setWindowTitle("Harris detected features (Tiled image)");
wind1.showTiledImageAndPoints( img, featsHarris );
cout << "Extracting Harris features... [f_harris.txt]";
fExt.options.harrisOptions.tile_image = false;
tictac.Tic();
fExt.detectFeatures( img, featsHarris );
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
featsHarris.saveToTextFile("f_harris.txt");
wind2.setWindowTitle("Harris detected features");
wind2.showTiledImageAndPoints( img, featsHarris );
mrpt::system::pause();
return;
}
double poses_test_convert_quat_ypr_pdf(int a1, int a2)
{
if (a1>=0)
mrpt::global_settings::USE_SUT_QUAT2EULER_CONVERSION = a1!=0;
const long N = 2000;
CPose3D a_mean(1.0,2.0,3.0,DEG2RAD(10),DEG2RAD(50),DEG2RAD(-30));
CMatrixDouble66 a_cov;
{
CMatrixFixedNumeric<double,6,8> v;
mrpt::random::randomGenerator.randomize(1234);
mrpt::random::randomGenerator.drawGaussian1DMatrix(v);
v*=0.1;
a_cov.multiply_AAt(v); // COV = v*vt
for (int i=3;i<6;i++) a_cov(i,i)+=square(DEG2RAD(2.0));
}
CPose3DPDFGaussian a(a_mean,a_cov);
CPose3DQuatPDFGaussian b =CPose3DQuatPDFGaussian(a);
double x;
CTicTac tictac;
for (long i=0;i<N;i++)
{
CPose3DPDFGaussian q(b);
x=q.mean.x();
}
double T = tictac.Tac()/N;
dummy_do_nothing_with_string( mrpt::format("%f",x) );
CPose3DPDFGaussian q(b);
//cout << q.cov << endl;
return T;
}
// ------------------------------------------------------
// Benchmark Point Maps
// ------------------------------------------------------
double pointmap_test_0(int a1, int a2)
{
// test 0: insert scan
// ----------------------------------------
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) );
scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) );
memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) );
memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) );
CSimplePointsMap pt_map;
pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03;
CPose3D pose;
CTicTac tictac;
for (int n=0;n<a2;n++)
{
pt_map.clear();
for (long i=0;i<a1;i++)
{
pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02);
pt_map.insertObservation(&scan1, &pose);
}
}
return tictac.Tac()/a2;
}
// ------------------------------------------------------
// Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_Harris_SAD( int w, int h )
{
CTicTac tictac;
CImage imL, imR;
CFeatureExtraction fExt;
CFeatureList featsHarris_L, featsHarris_R;
CMatchedFeatureList mHarris;
getTestImage(0,imR);
getTestImage(1,imL);
// Extract features: HARRIS
fExt.options.featsType = featHarris;
TMatchingOptions opt;
const size_t N = 20;
opt.matching_method = TMatchingOptions::mmSAD;
// HARRIS
tictac.Tic();
for (size_t i=0;i<N;i++)
{
fExt.detectFeatures( imL, featsHarris_L, 0, NFEATS );
fExt.detectFeatures( imR, featsHarris_R, 0, NFEATS );
//nMatches =
matchFeatures( featsHarris_L, featsHarris_R, mHarris, opt );
}
const double T = tictac.Tac()/N;
// cout << endl << "L: " << featsHarris_L.size() << " R: " << featsHarris_R.size() << " M: " << mHarris.size() << endl;
return T;
}
double grid_test_5_6(int a1, int a2)
{
randomGenerator.randomize(333);
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.loadFromVectors( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]), SCAN_RANGES_1,SCAN_VALID_1 );
COccupancyGridMap2D gridmap(-20,20,-20,20, 0.05f);
gridmap.insertionOptions.wideningBeamsWithDistance = a1!=0;
const long N = 3000;
CTicTac tictac;
for (long i=0;i<N;i++)
{
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
CPose3D pose3D(pose);
gridmap.insertObservation( &scan1, &pose3D );
}
return tictac.Tac()/N;
}
double stereoimage_rectify(int, int)
{
const CImage imgL(w, h, IMG_CHANNELS), imgR(w, h, IMG_CHANNELS);
CImage imgL2, imgR2;
mrpt::img::TStereoCamera params;
params.loadFromConfigFile(
"CAMERA_PARAMS",
mrpt::config::CConfigFileMemory(std::string(EXAMPLE_STEREO_CALIB)));
params.scaleToResolution(w, h);
mrpt::vision::CStereoRectifyMap rectify_map;
rectify_map.enableResizeOutput((w2 != w || h2 != h), w2, h2);
rectify_map.setFromCamParams(params);
CTicTac tictac;
const size_t N = 20;
tictac.Tic();
for (size_t i = 0; i < N; i++)
{
rectify_map.rectify(imgL, imgR, imgL2, imgR2);
}
return tictac.Tac() / N;
}
// ------------------------------------------------------
// Benchmark: Harris + SAD
// ------------------------------------------------------
double feature_matching_test_FAST_CC( int w, int h )
{
CTicTac tictac;
CImage imL, imR;
CFeatureExtraction fExt;
CFeatureList featsFAST_L, featsFAST_R;
CMatchedFeatureList mFAST;
getTestImage(0,imR);
getTestImage(1,imL);
// Extract features: HARRIS
fExt.options.featsType = featFAST;
//size_t nMatches;
TMatchingOptions opt;
const size_t N = 20;
// HARRIS
tictac.Tic();
for (size_t i=0;i<N;i++)
{
fExt.detectFeatures( imL, featsFAST_L, 0, NFEATS );
fExt.detectFeatures( imR, featsFAST_R, 0, NFEATS );
//nMatches =
matchFeatures( featsFAST_L, featsFAST_R, mFAST, opt );
}
const double T = tictac.Tac()/N;
// cout << endl << "L: " << featsFAST_L.size() << " R: " << featsFAST_R.size() << " M: " << mFAST.size() << endl;
return T;
}
double feature_extraction_test_FASTER_quick( int N, int threshold )
{
CTicTac tictac;
// Generate a random image
CImage img;
getTestImage(0,img);
img.grayscaleInPlace();
TSimpleFeatureList corners;
tictac.Tic();
if (TYP==featFASTER9)
for (int i=0;i<N;i++)
CFeatureExtraction::detectFeatures_SSE2_FASTER9(img,corners,threshold);
else if (TYP==featFASTER10)
for (int i=0;i<N;i++)
CFeatureExtraction::detectFeatures_SSE2_FASTER10(img,corners,threshold);
else if (TYP==featFASTER12)
for (int i=0;i<N;i++)
CFeatureExtraction::detectFeatures_SSE2_FASTER12(img,corners,threshold);
const double T = tictac.Tac()/N;
return T;
}
double image_KLTscore(int WIN, int N)
{
static const size_t w = 800;
static const size_t h = 800;
CImage img(w, h, CH_GRAY);
for (int i = 0; i < 5000; i++)
img.line(
getRandomGenerator().drawUniform(0, w - 1),
getRandomGenerator().drawUniform(0, h - 1),
getRandomGenerator().drawUniform(0, w - 1),
getRandomGenerator().drawUniform(0, h - 1),
TColor(getRandomGenerator().drawUniform32bit()));
ASSERT_BELOW_(WIN, 128);
int x = 0;
int y = 0;
CTicTac tictac;
tictac.Tic();
for (int i = 0; i < N; i++)
{
// float r =
img.KLT_response(x | 128, y | 128, WIN);
x++;
x &= 0x1FF;
y++;
y &= 0x1FF;
}
double R = tictac.Tac() / N;
return R;
}
double image_test_2(int w, int h)
{
CImage img(w, h, 3), img2;
#if MRPT_HAS_OPENCV
// int oldVal = cvUseOptimized(1);
#endif
for (int i = 0; i < 5000; i++)
img.line(
getRandomGenerator().drawUniform(0, w - 1),
getRandomGenerator().drawUniform(0, h - 1),
getRandomGenerator().drawUniform(0, w - 1),
getRandomGenerator().drawUniform(0, h - 1),
TColor(getRandomGenerator().drawUniform32bit()));
CTicTac tictac;
const size_t N = 50;
tictac.Tic();
for (size_t i = 0; i < N; i++) img.filterGaussian(img2, 7, 7);
double R = tictac.Tac() / N;
#if MRPT_HAS_OPENCV
// cvUseOptimized(oldVal);
#endif
return R;
}
double grid_test_8(int a1, int a2)
{
randomGenerator.randomize(333);
// prepare the laser scan:
CObservation2DRangeScan scan1;
scan1.aperture = M_PIf;
scan1.rightToLeft = true;
scan1.loadFromVectors( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]), SCAN_RANGES_1,SCAN_VALID_1 );
COccupancyGridMap2D gridmap(-20,20,-20,20, 0.05f);
// test 8: Likelihood computation
const long N = 5000;
CPose3D pose3D(0,0,0);
gridmap.insertObservation( &scan1, &pose3D );
double R = 0;
CTicTac tictac;
for (long i=0;i<N;i++)
{
CPose2D pose(
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-1.0,1.0),
randomGenerator.drawUniform(-M_PI,M_PI) );
R+=gridmap.computeObservationLikelihood(&scan1,pose);
}
return tictac.Tac()/N;
}
// ------------------------------------------------------
// TestJoystick
// ------------------------------------------------------
void TestJoystick()
{
// Open first joystick:
// ---------------------------
float x,y,z;
vector_bool buttons;
CTicTac tictac;
CJoystick joy;
const int nJoystick = 0; // The first one
printf("Press any key to stop program...\n");
while ( !mrpt::system::os::kbhit() )
{
tictac.Tic();
if (joy.getJoystickPosition(nJoystick,x,y,z,buttons) )
{
double t = tictac.Tac();
printf("Joystick readings: %.03f, %.03f, %.03f (", x, y, z);
for(unsigned b=0;b<buttons.size();b++)
printf("B%u:%c ", b,buttons[b] ? 'X':'-');
printf(") [Query %uus] \r", (unsigned)(t*1e6));
fflush(stdout);
}
else
{
printf("Error reading from joystick, please connect one to the system...\r");
}
mrpt::system::sleep(20);
}
}
void matching(data_st *data)
{
CTicTac tictac;
TMatchingOptions opt;
opt.useXRestriction=false;
if(data->matching_type==epipolar_match)
{
opt.useEpipolarRestriction=true;
opt.parallelOpticalAxis=false;
opt.epipolar_TH=2.0;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
opt.F.loadFromTextFile("FM_Harris2ways.txt");
}
else
{
opt.useEpipolarRestriction=false;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
}
cout<<"\nCalculating matches...\n";
//opt.F=data->FM_Harris2ways;
opt.matching_method=TMatchingOptions::mmDescriptorSURF;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris, data->featsHarris2, data->Harris_matched, opt);
cout << "Detected " << data->Harris_matched.size() << " Harris matches in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched.saveToTextFile("Harris_matches.txt");
//Calculate matches in the inverse way
//opt.F=(~opt.F);
//opt.F=(~data->FM_Harris2ways);
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris2, data->featsHarris, data->Harris_matched_inv, opt);
cout << "Detected " << data->Harris_matched_inv.size() << " Harris matches 'INV' in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched_inv.saveToTextFile("Harris_matches_inv.txt");
// Only matches found in both ways are kept in a new file (Detect matches in only one way and delete it)
data->Harris_matched2ways = comp2Matched_lists(data->Harris_matched, data->Harris_matched_inv);
cout << data->Harris_matched2ways.size() << " Harris matches in both ways\n" << endl;
data->matching_done=true;
cout<<"\nMatching finished\n";
}
void TestCapture_OpenCV()
{
CImageGrabber_OpenCV *capture = NULL;
if (LIVE_CAM)
{
#if 0 // test: Select the desired resolution
mrpt::vision::TCaptureCVOptions opts;
opts.frame_width = 320;
opts.frame_height = 240;
capture = new CImageGrabber_OpenCV( 0, CAMERA_CV_AUTODETECT, opts );
#else
capture = new CImageGrabber_OpenCV( N_CAM_TO_OPEN, CAMERA_CV_AUTODETECT);
#endif
}
else
{
capture = new CImageGrabber_OpenCV( AVI_TO_OPEN );
}
CTicTac tictac;
cout << "Press any key to stop capture to 'capture.rawlog'..." << endl;
CFileGZOutputStream fil("./capture.rawlog");
CDisplayWindow win("Capturing...");
int cnt = 0;
while (!mrpt::system::os::kbhit())
{
if ( (cnt++ % 20) == 0 )
{
if (cnt>0)
{
double t = tictac.Tac();
double FPS = 20 / t;
printf("\n %f FPS\n", FPS);
}
tictac.Tic();
}
CObservationImagePtr obs= CObservationImage::Create(); // Memory will be freed by SF destructor in each loop.
if (!capture->getObservation( *obs ))
{
cerr << "Error retrieving images!" << endl;
break;
}
fil << obs;
cout << "."; cout.flush();
if (win.isOpen())
win.showImage( obs->image );
}
delete capture;
}
double grid_test_7(int a1, int a2)
{
COccupancyGridMap2D gridmap(-20,20,-20,20, 0.05f);
CTicTac tictac;
gridmap.resizeGrid(-30,30,-40,40);
return tictac.Tac();
}
double matrix_test_det_fix(int a1, int a2)
{
CMatrixFixed<T, DIM1, DIM1> A;
getRandomGenerator().drawGaussian1DMatrix(A, T(0), T(1));
const long N = 10000;
CTicTac tictac;
for (long i = 0; i < N; i++) A.det();
return tictac.Tac() / N;
}
TEST(Matrices,mahalanobis)
{
using namespace detail_testMatrixMaha;
CTicTac time;
vector<CMatrixTemplateNumeric<NumericType> > matrices(howMany,CMatrixTemplateNumeric<NumericType>(matrixSize,matrixSize));
vector<mrpt::dynamicsize_vector<NumericType> > vectors(howMany,mrpt::dynamicsize_vector<NumericType>(matrixSize));
mrpt::dynamicsize_vector<NumericType> res1(howMany);
mrpt::dynamicsize_vector<NumericType> res2(howMany);
generate(matrices.begin(),matrices.end(),PositiveSemidefiniteGenerator<NumericType>(matrixSize));
generate(vectors.begin(),vectors.end(),RandomVectorGenerator<NumericType>(matrixSize));
time.Tic();
transform(itDouble(vectors.begin(),matrices.begin()),itDouble(vectors.end(),matrices.end()),res1.begin(),&get_xCxT_basic<NumericType>);
double t=time.Tac();
time.Tic();
transform(itDouble(vectors.begin(),matrices.begin()),itDouble(vectors.end(),matrices.end()),res2.begin(),Get_xCxT_cholesky<NumericType>(matrixSize));
t=time.Tac();
EXPECT_TRUE(0==accumulate(itDouble(res1.begin(),res2.begin()),itDouble(res1.end(),res2.end()),NumericType(0.0),&compareAndSum<NumericType>));
}
double matrix_test_inv_dyn(int a1, int a2)
{
CMatrixDynamic<T> A(DIM1, DIM1);
CMatrixDynamic<T> A2(DIM1, DIM1);
getRandomGenerator().drawGaussian1DMatrix(A, T(0), T(1));
const long N = 1000;
CTicTac tictac;
for (long i = 0; i < N; i++) A2 = A.inverse_LLt();
return tictac.Tac() / N;
}
int main()
{
try
{
// The landmark (global) position: 3D (x,y,z)
CPoint3D L(0,4,2);
// Robot pose: 2D (x,y,phi)
CPose2D R(2,1, DEG2RAD(45.0f) );
// Camera pose relative to the robot: 6D (x,y,z,yaw,pitch,roll).
CPose3D C(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f) );
// TEST 1. Relative position L' of the landmark wrt the camera
// --------------------------------------------------------------
cout << "L: " << L << endl;
cout << "R: " << R << endl;
cout << "C: " << C << endl;
cout << "R+C:" << (R+C) << endl;
//cout << (R+C).getHomogeneousMatrix();
CPoint3D L2;
CTicTac tictac;
tictac.Tic();
size_t i,N = 10000;
for (i=0;i<N;i++)
L2 = L - (R+C);
cout << "Computation in: " << 1e6 * tictac.Tac()/((double)N) << " us" << endl;
cout << "L': " << L2 << endl;
// TEST 2. Reconstruct the landmark position:
// --------------------------------------------------------------
CPoint3D L3 = R + C + L2;
cout << "R(+)C(+)L' = " << L3 << endl;
cout << "Should be equal to L = " << L << endl;
// TEST 3. Distance from the camera to the landmark
// --------------------------------------------------------------
cout << "|(R(+)C)-L|= " << (R+C).distanceTo(L) << endl;
cout << "|L-(R(+)C)|= " << (R+C).distanceTo(L) << endl;
return 0;
} catch (exception &e)
{
cerr << "EXCEPCTION: " << e.what() << endl;
return -1;
}
catch (...)
{
cerr << "Untyped excepcion!!";
return -1;
}
}
double image_rgb2gray_8u(int w, int h)
{
CImage img(w, h, CH_RGB), img2;
CTicTac tictac;
const size_t N = 300;
tictac.Tic();
for (size_t i = 0; i < N; i++) img.grayscale(img2);
return tictac.Tac() / N;
}
double matrix_test_chol_fix(int a1, int a2)
{
CMatrixFixedNumeric<T,DIM1,DIM1> A = randomGenerator.drawDefinitePositiveMatrix(DIM1, 0.2);
CMatrixFixedNumeric<T,DIM1,DIM1> chol_U;
const long N = 100;
CTicTac tictac;
for (long i=0;i<N;i++)
{
A.chol(chol_U);
}
return tictac.Tac()/N;
}
double matrix_test_unit_fix(int a1, int a2)
{
CMatrixFixedNumeric<T,DIM,DIM> C;
const long N = 1000000;
CTicTac tictac;
for (long i=0;i<N;i++)
{
C.resize(DIM,DIM);
C.setIdentity();
}
return tictac.Tac()/N;
}
double matrix_test_unit_dyn(int a1, int a2)
{
CMatrixTemplateNumeric<T> C(a1,a1);
const long N = 1000000;
CTicTac tictac;
for (long i=0;i<N;i++)
{
C.resize(a1,a1);
C.setIdentity();
}
return tictac.Tac()/N;
}
double matrix_test_det_dyn(int a1, int a2)
{
CMatrixTemplateNumeric<T> A(DIM1,DIM1);
randomGenerator.drawGaussian1DMatrix(A,T(0),T(1));
const long N = 10000;
CTicTac tictac;
for (long i=0;i<N;i++)
{
A.det();
}
return tictac.Tac()/N;
}
double matrix_test_inv_fix(int a1, int a2)
{
CMatrixFixedNumeric<T,DIM1,DIM1> A,A2;
randomGenerator.drawGaussian1DMatrix(A,T(0),T(1));
const long N = 1000;
CTicTac tictac;
for (long i=0;i<N;i++)
{
A.inv(A2);
}
return tictac.Tac()/N;
}
double image_halfsample_smooth(int w, int h)
{
CImage img(w, h, IMG_CHANNELS), img2;
CTicTac tictac;
const size_t N = 300;
tictac.Tic();
for (size_t i = 0; i < N; i++) img.scaleHalfSmooth(img2);
return tictac.Tac() / N;
}
