Mercurial > hg > orthanc-stone
changeset 188:45b03b04a777 wasm
calibration of FiniteProjectiveCamera
| author | Sebastien Jodogne <s.jodogne@gmail.com> |
|---|---|
| date | Fri, 16 Mar 2018 13:02:17 +0100 |
| parents | a12ca0f4aaaa |
| children | 964118e7e6de |
| files | Framework/Toolbox/FiniteProjectiveCamera.cpp Framework/Toolbox/FiniteProjectiveCamera.h UnitTestsSources/UnitTestsMain.cpp |
| diffstat | 3 files changed, 125 insertions(+), 8 deletions(-) [+] |
line wrap: on
line diff
--- a/Framework/Toolbox/FiniteProjectiveCamera.cpp Thu Mar 15 17:45:48 2018 +0100 +++ b/Framework/Toolbox/FiniteProjectiveCamera.cpp Fri Mar 16 13:02:17 2018 +0100 @@ -21,6 +21,8 @@ #include "FiniteProjectiveCamera.h" +#include "GeometryToolbox.h" + #include <Core/Logging.h> #include <Core/OrthancException.h> @@ -43,8 +45,6 @@ const Matrix& r, const Vector& c) { - using namespace boost::numeric::ublas; - if (k.size1() != 3 || k.size2() != 3 || !LinearAlgebra::IsCloseToZero(k(1, 0)) || @@ -80,21 +80,22 @@ ComputeMInverse(); - Matrix tmp = identity_matrix<double>(3); + Matrix tmp = LinearAlgebra::IdentityMatrix(3); tmp.resize(3, 4); tmp(0, 3) = -c[0]; tmp(1, 3) = -c[1]; tmp(2, 3) = -c[2]; - tmp = prod(r, tmp); - p_ = prod(k, tmp); + p_ = LinearAlgebra::Product(k, r, tmp); + + assert(p_.size1() == 3 && + p_.size2() == 4); + } void FiniteProjectiveCamera::Setup(const Matrix& p) { - using namespace boost::numeric::ublas; - if (p.size1() != 3 || p.size2() != 4) { @@ -118,7 +119,7 @@ LinearAlgebra::RQDecomposition3x3(k_, r_, m); ComputeMInverse(); - c_ = prod(-minv_, p4); + c_ = LinearAlgebra::Product(-minv_, p4); } @@ -218,4 +219,73 @@ { return boost::numeric::ublas::prod(p_, SetupApply(v, true)); } + + + static Vector AddHomogeneousCoordinate(const Vector& p) + { + assert(p.size() == 3); + return LinearAlgebra::CreateVector(p[0], p[1], p[2], 1); + } + + + FiniteProjectiveCamera::FiniteProjectiveCamera(const Vector& camera, + const Vector& principalPoint, + double angle, + unsigned int imageWidth, + unsigned int imageHeight, + double pixelSpacingX, + double pixelSpacingY) + { + if (camera.size() != 3 || + principalPoint.size() != 3 || + LinearAlgebra::IsCloseToZero(pixelSpacingX) || + LinearAlgebra::IsCloseToZero(pixelSpacingY)) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + const double focal = boost::numeric::ublas::norm_2(camera - principalPoint); + + if (LinearAlgebra::IsCloseToZero(focal)) + { + LOG(ERROR) << "Camera lies on the image plane"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + Matrix a; + LinearAlgebra::AlignVectorsWithRotation(a, camera - principalPoint, + LinearAlgebra::CreateVector(0, 0, -1)); + + Matrix r = LinearAlgebra::Product(GeometryToolbox::CreateRotationMatrixAlongZ(angle), a); + + Matrix k = LinearAlgebra::ZeroMatrix(3, 3); + k(0,0) = focal / pixelSpacingX; + k(1,1) = focal / pixelSpacingY; + k(0,2) = static_cast<double>(imageWidth) / 2.0; + k(1,2) = static_cast<double>(imageHeight) / 2.0; + k(2,2) = 1; + + Setup(k, r, camera); + + { + // Sanity checks + Vector v1 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(camera)); + Vector v2 = LinearAlgebra::Product(p_, AddHomogeneousCoordinate(principalPoint)); + + if (!LinearAlgebra::IsCloseToZero(v1[2]) || // Camera is mapped to singularity + LinearAlgebra::IsCloseToZero(v2[2])) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); + } + + // The principal point must be mapped to the center of the image + v2 /= v2[2]; + + if (!LinearAlgebra::IsNear(v2[0], static_cast<double>(imageWidth) / 2.0) || + !LinearAlgebra::IsNear(v2[1], static_cast<double>(imageHeight) / 2.0)) + { + throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); + } + } + } }
--- a/Framework/Toolbox/FiniteProjectiveCamera.h Thu Mar 15 17:45:48 2018 +0100 +++ b/Framework/Toolbox/FiniteProjectiveCamera.h Fri Mar 16 13:02:17 2018 +0100 @@ -62,6 +62,15 @@ FiniteProjectiveCamera(const double p[12]); + // Constructor that implements camera calibration + FiniteProjectiveCamera(const Vector& camera, + const Vector& principalPoint, + double angle, + unsigned int imageWidth, + unsigned int imageHeight, + double pixelSpacingX, + double pixelSpacingY); + const Matrix& GetMatrix() const { return p_;
--- a/UnitTestsSources/UnitTestsMain.cpp Thu Mar 15 17:45:48 2018 +0100 +++ b/UnitTestsSources/UnitTestsMain.cpp Fri Mar 16 13:02:17 2018 +0100 @@ -620,6 +620,44 @@ } +TEST(FiniteProjectiveCamera, Calibration) +{ + unsigned int volumeWidth = 512; + unsigned int volumeHeight = 512; + unsigned int volumeDepth = 110; + + OrthancStone::Vector camera = OrthancStone::LinearAlgebra::CreateVector + (-1000, -5000, -static_cast<double>(volumeDepth) * 32); + + OrthancStone::Vector principalPoint = OrthancStone::LinearAlgebra::CreateVector + (volumeWidth/2, volumeHeight/2, volumeDepth * 2); + + OrthancStone::FiniteProjectiveCamera c(camera, principalPoint, 0, 512, 512, 1, 1); + + double swapv[9] = { 1, 0, 0, + 0, -1, 512, + 0, 0, 1 }; + OrthancStone::Matrix swap; + OrthancStone::LinearAlgebra::FillMatrix(swap, 3, 3, swapv); + + OrthancStone::Matrix p = OrthancStone::LinearAlgebra::Product(swap, c.GetMatrix()); + p /= p(2,3); + + ASSERT_NEAR( 1.04437, p(0,0), 0.00001); + ASSERT_NEAR(-0.0703111, p(0,1), 0.00000001); + ASSERT_NEAR(-0.179283, p(0,2), 0.000001); + ASSERT_NEAR( 61.7431, p(0,3), 0.0001); + ASSERT_NEAR( 0.11127, p(1,0), 0.000001); + ASSERT_NEAR(-0.595541, p(1,1), 0.000001); + ASSERT_NEAR( 0.872211, p(1,2), 0.000001); + ASSERT_NEAR( 203.748, p(1,3), 0.001); + ASSERT_NEAR( 3.08593e-05, p(2,0), 0.0000000001); + ASSERT_NEAR( 0.000129138, p(2,1), 0.000000001); + ASSERT_NEAR( 9.18901e-05, p(2,2), 0.0000000001); + ASSERT_NEAR( 1, p(2,3), 0.0000001); +} + + int main(int argc, char **argv) { Orthanc::Logging::Initialize();