diff --git a/apps/test/camerashot/camerashot.pro b/apps/test/camerashot/camerashot.pro new file mode 100644 index 00000000..4dc9b0c2 --- /dev/null +++ b/apps/test/camerashot/camerashot.pro @@ -0,0 +1,10 @@ +TARGET = camerashot_test +INCLUDEPATH += . ../../.. +CONFIG += console stl +TEMPLATE = app +SOURCES += camerashot_test.cpp + +win32: CONFIG += NOMINMAX + +# Mac specific Config required to avoid to make application bundles +CONFIG -= app_bundle diff --git a/apps/test/camerashot/camerashot_test.cpp b/apps/test/camerashot/camerashot_test.cpp new file mode 100644 index 00000000..d2f7aeeb --- /dev/null +++ b/apps/test/camerashot/camerashot_test.cpp @@ -0,0 +1,234 @@ + +// STD headers +#include +#include + +// VCG headers +#include +#include + +double dist2(vcg::Point2d p1, vcg::Point2d p2) +{ + double d = sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1])); + return d; +} + +double dist3(vcg::Point3d p1, vcg::Point3d p2) +{ + 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])); + return d; +} + +// TEST1 - PROJECT A 3D POINT IN WORLD COORDINATE ON THE IMAGE PLANE +/////////////////////////////////////////////////////////////////////////////// +void test1(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2) +{ + vcg::Point2d p1proj, p2proj; + + p1proj = shot1.Project(p1); + p2proj = shot2.Project(p2); + + vcg::Point2d p1test(633.58101456110933, 400.0); + vcg::Point2d p2test(289.02943695191425, 400.0); + + assert(dist2(p1proj,p1test) < 0.00000001); + assert(dist2(p2proj,p2test) < 0.00000001); +} + +// TEST 2 - PROJECTION AND UNPROJECTION +/////////////////////////////////////////////////////////////////////////////// +void test2(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2) +{ + vcg::Point2d p1proj, p2proj; + + p1proj = shot1.Project(p1); + p2proj = shot2.Project(p2); + + vcg::Point3d p1unproj, p2unproj; + vcg::Point3d pcam1, pcam2; + + vcg::Matrix44d R1 = shot1.Extrinsics.Rot(); + vcg::Point4d pp(-10.0, -5.0, -70.0, 1.0); + pp = R1 * pp; + + pcam1 = shot1.ConvertWorldToCameraCoordinates(p1); + p1unproj = shot1.UnProject(p1proj, pcam1[2]); + pcam2 = shot2.ConvertWorldToCameraCoordinates(p2); + p2unproj = shot2.UnProject(p2proj, pcam2[2]); + + assert(dist3(p1, p1unproj) < 0.00000001); + assert(dist3(p2, p2unproj) < 0.00000001); +} + +// TEST 3 - CAMERA CONVERSION - CONVERT FOCAL IN PIXELS IN FOCAL IN MM +/////////////////////////////////////////////////////////////////////////////// +void test3(vcg::Shotd shot, vcg::Point3d p1) +{ + vcg::Shotd shotpx = shot; + + // we assume focal is in pixels + shotpx.Intrinsics.FocalMm = 1028.5949393128985805389837482; + shotpx.Intrinsics.PixelSizeMm[0] = 1.0; + shotpx.Intrinsics.PixelSizeMm[1] = 1.0; + + vcg::Point2d pproj; + pproj = shotpx.Project(p1); + + vcg::Point2d p1proj; + p1proj = shot.Project(p1); + + assert(dist2(pproj, p1proj) < 0.000000001); + + // CONVERSION - (ccd is assumed to be 35mm width) + + double ccd_width = 35.0; // conventionally assumed + double ccd_height = (ccd_width * shotpx.Intrinsics.ViewportPx[1]) / shotpx.Intrinsics.ViewportPx[0]; + shotpx.Intrinsics.PixelSizeMm[0] = (ccd_width / shotpx.Intrinsics.ViewportPx[0]); + shotpx.Intrinsics.PixelSizeMm[1] = (ccd_height / shotpx.Intrinsics.ViewportPx[1]); + shotpx.Intrinsics.FocalMm = (ccd_width * shotpx.Intrinsics.FocalMm) / shotpx.Intrinsics.ViewportPx[0]; // NOW FOCAL IS IN MM + + p1proj = shotpx.Project(p1); + + assert(dist2(pproj, p1proj) < 0.000000001); +} + +// TEST 4 - CAMERA MODIFICATION - CHANGE SCALE FACTOR OF THE WORLD +/////////////////////////////////////////////////////////////////////////////// +void test4(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2) +{ + //...TODO... +} + +// TEST 5 - CAMERA MODIFICATION - ROTATION + TRANSLATION +/////////////////////////////////////////////////////////////////////////////// +void test5(vcg::Shotd shot1, vcg::Shotd shot2, vcg::Point3d p1, vcg::Point3d p2) +{ + vcg::Matrix44d R = shot1.Extrinsics.Rot(); + vcg::Point3d tr = shot1.Extrinsics.Tra(); + + vcg::Matrix44d T; + T.SetIdentity(); + R.ElementAt(3,0) = tr[0]; + R.ElementAt(3,1) = tr[1]; + R.ElementAt(3,2) = tr[2]; + R.ElementAt(3,3) = 1.0; + + shot1.MultMatrix(T); + shot1.MultMatrix(R.transpose()); + + R = shot2.Extrinsics.Rot(); + tr = shot2.Extrinsics.Tra(); + + T.ElementAt(3,0) = -tr[0]; + T.ElementAt(3,1) = -tr[1]; + T.ElementAt(3,2) = -tr[2]; + T.ElementAt(3,3) = 1.0; + + shot1.MultMatrix(R); + shot1.MultMatrix(T); + + vcg::Point2d p1proj1, p2proj1, p1proj2, p2proj2; + p1proj1 = shot1.Project(p1); + p1proj2 = shot2.Project(p1); + + p2proj1 = shot1.Project(p2); + p2proj2 = shot2.Project(p2); + + assert(dist2(p1proj1, p1proj2) < 0.00001); + assert(dist2(p2proj1, p2proj2) < 0.00001); +} + + +int main() +{ + vcg::Point3d p1(20.0, 25.0, 10.0); + vcg::Point3d p2(-6.0, 40.0, 50.0); + vcg::Shotd shot1; + vcg::Shotd shot2; + + // Initialize camera 1 (C1) + shot1.Intrinsics.cameraType = vcg::Camera::PERSPECTIVE; + shot1.Intrinsics.FocalMm = 30.0; + shot1.Intrinsics.CenterPx[0] = 600.0; shot1.Intrinsics.CenterPx[1] = 400.0; + shot1.Intrinsics.ViewportPx[0] = 1200; shot1.Intrinsics.ViewportPx[1] = 800; + shot1.Intrinsics.PixelSizeMm[0] = 0.029166; shot1.Intrinsics.PixelSizeMm[1] = 0.029166; + + // no distorion is assumed (!) + shot1.Intrinsics.DistorCenterPx[0] = shot1.Intrinsics.DistorCenterPx[1] = 0.0; + shot1.Intrinsics.k[0] = 0.0; shot1.Intrinsics.k[1] = 0.0; shot1.Intrinsics.k[2] = 0.0; shot1.Intrinsics.k[3] = 0.0; + + vcg::Matrix44d R1; // -10 degree around Y axis + double deg2rad = 0.01745329251994329576923690768489; + R1.ElementAt(0,0) = std::cos(-10.0*deg2rad); + R1.ElementAt(0,1) = 0.0; + R1.ElementAt(0,2) = std::sin(-10.0*deg2rad); + R1.ElementAt(0,3) = 0.0; + R1.ElementAt(1,0) = 0.0; + R1.ElementAt(1,1) = 0.0; + R1.ElementAt(1,2) = 0.0; + R1.ElementAt(1,3) = 0.0; + R1.ElementAt(2,0) = -std::sin(-10.0*deg2rad); + R1.ElementAt(2,1) = 0.0; + R1.ElementAt(2,2) = std::cos(-10.0*deg2rad); + R1.ElementAt(2,3) = 0.0; + R1.ElementAt(3,0) = 0.0; + R1.ElementAt(3,1) = 0.0; + R1.ElementAt(3,2) = 0.0; + R1.ElementAt(3,3) = 1.0; + + vcg::Point3d T1(30.0, 30.0, 80.0); + shot1.Extrinsics.SetTra(T1); + shot1.Extrinsics.SetRot(R1); + + // Initialize camera 2 (C2) + shot2.Intrinsics.cameraType = vcg::Camera::PERSPECTIVE; + shot2.Intrinsics.FocalMm = 30.0; + shot2.Intrinsics.CenterPx[0] = 600.0; shot2.Intrinsics.CenterPx[1] = 400.0; + shot2.Intrinsics.ViewportPx[0] = 1200; shot2.Intrinsics.ViewportPx[1] = 800; + shot2.Intrinsics.PixelSizeMm[0] = 0.029166; shot2.Intrinsics.PixelSizeMm[1] = 0.029166; + + // no distortion is assumed (!) + shot2.Intrinsics.DistorCenterPx[0] = shot2.Intrinsics.DistorCenterPx[1] = 0.0; + shot2.Intrinsics.k[0] = 0.0; shot2.Intrinsics.k[1] = 0.0; shot2.Intrinsics.k[2] = 0.0; shot2.Intrinsics.k[3] = 0.0; + + + vcg::Matrix44d R2; // 18 degree around Y axis (+ 180 degree for the correct orientation of the camera) + R2.ElementAt(0,0) = std::cos(-45.0*deg2rad); + R2.ElementAt(0,1) = 0.0; + R2.ElementAt(0,2) = std::sin(-45.0*deg2rad); + R2.ElementAt(0,3) = 0.0; + R2.ElementAt(1,0) = 0.0; + R2.ElementAt(1,1) = 0.0; + R2.ElementAt(1,2) = 0.0; + R2.ElementAt(1,3) = 0.0; + R2.ElementAt(2,0) = -std::sin(-45.0*deg2rad); + R2.ElementAt(2,1) = 0.0; + R2.ElementAt(2,2) = std::cos(-45.0*deg2rad); + R2.ElementAt(2,3) = 0.0; + R2.ElementAt(3,0) = 0.0; + R2.ElementAt(3,1) = 0.0; + R2.ElementAt(3,2) = 0.0; + R2.ElementAt(3,3) = 1.0; + + vcg::Point3d T2(50.0, 30.0, 80.0); + shot2.Extrinsics.SetTra(T2); + shot2.Extrinsics.SetRot(R2); + + + // TEST 1 - project a 3D point in World coordinates on the image plane + test1(shot1, shot2, p1, p2); + + // TEST 2 - projection and unprojection + test2(shot1, shot2, p1, p2); + + // TEST 3 - CAMERA CONVERSION - CONVERT FOCAL IN PIXELS IN FOCAL IN MM + test3(shot1, p1); + + // TEST 4 - CAMERA MODIFICATION - CHANGE SCALE FACTOR OF THE WORLD + test4(shot1, shot2, p1, p2); + + // TEST 5 - CAMERA MODIFICATION - ROTATION + TRANSLATION + test5(shot1, shot2, p1, p2); + + return 0; +}