shape_reconstruction: camerashot

Go to the documentation of this file.
00001 
00002 // STD headers
00003 #include <iostream>
00004 #include <assert.h>
00005 
00006 // VCG headers
00007 #include <vcg/math/camera.h>
00008 #include <vcg/math/shot.h>
00009 
00010 static double precision = 0.000000001;  // 1e-9
00011 
00012 double dist2(vcg::Point2d p1, vcg::Point2d p2)
00013 {
00014   double d = sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1]));
00015   return d;
00016 }
00017 
00018 double dist3(vcg::Point3d p1, vcg::Point3d p2)
00019 {
00020   double d = sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1]) + (p1[2] - p2[2]) * (p1[2] - p2[2]));
00021   return d;
00022 }
00023 
00024 double checkIdentity(vcg::Matrix44d M)
00025 {
00026   double d;
00027 
00028   d = (M.ElementAt(0,0) - 1.0) * (M.ElementAt(0,0) - 1.0) + M.ElementAt(0,1) * M.ElementAt(0,1) + M.ElementAt(0,2) * M.ElementAt(0,2) + M.ElementAt(0,3) * M.ElementAt(0,3);
00029   d += M.ElementAt(1,0) * M.ElementAt(1,0) + (M.ElementAt(1,1) - 1.0) * (M.ElementAt(1,1) - 1.0) + M.ElementAt(1,2) * M.ElementAt(1,2) + M.ElementAt(1,3) * M.ElementAt(1,3);
00030   d += M.ElementAt(2,0) * M.ElementAt(2,0) + M.ElementAt(2,1) * M.ElementAt(2,1) + (M.ElementAt(2,2) - 1.0) * (M.ElementAt(2,2) - 1.0) + M.ElementAt(2,3) * M.ElementAt(2,3);
00031   d += M.ElementAt(3,0) * M.ElementAt(3,0) + M.ElementAt(3,1) * M.ElementAt(3,1) + M.ElementAt(3,2) * M.ElementAt(3,2) + (M.ElementAt(3,3) - 1.0) * (M.ElementAt(3,3) - 1.0);
00032 
00033   return d;
00034 }
00035 
00036 // TEST1 - PROJECT A 3D POINT IN WORLD COORDINATE ON THE IMAGE PLANE
00038 bool test1(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00039 {
00040   vcg::Point2d p1proj, p2proj;
00041 
00042   p1proj = shot1.Project(p1);
00043   p2proj = shot2.Project(p2);
00044 
00045   vcg::Point2d p1test(633.58101456110933, 327.22860336234237);
00046   vcg::Point2d p2test(289.02943695191425, 315.42715619973069);
00047 
00048   if (dist2(p1proj,p1test) > precision)
00049     return false;
00050 
00051   if (dist2(p2proj,p2test) > precision)
00052     return false;
00053 
00054   return true;
00055 }
00056 
00057 // TEST 2 - PROJECTION AND UNPROJECTION
00059 bool test2(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00060 {
00061   vcg::Point2d p1proj, p2proj;
00062 
00063   p1proj = shot1.Project(p1);
00064   p2proj = shot2.Project(p2);
00065 
00066   vcg::Point3d p1unproj, p2unproj;
00067   vcg::Point3d pcam1, pcam2;
00068 
00069   pcam1 = shot1.ConvertWorldToCameraCoordinates(p1);
00070   p1unproj = shot1.UnProject(p1proj, pcam1[2]);
00071   pcam2 = shot2.ConvertWorldToCameraCoordinates(p2);
00072   p2unproj = shot2.UnProject(p2proj, pcam2[2]);
00073 
00074   if (dist3(p1, p1unproj) > precision)
00075     return false;
00076 
00077   if (dist3(p2, p2unproj) > precision)
00078     return false;
00079 
00080   return true;
00081 }
00082 
00083 // TEST 3 - DEPTH COMPUTATION
00085 bool test3(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00086 {
00087   vcg::Point2d p1proj, p2proj;
00088 
00089   p1proj = shot1.Project(p1);
00090   p2proj = shot2.Project(p2);
00091 
00092   vcg::Point3d p1unproj, p2unproj;
00093 
00094   double depth1 = shot1.Depth(p1);
00095   double depth2 = shot2.Depth(p2);
00096 
00097   p1unproj = shot1.UnProject(p1proj, depth1);
00098   p2unproj = shot2.UnProject(p2proj, depth2);
00099 
00100   if (dist3(p1, p1unproj) > precision)
00101     return false;
00102 
00103   if (dist3(p2, p2unproj) > precision)
00104     return false;
00105 
00106   return true;
00107 }
00108 
00109 // TEST 4 - CAMERA CONVERSION - CONVERT FOCAL IN PIXELS IN FOCAL IN MM
00111 bool test4(vcg::Shotd shot, vcg::Point3d p1)
00112 {
00113   vcg::Shotd shotpx = shot;
00114 
00115   // we assume focal is in pixels
00116   shotpx.Intrinsics.FocalMm = 1028.5949393128985805389837482;
00117   shotpx.Intrinsics.PixelSizeMm[0] = 1.0;
00118   shotpx.Intrinsics.PixelSizeMm[1] = 1.0;
00119 
00120   vcg::Point2d pproj;
00121   pproj = shotpx.Project(p1);
00122 
00123   vcg::Point2d p1proj;
00124   p1proj = shot.Project(p1);
00125 
00126   if(dist2(pproj, p1proj) > precision)
00127     return false;
00128 
00129   // CONVERSION - (ccd is assumed to be 35mm width)
00130   shot.ConvertFocalToMM();
00131 
00132   p1proj = shotpx.Project(p1);
00133 
00134   if (dist2(pproj, p1proj) > precision)
00135     return false;
00136 
00137   return true;
00138 }
00139 
00140 // TEST 5 - CAMERA-SHOT MODIFICATION - CHANGE SCALE FACTOR OF THE WORLD
00142 bool test5(vcg::Shotd shot, vcg::Point3d p1, vcg::Point3d p2)
00143 {
00144   vcg::Point2d p1projPX, p2projPX;
00145   p1projPX = shot.Project(p1);
00146   p2projPX = shot.Project(p2);
00147 
00148   vcg::Point2d p1proj, p2proj;
00149   p1proj = shot.Intrinsics.ViewportPxToLocal(p1projPX);
00150   p2proj = shot.Intrinsics.ViewportPxToLocal(p2projPX);
00151 
00152   vcg::Point2d diff;
00153   double distance_before_world_scaling;
00154   diff = (p2proj-p1proj);
00155   distance_before_world_scaling = diff.Norm();
00156 
00157   // WORLD SCALING
00158   double scalefactor = 20.0;
00159 
00160   // adjust World 3D points
00161   p1 *= scalefactor;
00162   p2 *= scalefactor;
00163 
00164   shot.RescalingWorld(scalefactor);
00165 
00166   p1projPX = shot.Project(p1);
00167   p2projPX = shot.Project(p2);
00168 
00169   p1proj = shot.Intrinsics.ViewportPxToLocal(p1projPX);
00170   p2proj = shot.Intrinsics.ViewportPxToLocal(p2projPX);
00171 
00172   double distance_after_world_scaling;
00173   diff = (p2proj - p1proj);
00174   distance_after_world_scaling = diff.Norm();
00175 
00176   if (vcg::math::Abs(distance_before_world_scaling - (distance_after_world_scaling / scalefactor)) > precision)
00177     return false;
00178 
00179   return true;
00180 }
00181 
00182 // TEST 6 - WORLD-TO-EXTRINSICS AND EXTRINSICS-TO-WORLD TRANSFORMATIONS
00183 bool test6(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00184 {
00185   vcg::Matrix44d WtoE1 = shot1.GetWorldToExtrinsicsMatrix();
00186   vcg::Matrix44d WtoE2 = shot2.GetWorldToExtrinsicsMatrix();
00187   vcg::Matrix44d EtoW1 = shot1.GetExtrinsicsToWorldMatrix();
00188   vcg::Matrix44d EtoW2 = shot2.GetExtrinsicsToWorldMatrix();
00189 
00190   vcg::Matrix44d I1 = WtoE1 * EtoW1;
00191   vcg::Matrix44d I2 = WtoE2 * EtoW2;
00192   vcg::Matrix44d I3 = EtoW1 * WtoE1;
00193   vcg::Matrix44d I4 = EtoW2 * WtoE2;
00194 
00195   if (checkIdentity(I1) > precision)
00196     return false;
00197 
00198   if (checkIdentity(I2) > precision)
00199     return false;
00200 
00201   if (checkIdentity(I3) > precision)
00202     return false;
00203 
00204   if (checkIdentity(I4) > precision)
00205     return false;
00206 
00207   vcg::Point3d axisX(1.0, 0.0, 0.0);
00208   vcg::Point3d axisY(0.0, 1.0, 0.0);
00209   vcg::Point3d axisZ(0.0, 0.0, 1.0);
00210 
00211   vcg::Point3d vx = EtoW1 * axisX;
00212   vcg::Point3d vy = EtoW1 * axisY;
00213   vcg::Point3d vz = EtoW1 * axisZ;
00214 
00215   if (dist3(vx, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(0)) > precision)
00216     return false;
00217 
00218   if (dist3(vy, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(1)) > precision)
00219     return false;
00220 
00221   if (dist3(vz, shot1.Extrinsics.Tra() + shot1.Extrinsics.Rot().GetRow3(2)) > precision)
00222     return false;
00223 
00224   return true;
00225 }
00226 
00227 
00228 // TEST 7 - SHOT MODIFICATION - ROTATION + TRANSLATION
00230 bool test7(vcg::Shotd shot1, vcg::Point3d p1, vcg::Point3d p2)
00231 {
00232   // store data
00233   vcg::Matrix44d Rorig = shot1.Extrinsics.Rot();
00234   vcg::Point3d Torig = shot1.Extrinsics.Tra();
00235 
00236   // pure translation
00237   vcg::Matrix44d T;
00238   T.SetIdentity();
00239   T.ElementAt(0,3) = 10.0;
00240   T.ElementAt(1,3) = 10.0;
00241   T.ElementAt(2,3) = 10.0;
00242   T.ElementAt(3,3) = 1.0;
00243 
00244   vcg::Point2d p1proj = shot1.Project(p1);
00245 
00246   vcg::Point3d tr(T.ElementAt(0,3), T.ElementAt(1,3), T.ElementAt(2,3));
00247   vcg::Point3d pt = p1 + tr;
00248   shot1.ApplyRigidTransformation(T);
00249   vcg::Point2d ptproj = shot1.Project(pt);
00250 
00251   if (dist2(p1proj, ptproj) > precision)
00252     return false;
00253 
00254   // restore the original reference frame to test another transformation
00255   shot1.Extrinsics.SetTra(Torig);
00256   shot1.Extrinsics.SetRot(Rorig);
00257 
00258   // pure rotation
00259   vcg::Matrix44d R;
00260   R.SetZero();
00261   R.ElementAt(0,2) = 1.0;
00262   R.ElementAt(1,1) = 1.0;
00263   R.ElementAt(2,0) = -1.0;
00264   R.ElementAt(3,3) = 1.0;
00265 
00266   vcg::Point3d pr = R * p1;
00267   shot1.ApplyRigidTransformation(R);
00268   vcg::Point2d prproj = shot1.Project(pr);
00269 
00270   if (dist2(p1proj, prproj) > precision)
00271     return false;
00272 
00273   // restore the original reference frame to test another transformation
00274   shot1.Extrinsics.SetTra(Torig);
00275   shot1.Extrinsics.SetRot(Rorig);
00276 
00277   // roto-translation
00278   vcg::Matrix44d RT = T * R;
00279 
00280   vcg::Point3d prt = R * p1 + tr;
00281   shot1.ApplyRigidTransformation(RT);
00282   vcg::Point2d prtproj = shot1.Project(prt);
00283 
00284   if (dist2(p1proj, prtproj) > precision)
00285     return false;
00286 
00287   return true;
00288 }
00289 
00290 // TEST 8 - SHOT MODIFICATION - ROTATION + TRANSLATION
00292 bool test8(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00293 {
00294   // put shot1 reference frame into the origin of the World coordinates system
00295   vcg::Matrix44d M = shot1.GetWorldToExtrinsicsMatrix();
00296 
00297   // NOTE: The roto-translation which maps the point p in World coordinates to the Shot frame
00298   //       applied to the frame bring it in the world frame.
00299 
00300   shot1.ApplyRigidTransformation(M);
00301 
00302   // then, put in the shot2 reference frame
00303   M = shot2.GetExtrinsicsToWorldMatrix();
00304   shot1.ApplyRigidTransformation(M);
00305 
00306   // test..
00307   vcg::Point2d p1proj1, p2proj1, p1proj2, p2proj2;
00308   p1proj1 = shot1.Project(p1);
00309   p1proj2 = shot2.Project(p1);
00310 
00311   p2proj1 = shot1.Project(p2);
00312   p2proj2 = shot2.Project(p2);
00313 
00314   if (dist2(p1proj1, p1proj2) > precision)
00315     return false;
00316 
00317   if (dist2(p2proj1, p2proj2) > precision)
00318     return false;
00319 
00320   return true;
00321 }
00322 
00323 // TEST 9 - SHOT MODIFICATION - ROTATION + TRANSLATION
00325 bool test9(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00326 {
00327   vcg::Matrix44d M1 = shot1.GetExtrinsicsToWorldMatrix();
00328   vcg::Matrix44d M2 = shot2.GetWorldToExtrinsicsMatrix();
00329   vcg::Matrix44d M;
00330   M = M2 * M1;  // is the roto-translation that maps a point in the frame of Shot1 in a point in the frame of Shot2
00331   // BUT M2 brings the frame of Shot2 in the World system and M1 brings the World system in the Shot1 reference frame
00332   // HENCE the combined roto-translation which maps the frame of Shot2 in the frame of Shot2 is M1 * M2 (!!!)
00333 
00334   vcg::Matrix44d Mframe = M1 * M2;
00335 
00336   // apply it..
00337   shot2.ApplyRigidTransformation(Mframe);
00338 
00339   // and test it..
00340   vcg::Point2d p1proj1, p2proj1, p1proj2, p2proj2;
00341   p1proj1 = shot1.Project(p1);
00342   p1proj2 = shot2.Project(p1);
00343 
00344   p2proj1 = shot1.Project(p2);
00345   p2proj2 = shot2.Project(p2);
00346 
00347   if (dist2(p1proj1, p1proj2) > precision)
00348     return false;
00349 
00350   if (dist2(p2proj1, p2proj2) > precision)
00351     return false;
00352 
00353   return true;
00354 }
00355 
00356 // TEST 10 - SHOT MODIFICATION - SIMILARITY
00358 bool test10(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2)
00359 {
00360   vcg::Point2d p1proj = shot1.Project(p1);
00361 
00362   // store data
00363   vcg::Matrix44d Rorig = shot1.Extrinsics.Rot();
00364   vcg::Point3d Torig = shot1.Extrinsics.Tra();
00365 
00366   // pure translation
00367   vcg::Matrix44d T;
00368   T.SetIdentity();
00369   T.ElementAt(0,3) = 10.0;
00370   T.ElementAt(1,3) = 10.0;
00371   T.ElementAt(2,3) = 10.0;
00372   T.ElementAt(3,3) = 1.0;
00373 
00374   vcg::Point3d tr(T.ElementAt(0,3), T.ElementAt(1,3), T.ElementAt(2,3));
00375 
00376   // pure rotation
00377   vcg::Matrix44d R;
00378   R.SetZero();
00379   R.ElementAt(0,2) = 1.0;
00380   R.ElementAt(1,1) = 1.0;
00381   R.ElementAt(2,0) = -1.0;
00382   R.ElementAt(3,3) = 1.0;
00383 
00384   // scaling
00385   vcg::Matrix44d S;
00386   double scale = 10.0;
00387   S.SetIdentity();
00388   S *= scale;
00389   S.ElementAt(3,3) = 1.0;
00390 
00391   vcg::Point3d psim = R * S * p1 + tr;
00392 
00393   vcg::Matrix44d SRT = T * R * S;
00394 
00395   shot1.ApplySimilarity(SRT);
00396   vcg::Point2d psimproj = shot1.Project(psim);
00397 
00398   if (dist2(p1proj, psimproj) > precision)
00399     return false;
00400 
00401   // restore the original reference frame to test another transformation
00402   shot1.Extrinsics.SetTra(Torig);
00403   shot1.Extrinsics.SetRot(Rorig);
00404 
00405   vcg::Similarityd sm;
00406   double pihalf = 3.1415926535897932384626433832795 / 2.0;
00407   sm.SetRotate(pihalf, vcg::Point3d(0.0,1.0,0.0));
00408   sm.sca = scale;
00409   sm.tra = tr;
00410 
00411   shot1.ApplySimilarity(sm);
00412   psimproj = shot1.Project(psim);
00413 
00414   if (dist2(p1proj, psimproj) > precision)
00415     return false;
00416 
00417   return true;
00418 }
00419 
00420 
00421 int main()
00422 {
00423   vcg::Point3d p1(20.0, 25.0, 10.0);
00424   vcg::Point3d p2(-6.0, 25.0, 50.0);
00425   vcg::Shotd shot1;
00426   vcg::Shotd shot2;
00427 
00428   // Initialize camera 1 (C1)
00429   shot1.Intrinsics.cameraType = vcg::Camera<double>::PERSPECTIVE;
00430   shot1.Intrinsics.FocalMm = 30.0;
00431   shot1.Intrinsics.CenterPx[0] = 600.0; shot1.Intrinsics.CenterPx[1] = 400.0;
00432   shot1.Intrinsics.ViewportPx[0] = 1200; shot1.Intrinsics.ViewportPx[1] = 800;
00433   shot1.Intrinsics.PixelSizeMm[0] = 0.029166; shot1.Intrinsics.PixelSizeMm[1] = 0.029166;
00434 
00435   // no distorion is assumed (!)
00436   shot1.Intrinsics.DistorCenterPx[0] = shot1.Intrinsics.DistorCenterPx[1] = 0.0;
00437   shot1.Intrinsics.k[0] = 0.0; shot1.Intrinsics.k[1] = 0.0; shot1.Intrinsics.k[2] = 0.0; shot1.Intrinsics.k[3] = 0.0;
00438 
00439   vcg::Matrix44d R1; // -10 degree around Y axis
00440   double deg2rad = 0.01745329251994329576923690768489;
00441   R1.ElementAt(0,0) = vcg::math::Cos(-10.0*deg2rad);
00442   R1.ElementAt(0,1) = 0.0;
00443   R1.ElementAt(0,2) = vcg::math::Sin(-10.0*deg2rad);
00444   R1.ElementAt(0,3) = 0.0;
00445   R1.ElementAt(1,0) = 0.0;
00446   R1.ElementAt(1,1) = 1.0;
00447   R1.ElementAt(1,2) = 0.0;
00448   R1.ElementAt(1,3) = 0.0;
00449   R1.ElementAt(2,0) = -vcg::math::Sin(-10.0*deg2rad);
00450   R1.ElementAt(2,1) = 0.0;
00451   R1.ElementAt(2,2) = vcg::math::Cos(-10.0*deg2rad);
00452   R1.ElementAt(2,3) = 0.0;
00453   R1.ElementAt(3,0) = 0.0;
00454   R1.ElementAt(3,1) = 0.0;
00455   R1.ElementAt(3,2) = 0.0;
00456   R1.ElementAt(3,3) = 1.0;
00457 
00458   vcg::Point3d T1(30.0, 30.0, 80.0);
00459   shot1.Extrinsics.SetTra(T1);
00460   shot1.Extrinsics.SetRot(R1);
00461 
00462   // Initialize camera 2 (C2)
00463   shot2.Intrinsics.cameraType = vcg::Camera<double>::PERSPECTIVE;
00464   shot2.Intrinsics.FocalMm = 30.0;
00465   shot2.Intrinsics.CenterPx[0] = 600.0; shot2.Intrinsics.CenterPx[1] = 400.0;
00466   shot2.Intrinsics.ViewportPx[0] = 1200; shot2.Intrinsics.ViewportPx[1] = 800;
00467   shot2.Intrinsics.PixelSizeMm[0] = 0.029166; shot2.Intrinsics.PixelSizeMm[1] = 0.029166;
00468 
00469   // no distortion is assumed (!)
00470   shot2.Intrinsics.DistorCenterPx[0] = shot2.Intrinsics.DistorCenterPx[1] = 0.0;
00471   shot2.Intrinsics.k[0] = 0.0; shot2.Intrinsics.k[1] = 0.0; shot2.Intrinsics.k[2] = 0.0; shot2.Intrinsics.k[3] = 0.0;
00472 
00473 
00474   vcg::Matrix44d R2; // 18 degree around Y axis (+ 180 degree for the correct orientation of the camera)
00475   R2.ElementAt(0,0) = vcg::math::Cos(-45.0*deg2rad);
00476   R2.ElementAt(0,1) = 0.0;
00477   R2.ElementAt(0,2) = vcg::math::Sin(-45.0*deg2rad);
00478   R2.ElementAt(0,3) = 0.0;
00479   R2.ElementAt(1,0) = 0.0;
00480   R2.ElementAt(1,1) = 1.0;
00481   R2.ElementAt(1,2) = 0.0;
00482   R2.ElementAt(1,3) = 0.0;
00483   R2.ElementAt(2,0) = -vcg::math::Sin(-45.0*deg2rad);
00484   R2.ElementAt(2,1) = 0.0;
00485   R2.ElementAt(2,2) = vcg::math::Cos(-45.0*deg2rad);
00486   R2.ElementAt(2,3) = 0.0;
00487   R2.ElementAt(3,0) = 0.0;
00488   R2.ElementAt(3,1) = 0.0;
00489   R2.ElementAt(3,2) = 0.0;
00490   R2.ElementAt(3,3) = 1.0;
00491 
00492   vcg::Point3d T2(50.0, 30.0, 80.0);
00493   shot2.Extrinsics.SetTra(T2);
00494   shot2.Extrinsics.SetRot(R2);
00495 
00496 
00497   // TEST 1 - project a 3D point in World coordinates on the image plane
00498   if (test1(shot1, shot2, p1, p2))
00499   {
00500     std::cout << "TEST 1 (projection) - PASSED(!)" << std::endl;
00501   }
00502   else
00503     std::cout << "TEST 1 (projection) - FAILED(!)" << std::endl;
00504 
00505   // TEST 2 - projection and unprojection
00506   if (test2(shot1, shot2, p1, p2))
00507   {
00508     std::cout << "TEST 2 (unprojection) - PASSED(!)" << std::endl;
00509   }
00510   else
00511   {
00512     std::cout << "TEST 2 (unprojection) - FAILED(!)" << std::endl;
00513   }
00514 
00515   // TEST 3 - DEPTH COMPUTATION
00516   if (test3(shot1, shot2, p1, p2))
00517   {
00518     std::cout << "TEST 3 (depth computation) - PASSED(!)" << std::endl;
00519   }
00520   else
00521   {
00522     std::cout << "TEST 3 (depth computation) - FAILED(!)" << std::endl;
00523   }
00524 
00525 
00526   // TEST 4 - CAMERA CONVERSION - CONVERT FOCAL IN PIXELS IN FOCAL IN MM
00527   if (test4(shot1, p1))
00528   {
00529     std::cout << "TEST 4 (focal in px to focal in mm) - PASSED(!)" << std::endl;
00530   }
00531   else
00532   {
00533     std::cout << "TEST 4 (focal in px to focal in mm) - FAILED(!)" << std::endl;
00534   }
00535 
00536   // TEST 5 - CAMERA-SHOT MODIFICATION - CHANGE SCALE FACTOR OF THE WORLD
00537   if (test5(shot1, p1, p2))
00538   {
00539     std::cout << "TEST 5 (scaling the World) - PASSED(!)" << std::endl;
00540   }
00541   else
00542   {
00543     std::cout << "TEST 5 (scaling the World) - FAILED(!)" << std::endl;
00544   }
00545 
00546   // TEST 6 - WORLD-TO-EXTRINSICS AND EXTRINSICS-TO-WORLD TRANSFORMATIONS
00547   if (test6(shot1, shot2, p1, p2))
00548   {
00549     std::cout << "TEST 6 (World-to-Extrinsics and Extrinsics-to-World) - PASSED(!)" << std::endl;
00550   }
00551   else
00552   {
00553     std::cout << "TEST 6 (World-to-Extrinsics and Extrinsics-to-World) - FAILE(!)" << std::endl;
00554   }
00555 
00556   // TEST 7 - SHOT MODIFICATION - ROTO-TRANSLATION OF THE SHOT COORDINATES SYSTEM
00557   if (test7(shot1, p1, p2))
00558   {
00559     std::cout << "TEST 7 (roto-translation of the Shot coordinates system) - PASSED(!)" << std::endl;
00560   }
00561   else
00562   {
00563     std::cout << "TEST 7 (roto-translation of the Shot coordinates system) - FAILED(!)" << std::endl;
00564   }
00565 
00566   // TEST 7 - SHOT MODIFICATION - ROTO-TRANSLATION OF THE SHOT COORDINATES SYSTEM
00567   if (test8(shot1, shot2, p1, p2))
00568   {
00569     std::cout << "TEST 8 (roto-translation of the Shot coordinates system) - PASSED(!)" << std::endl;
00570   }
00571   else
00572   {
00573     std::cout << "TEST 8 (roto-translation of the Shot coordinates system) - FAILED(!)" << std::endl;
00574   }
00575 
00576   // TEST 9 - SHOT MODIFICATION - ROTO-TRANSLATION OF THE SHOT COORDINATES SYSTEM
00577   if (test9(shot1, shot2, p1, p2))
00578   {
00579     std::cout << "TEST 9 (roto-translation of the Shot coordinates system) - PASSED(!)" << std::endl;
00580   }
00581   else
00582   {
00583     std::cout << "TEST 9 (roto-translation of the Shot coordinates system) - FAILED(!)" << std::endl;
00584   }
00585 
00586   // TEST 10 - SHOT MODIFICATION - SIMILARITY TRANSFORMATION
00587   if (test10(shot1, shot2, p1, p2))
00588   {
00589     std::cout << "TEST 10 (similarity transform of the Shot coordinates system) - PASSED(!)" << std::endl;
00590   }
00591   else
00592   {
00593     std::cout << "TEST 10 (similarity transform of the Shot coordinates system) - FAILED(!)" << std::endl;
00594   }
00595 
00596   return 0;
00597 }