# HG changeset patch # User Sebastien Jodogne # Date 1518607496 -3600 # Node ID 77715c3407676924f6106d2158927c9fa7f71a73 # Parent 197a5ddaf68ca216beed1cbb83002bca30084a60 unit testing FiniteProjectiveCamera diff -r 197a5ddaf68c -r 77715c340767 UnitTestsSources/UnitTestsMain.cpp --- a/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 11:29:26 2018 +0100 +++ b/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 12:24:56 2018 +0100 @@ -147,7 +147,58 @@ } -TEST(FiniteProjectiveCamera, Decomposition) +static bool CompareMatrix(const OrthancStone::Matrix& a, + const OrthancStone::Matrix& b, + double threshold = 0.00000001) +{ + if (a.size1() != b.size1() || + a.size2() != b.size2()) + { + return false; + } + + for (size_t i = 0; i < a.size1(); i++) + { + for (size_t j = 0; j < a.size2(); j++) + { + if (fabs(a(i, j) - b(i, j)) > threshold) + { + LOG(ERROR) << "Too large difference in component (" + << i << "," << j << "): " << a(i,j) << " != " << b(i,j); + return false; + } + } + } + + return true; +} + + +static bool CompareVector(const OrthancStone::Vector& a, + const OrthancStone::Vector& b, + double threshold = 0.00000001) +{ + if (a.size() != b.size()) + { + return false; + } + + for (size_t i = 0; i < a.size(); i++) + { + if (fabs(a(i) - b(i)) > threshold) + { + LOG(ERROR) << "Too large difference in component " + << i << ": " << a(i) << " != " << b(i); + return false; + } + } + + return true; +} + + + +TEST(FiniteProjectiveCamera, Decomposition1) { // Example 6.2 of "Multiple View Geometry in Computer Vision - 2nd // edition" (page 163) @@ -196,21 +247,192 @@ OrthancStone::FiniteProjectiveCamera camera2(camera.GetIntrinsicParameters(), camera.GetRotation(), camera.GetCenter()); - ASSERT_EQ(3u, camera2.GetMatrix().size1()); - ASSERT_EQ(4u, camera2.GetMatrix().size2()); - ASSERT_EQ(3u, camera2.GetIntrinsicParameters().size1()); - ASSERT_EQ(3u, camera2.GetIntrinsicParameters().size2()); - ASSERT_EQ(3u, camera2.GetRotation().size1()); - ASSERT_EQ(3u, camera2.GetRotation().size2()); - ASSERT_EQ(3u, camera2.GetCenter().size()); + + ASSERT_TRUE(CompareMatrix(camera.GetMatrix(), camera2.GetMatrix())); + ASSERT_TRUE(CompareMatrix(camera.GetIntrinsicParameters(), camera2.GetIntrinsicParameters())); + ASSERT_TRUE(CompareMatrix(camera.GetRotation(), camera2.GetRotation())); + ASSERT_TRUE(CompareVector(camera.GetCenter(), camera2.GetCenter())); +} + + +TEST(FiniteProjectiveCamera, Decomposition2) +{ + const double p[] = { 1188.111986, 580.205341, -808.445330, 128000.000000, -366.466264, 1446.510501, 418.499736, 128000.000000, -0.487118, 0.291726, -0.823172, 500.000000 }; + const double k[] = { -1528.494743, 0.000000, 256.000000, 0.000000, 1528.494743, 256.000000, 0.000000, 0.000000, 1.000000 }; + const double r[] = { -0.858893, -0.330733, 0.391047, -0.158171, 0.897503, 0.411668, -0.487118, 0.291726, -0.823172 }; + const double c[] = { 243.558936, -145.863085, 411.585964 }; + + OrthancStone::FiniteProjectiveCamera camera(p); + ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation())); + + OrthancStone::FiniteProjectiveCamera camera2(k, r, c); + ASSERT_TRUE(CompareMatrix(camera.GetMatrix(), camera2.GetMatrix(), 1)); + ASSERT_TRUE(CompareMatrix(camera.GetIntrinsicParameters(), camera2.GetIntrinsicParameters(), 0.001)); + ASSERT_TRUE(CompareMatrix(camera.GetRotation(), camera2.GetRotation(), 0.000001)); + ASSERT_TRUE(CompareVector(camera.GetCenter(), camera2.GetCenter(), 0.0001)); +} + + +TEST(FiniteProjectiveCamera, Decomposition3) +{ + const double p[] = { 10, 0, 0, 0, + 0, 20, 0, 0, + 0, 0, 30, 0 }; + + OrthancStone::FiniteProjectiveCamera camera(p); + ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation())); + ASSERT_DOUBLE_EQ(10, camera.GetIntrinsicParameters() (0, 0)); + ASSERT_DOUBLE_EQ(20, camera.GetIntrinsicParameters() (1, 1)); + ASSERT_DOUBLE_EQ(30, camera.GetIntrinsicParameters() (2, 2)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (0, 0)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (1, 1)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (2, 2)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (0)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (1)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (2)); +} + + +TEST(FiniteProjectiveCamera, Decomposition4) +{ + const double p[] = { 1, 0, 0, 10, + 0, 1, 0, 20, + 0, 0, 1, 30 }; + + OrthancStone::FiniteProjectiveCamera camera(p); + ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation())); + ASSERT_DOUBLE_EQ(1, camera.GetIntrinsicParameters() (0, 0)); + ASSERT_DOUBLE_EQ(1, camera.GetIntrinsicParameters() (1, 1)); + ASSERT_DOUBLE_EQ(1, camera.GetIntrinsicParameters() (2, 2)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (0, 0)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (1, 1)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (2, 2)); + ASSERT_DOUBLE_EQ(-10, camera.GetCenter() (0)); + ASSERT_DOUBLE_EQ(-20, camera.GetCenter() (1)); + ASSERT_DOUBLE_EQ(-30, camera.GetCenter() (2)); +} + + +TEST(FiniteProjectiveCamera, Decomposition5) +{ + const double p[] = { 0, 0, 10, 0, + 0, 20, 0, 0, + 30, 0, 0, 0 }; + + OrthancStone::FiniteProjectiveCamera camera(p); + ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation())); + ASSERT_DOUBLE_EQ(-10, camera.GetIntrinsicParameters() (0, 0)); + ASSERT_DOUBLE_EQ(20, camera.GetIntrinsicParameters() (1, 1)); + ASSERT_DOUBLE_EQ(30, camera.GetIntrinsicParameters() (2, 2)); + ASSERT_DOUBLE_EQ(-1, camera.GetRotation() (0, 2)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (1, 1)); + ASSERT_DOUBLE_EQ(1, camera.GetRotation() (2, 0)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (0)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (1)); + ASSERT_DOUBLE_EQ(0, camera.GetCenter() (2)); + + OrthancStone::FiniteProjectiveCamera camera2(camera.GetIntrinsicParameters(), + camera.GetRotation(), + camera.GetCenter()); + ASSERT_TRUE(CompareMatrix(camera.GetMatrix(), camera2.GetMatrix())); + ASSERT_TRUE(CompareMatrix(camera.GetIntrinsicParameters(), camera2.GetIntrinsicParameters())); + ASSERT_TRUE(CompareMatrix(camera.GetRotation(), camera2.GetRotation())); + ASSERT_TRUE(CompareVector(camera.GetCenter(), camera2.GetCenter())); +} - for (size_t i = 0; i < 3; i++) + +static double GetCosAngle(const OrthancStone::Vector& a, + const OrthancStone::Vector& b) +{ + // Returns the cosine of the angle between two vectors + // https://en.wikipedia.org/wiki/Dot_product#Geometric_definition + return boost::numeric::ublas::inner_prod(a, b) / + (boost::numeric::ublas::norm_2(a) * boost::numeric::ublas::norm_2(b)); +} + + +TEST(FiniteProjectiveCamera, Ray) +{ + const double pp[] = { -1499.650894, 2954.618773, -259.737419, 637891.819097, + -2951.517707, -1501.019129, -285.785281, 637891.819097, + 0.008528, 0.003067, -0.999959, 2491.764918 }; + + const OrthancStone::FiniteProjectiveCamera camera(pp); + + ASSERT_NEAR(-21.2492, camera.GetCenter() (0), 0.0001); + ASSERT_NEAR(-7.64234, camera.GetCenter() (1), 0.00001); + ASSERT_NEAR(2491.66, camera.GetCenter() (2), 0.01); + + // Image plane that led to these parameters, with principal point at + // (256,256). The image has dimensions 512x512. + OrthancStone::Vector o, ax, ay; + OrthancStone::LinearAlgebra::AssignVector(o, 7.009620, 2.521030, -821.942000); + OrthancStone::LinearAlgebra::AssignVector(ax, -0.453219, 0.891399, -0.001131 ); + OrthancStone::LinearAlgebra::AssignVector(ay, -0.891359, -0.453210, -0.008992); + OrthancStone::CoordinateSystem3D imagePlane(o, ax, ay); + + // Back-projection of the principal point { - for (size_t j = 0; j < 4; j++) + OrthancStone::Vector ray = camera.GetRayDirection(256, 256); + + // The principal axis vector is orthogonal to the image plane + // (i.e. parallel to the plane normal), in the opposite direction + // ("-1" corresponds to "cos(pi)"). + ASSERT_NEAR(-1, GetCosAngle(ray, imagePlane.GetNormal()), 0.0000001); + + // Forward projection of principal axis, resulting in the principal point + double x, y; + camera.ApplyFinite(x, y, camera.GetCenter() - ray); + + ASSERT_NEAR(256, x, 0.00001); + ASSERT_NEAR(256, y, 0.00001); + } + + // Back-projection of the 4 corners of the image + std::vector cx, cy; + cx.push_back(0); + cy.push_back(0); + cx.push_back(512); + cy.push_back(0); + cx.push_back(512); + cy.push_back(512); + cx.push_back(0); + cy.push_back(512); + + bool first = true; + double angle; + + for (size_t i = 0; i < cx.size(); i++) + { + OrthancStone::Vector ray = camera.GetRayDirection(cx[i], cy[i]); + + // Check that the angle wrt. principal axis is the same for all + // the 4 corners + double a = GetCosAngle(ray, imagePlane.GetNormal()); + if (first) { - ASSERT_NEAR(camera.GetMatrix() (i, j), - camera2.GetMatrix() (i, j), 0.00000001); + first = false; + angle = a; + } + else + { + ASSERT_NEAR(angle, a, 0.000001); } + + // Forward projection of the ray, going back to the original point + double x, y; + camera.ApplyFinite(x, y, camera.GetCenter() - ray); + + ASSERT_NEAR(cx[i], x, 0.00001); + ASSERT_NEAR(cy[i], y, 0.00001); + + // Alternative construction, by computing the intersection of the + // ray with the image plane + OrthancStone::Vector p; + ASSERT_TRUE(imagePlane.IntersectLine(p, camera.GetCenter(), -ray)); + imagePlane.ProjectPoint(x, y, p); + ASSERT_NEAR(cx[i], x + 256, 0.01); + ASSERT_NEAR(cy[i], y + 256, 0.01); } }