Mercurial > hg > orthanc-stone
changeset 161:197a5ddaf68c wasm
FiniteProjectiveCamera
| author | Sebastien Jodogne <s.jodogne@gmail.com> |
|---|---|
| date | Wed, 14 Feb 2018 11:29:26 +0100 |
| parents | e9dae7e7bffc |
| children | 77715c340767 |
| files | Framework/Toolbox/FiniteProjectiveCamera.cpp Framework/Toolbox/FiniteProjectiveCamera.h Framework/Toolbox/LinearAlgebra.cpp Framework/Toolbox/LinearAlgebra.h Resources/CMake/OrthancStoneConfiguration.cmake UnitTestsSources/UnitTestsMain.cpp |
| diffstat | 6 files changed, 418 insertions(+), 4 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Framework/Toolbox/FiniteProjectiveCamera.cpp Wed Feb 14 11:29:26 2018 +0100 @@ -0,0 +1,221 @@ +/** + * Stone of Orthanc + * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics + * Department, University Hospital of Liege, Belgium + * Copyright (C) 2017-2018 Osimis S.A., Belgium + * + * This program is free software: you can redistribute it and/or + * modify it under the terms of the GNU Affero General Public License + * as published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + **/ + + +#include "FiniteProjectiveCamera.h" + +#include <Core/Logging.h> +#include <Core/OrthancException.h> + +namespace OrthancStone +{ + void FiniteProjectiveCamera::ComputeMInverse() + { + using namespace boost::numeric::ublas; + + // inv(M) = inv(K * R) = inv(R) * inv(K) = R' * inv(K). This + // matrix is always invertible, by definition of finite + // projective cameras (page 157). + Matrix kinv; + LinearAlgebra::InvertUpperTriangularMatrix(kinv, k_); + minv_ = prod(trans(r_), kinv); + } + + + void FiniteProjectiveCamera::Setup(const Matrix& k, + const Matrix& r, + const Vector& c) + { + using namespace boost::numeric::ublas; + + if (k.size1() != 3 || + k.size2() != 3 || + !LinearAlgebra::IsCloseToZero(k(1, 0)) || + !LinearAlgebra::IsCloseToZero(k(2, 0)) || + !LinearAlgebra::IsCloseToZero(k(2, 1))) + { + LOG(ERROR) << "Invalid intrinsic parameters"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + if (r.size1() != 3 || + r.size2() != 3) + { + LOG(ERROR) << "Invalid size for a 3D rotation matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + if (!LinearAlgebra::IsRotationMatrix(r, 100.0 * std::numeric_limits<float>::epsilon())) + { + LOG(ERROR) << "Invalid rotation matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + if (c.size() != 3) + { + LOG(ERROR) << "Invalid camera center"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + k_ = k; + r_ = r; + c_ = c; + + ComputeMInverse(); + + Matrix tmp = identity_matrix<double>(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); + } + + + void FiniteProjectiveCamera::Setup(const Matrix& p) + { + using namespace boost::numeric::ublas; + + if (p.size1() != 3 || + p.size2() != 4) + { + LOG(ERROR) << "Invalid camera matrix"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + p_ = p; + + // M is the left 3x3 submatrix of "P" + Matrix m = p; + m.resize(3, 3); + + // p4 is the last column of "P" + Vector p4(3); + p4[0] = p(0, 3); + p4[1] = p(1, 3); + p4[2] = p(2, 3); + + // The RQ decomposition is explained on page 157 + LinearAlgebra::RQDecomposition3x3(k_, r_, m); + ComputeMInverse(); + + c_ = prod(-minv_, p4); + } + + + FiniteProjectiveCamera::FiniteProjectiveCamera(const double k[9], + const double r[9], + const double c[3]) + { + Matrix kk, rr; + Vector cc; + + LinearAlgebra::FillMatrix(kk, 3, 3, k); + LinearAlgebra::FillMatrix(rr, 3, 3, r); + LinearAlgebra::FillVector(cc, 3, c); + + Setup(kk, rr, cc); + } + + + FiniteProjectiveCamera::FiniteProjectiveCamera(const double p[12]) + { + Matrix pp; + LinearAlgebra::FillMatrix(pp, 3, 4, p); + Setup(pp); + } + + + Vector FiniteProjectiveCamera::GetRayDirection(double x, + double y) const + { + // This derives from Equation (6.14) on page 162, taking "mu = + // 1" and noticing that "-inv(M)*p4" corresponds to the camera + // center in finite projective cameras + + // The (x,y) coordinates on the imaged plane, as an homogeneous vector + Vector xx(3); + xx[0] = x; + xx[1] = y; + xx[2] = 1.0; + + return boost::numeric::ublas::prod(minv_, xx); + } + + + + static Vector SetupApply(const Vector& v, + bool infinityAllowed) + { + if (v.size() == 3) + { + // Vector "v" in non-homogeneous coordinates, add the homogeneous component + Vector vv; + LinearAlgebra::AssignVector(vv, v[0], v[1], v[2], 1.0); + return vv; + } + else if (v.size() == 4) + { + // Vector "v" is already in homogeneous coordinates + + if (!infinityAllowed && + LinearAlgebra::IsCloseToZero(v[3])) + { + LOG(ERROR) << "Cannot apply a finite projective camera to a " + << "point at infinity with this method"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + + return v; + } + else + { + LOG(ERROR) << "The input vector must represent a point in 3D"; + throw Orthanc::OrthancException(Orthanc::ErrorCode_ParameterOutOfRange); + } + } + + + void FiniteProjectiveCamera::ApplyFinite(double& x, + double& y, + const Vector& v) const + { + Vector p = boost::numeric::ublas::prod(p_, SetupApply(v, false)); + + if (LinearAlgebra::IsCloseToZero(p[2])) + { + // Point at infinity: Should not happen with a finite input point + throw Orthanc::OrthancException(Orthanc::ErrorCode_InternalError); + } + else + { + x = p[0] / p[2]; + y = p[1] / p[2]; + } + } + + + Vector FiniteProjectiveCamera::ApplyGeneral(const Vector& v) const + { + return boost::numeric::ublas::prod(p_, SetupApply(v, true)); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Framework/Toolbox/FiniteProjectiveCamera.h Wed Feb 14 11:29:26 2018 +0100 @@ -0,0 +1,101 @@ +/** + * Stone of Orthanc + * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics + * Department, University Hospital of Liege, Belgium + * Copyright (C) 2017-2018 Osimis S.A., Belgium + * + * This program is free software: you can redistribute it and/or + * modify it under the terms of the GNU Affero General Public License + * as published by the Free Software Foundation, either version 3 of + * the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Affero General Public License for more details. + * + * You should have received a copy of the GNU Affero General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + **/ + + +#pragma once + +#include "LinearAlgebra.h" + +namespace OrthancStone +{ + // Reference: "Multiple View Geometry in Computer Vision (2nd Edition)" + class FiniteProjectiveCamera : public boost::noncopyable + { + private: + Matrix p_; // 3x4 matrix - Equation (6.11) - page 157 + Matrix k_; // 3x3 matrix of intrinsic parameters - Equation (6.10) - page 157 + Matrix r_; // 3x3 rotation matrix in 3D space + Vector c_; // 3x1 vector in 3D space corresponding to camera center + Matrix minv_; // Inverse of the M = P(1:3,1:3) submatrix + + void ComputeMInverse(); + + void Setup(const Matrix& k, + const Matrix& r, + const Vector& c); + + void Setup(const Matrix& p); + + public: + FiniteProjectiveCamera(const Matrix& k, + const Matrix& r, + const Vector& c) + { + Setup(k, r, c); + } + + FiniteProjectiveCamera(const Matrix& p) + { + Setup(p); + } + + FiniteProjectiveCamera(const double k[9], + const double r[9], + const double c[3]); + + FiniteProjectiveCamera(const double p[12]); + + const Matrix& GetMatrix() const + { + return p_; + } + + const Matrix& GetRotation() const + { + return r_; + } + + const Vector& GetCenter() const + { + return c_; + } + + const Matrix& GetIntrinsicParameters() const + { + return k_; + } + + // Computes the 3D vector that represents the direction from the + // camera center to the (x,y) imaged point + Vector GetRayDirection(double x, + double y) const; + + // Apply the camera to a 3D point "v" that is not at infinity. "v" + // can be encoded either as a non-homogeneous vector (3 + // components), or as a homogeneous vector (4 components). + void ApplyFinite(double& x, + double& y, + const Vector& v) const; + + // Apply the camera to a 3D point "v" that is possibly at + // infinity. The result is a 2D point in homogeneous coordinates. + Vector ApplyGeneral(const Vector& v) const; + }; +}
--- a/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 09:18:06 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.cpp Wed Feb 14 11:29:26 2018 +0100 @@ -115,6 +115,20 @@ } + void AssignVector(Vector& v, + double v1, + double v2, + double v3, + double v4) + { + v.resize(4); + v[0] = v1; + v[1] = v2; + v[2] = v3; + v[3] = v4; + } + + bool IsNear(double x, double y) {
--- a/Framework/Toolbox/LinearAlgebra.h Wed Feb 14 09:18:06 2018 +0100 +++ b/Framework/Toolbox/LinearAlgebra.h Wed Feb 14 11:29:26 2018 +0100 @@ -60,6 +60,12 @@ double v2, double v3); + void AssignVector(Vector& v, + double v1, + double v2, + double v3, + double v4); + inline bool IsNear(double x, double y, double threshold)
--- a/Resources/CMake/OrthancStoneConfiguration.cmake Wed Feb 14 09:18:06 2018 +0100 +++ b/Resources/CMake/OrthancStoneConfiguration.cmake Wed Feb 14 11:29:26 2018 +0100 @@ -180,6 +180,7 @@ ${ORTHANC_STONE_DIR}/Framework/Toolbox/DicomStructureSet.cpp ${ORTHANC_STONE_DIR}/Framework/Toolbox/DownloadStack.cpp ${ORTHANC_STONE_DIR}/Framework/Toolbox/Extent2D.cpp + ${ORTHANC_STONE_DIR}/Framework/Toolbox/FiniteProjectiveCamera.cpp ${ORTHANC_STONE_DIR}/Framework/Toolbox/GeometryToolbox.cpp ${ORTHANC_STONE_DIR}/Framework/Toolbox/LinearAlgebra.cpp ${ORTHANC_STONE_DIR}/Framework/Toolbox/MessagingToolbox.cpp
--- a/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 09:18:06 2018 +0100 +++ b/UnitTestsSources/UnitTestsMain.cpp Wed Feb 14 11:29:26 2018 +0100 @@ -25,6 +25,7 @@ #include "../Framework/Layers/FrameRenderer.h" #include "../Framework/Layers/LayerSourceBase.h" #include "../Framework/Toolbox/DownloadStack.h" +#include "../Framework/Toolbox/FiniteProjectiveCamera.h" #include "../Framework/Toolbox/OrthancSlicesLoader.h" #include "../Framework/Volumes/ImageBuffer3D.h" #include "../Framework/Volumes/SlicedVolumeBase.h" @@ -136,11 +137,81 @@ TEST(GeometryToolbox, Interpolation) { // https://en.wikipedia.org/wiki/Bilinear_interpolation#Application_in_image_processing - ASSERT_FLOAT_EQ(146.1f, OrthancStone::GeometryToolbox::ComputeBilinearInterpolation(20.5f, 14.2f, 91, 210, 162, 95)); + ASSERT_FLOAT_EQ(146.1f, OrthancStone::GeometryToolbox::ComputeBilinearInterpolation + (20.5f, 14.2f, 91, 210, 162, 95)); + + ASSERT_FLOAT_EQ(123.35f, OrthancStone::GeometryToolbox::ComputeTrilinearInterpolation + (20.5f, 15.2f, 5.7f, + 91, 210, 162, 95, + 51, 190, 80, 92)); +} + + +TEST(FiniteProjectiveCamera, Decomposition) +{ + // Example 6.2 of "Multiple View Geometry in Computer Vision - 2nd + // edition" (page 163) + const double p[12] = { + 3.53553e+2, 3.39645e+2, 2.77744e+2, -1.44946e+6, + -1.03528e+2, 2.33212e+1, 4.59607e+2, -6.32525e+5, + 7.07107e-1, -3.53553e-1, 6.12372e-1, -9.18559e+2 + }; + + OrthancStone::FiniteProjectiveCamera camera(p); + ASSERT_EQ(3u, camera.GetMatrix().size1()); + ASSERT_EQ(4u, camera.GetMatrix().size2()); + ASSERT_EQ(3u, camera.GetIntrinsicParameters().size1()); + ASSERT_EQ(3u, camera.GetIntrinsicParameters().size2()); + ASSERT_EQ(3u, camera.GetRotation().size1()); + ASSERT_EQ(3u, camera.GetRotation().size2()); + ASSERT_EQ(3u, camera.GetCenter().size()); + + ASSERT_NEAR(1000.0, camera.GetCenter()[0], 0.01); + ASSERT_NEAR(2000.0, camera.GetCenter()[1], 0.01); + ASSERT_NEAR(1500.0, camera.GetCenter()[2], 0.01); - ASSERT_FLOAT_EQ(123.35f, OrthancStone::GeometryToolbox::ComputeTrilinearInterpolation(20.5f, 15.2f, 5.7f, - 91, 210, 162, 95, - 51, 190, 80, 92)); + ASSERT_NEAR(468.2, camera.GetIntrinsicParameters() (0, 0), 0.1); + ASSERT_NEAR(91.2, camera.GetIntrinsicParameters() (0, 1), 0.1); + ASSERT_NEAR(300.0, camera.GetIntrinsicParameters() (0, 2), 0.1); + ASSERT_NEAR(427.2, camera.GetIntrinsicParameters() (1, 1), 0.1); + ASSERT_NEAR(200.0, camera.GetIntrinsicParameters() (1, 2), 0.1); + ASSERT_NEAR(1.0, camera.GetIntrinsicParameters() (2, 2), 0.1); + + ASSERT_NEAR(0, camera.GetIntrinsicParameters() (1, 0), 0.0000001); + ASSERT_NEAR(0, camera.GetIntrinsicParameters() (2, 0), 0.0000001); + ASSERT_NEAR(0, camera.GetIntrinsicParameters() (2, 1), 0.0000001); + + ASSERT_NEAR(0.41380, camera.GetRotation() (0, 0), 0.00001); + ASSERT_NEAR(0.90915, camera.GetRotation() (0, 1), 0.00001); + ASSERT_NEAR(0.04708, camera.GetRotation() (0, 2), 0.00001); + ASSERT_NEAR(-0.57338, camera.GetRotation() (1, 0), 0.00001); + ASSERT_NEAR(0.22011, camera.GetRotation() (1, 1), 0.00001); + ASSERT_NEAR(0.78917, camera.GetRotation() (1, 2), 0.00001); + ASSERT_NEAR(0.70711, camera.GetRotation() (2, 0), 0.00001); + ASSERT_NEAR(-0.35355, camera.GetRotation() (2, 1), 0.00001); + ASSERT_NEAR(0.61237, camera.GetRotation() (2, 2), 0.00001); + + ASSERT_TRUE(OrthancStone::LinearAlgebra::IsRotationMatrix(camera.GetRotation())); + + 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()); + + for (size_t i = 0; i < 3; i++) + { + for (size_t j = 0; j < 4; j++) + { + ASSERT_NEAR(camera.GetMatrix() (i, j), + camera2.GetMatrix() (i, j), 0.00000001); + } + } }