00001 /* 00002 Copyright (C) 2010 - 2014 Christoph Haenisch 00003 00004 Chair of Medical Engineering (mediTEC) 00005 RWTH Aachen University 00006 Pauwelsstr. 20 00007 52074 Aachen 00008 Germany 00009 00010 Version 0.1.0 (2012-05-21) 00011 */ 00012 00019 #ifndef ESTIMATE_AFFINE_TRANSFORMATION_FROM_PLANE_TO_3D_HPP_8970243897 00020 #define ESTIMATE_AFFINE_TRANSFORMATION_FROM_PLANE_TO_3D_HPP_8970243897 00021 00022 00023 #include <cmath> 00024 #include <vector> 00025 00026 #include <Eigen/Core> 00027 #include <Eigen/Dense> 00028 #include <Eigen/StdVector> 00029 00030 #include "Coordinate.hpp" 00031 #include "ErrorObj.hpp" 00032 #include "EstimateTransformation3D.hpp" 00033 00034 00035 namespace TRTK 00036 { 00037 00038 00199 template <class T> 00200 class EstimateAffineTransformationFromPlaneTo3D : public EstimateTransformation3D<T> 00201 { 00202 private: 00203 00204 typedef EstimateTransformation3D<T> super; 00205 00206 public: 00207 00208 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00209 00210 typedef T value_type; 00211 00212 typedef typename super::VectorXT VectorXT; 00213 typedef typename super::MatrixXT MatrixXT; 00214 typedef typename super::Vector2T Vector2T; 00215 typedef typename super::Vector3T Vector3T; 00216 typedef typename super::Vector4T Vector4T; 00217 typedef typename super::RowVector2T RowVector2T; 00218 typedef typename super::RowVector3T RowVector3T; 00219 typedef typename super::Matrix2T Matrix2T; 00220 typedef typename super::Matrix3T Matrix3T; 00221 typedef typename super::Matrix4T Matrix4T; 00222 00223 using super::NOT_ENOUGH_POINTS; 00224 using super::UNEQUAL_NUMBER_OF_POINTS; 00225 using super::UNKNOWN_ERROR; 00226 using super::WRONG_POINT_SIZE; 00227 00228 EstimateAffineTransformationFromPlaneTo3D(); 00229 00230 EstimateAffineTransformationFromPlaneTo3D(const std::vector<Coordinate<T> > & source_points, 00231 const std::vector<Coordinate<T> > & target_points); 00232 00233 EstimateAffineTransformationFromPlaneTo3D(const std::vector<Vector3T> & source_points, 00234 const std::vector<Vector3T> & target_points); 00235 00236 EstimateAffineTransformationFromPlaneTo3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points, 00237 const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points); 00238 00239 virtual ~EstimateAffineTransformationFromPlaneTo3D(); 00240 00241 virtual void compute(); 00242 00243 Matrix3T getLinearTransformationMatrix() const; 00244 Vector3T getTranslationVector() const; 00245 }; 00246 00247 00253 template <class T> 00254 EstimateAffineTransformationFromPlaneTo3D<T>::EstimateAffineTransformationFromPlaneTo3D() 00255 { 00256 } 00257 00258 00280 template <class T> 00281 EstimateAffineTransformationFromPlaneTo3D<T>::EstimateAffineTransformationFromPlaneTo3D(const std::vector<Coordinate<T> > & source_points, 00282 const std::vector<Coordinate<T> > & target_points) 00283 { 00284 super::setSourcePoints(source_points); 00285 super::setTargetPoints(target_points); 00286 } 00287 00288 00302 template <class T> 00303 EstimateAffineTransformationFromPlaneTo3D<T>::EstimateAffineTransformationFromPlaneTo3D(const std::vector<Vector3T> & source_points, 00304 const std::vector<Vector3T> & target_points) 00305 { 00306 super::setSourcePoints(source_points); 00307 super::setTargetPoints(target_points); 00308 } 00309 00310 00324 template <class T> 00325 EstimateAffineTransformationFromPlaneTo3D<T>::EstimateAffineTransformationFromPlaneTo3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points, 00326 const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points) 00327 { 00328 super::setSourcePoints(source_points); 00329 super::setTargetPoints(target_points); 00330 } 00331 00332 00338 template <class T> 00339 EstimateAffineTransformationFromPlaneTo3D<T>::~EstimateAffineTransformationFromPlaneTo3D() 00340 { 00341 } 00342 00343 00364 template <class T> 00365 void EstimateAffineTransformationFromPlaneTo3D<T>::compute() 00366 { 00367 if (super::m_source_points.size() != super::m_target_points.size()) 00368 { 00369 ErrorObj error; 00370 error.setClassName("EstimateAffineTransformationFromPlaneTo3D<T>"); 00371 error.setFunctionName("compute"); 00372 error.setErrorMessage("Point sets must have the same cardinality."); 00373 error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS); 00374 throw error; 00375 } 00376 00377 if (super::m_source_points.cols() < 4 || super::m_target_points.cols() < 4) 00378 { 00379 ErrorObj error; 00380 error.setClassName("EstimateAffineTransformationFromPlaneTo3D<T>"); 00381 error.setFunctionName("compute"); 00382 error.setErrorMessage("Not enough points to compute transformation matrix."); 00383 error.setErrorCode(NOT_ENOUGH_POINTS); 00384 throw error; 00385 } 00386 00387 /* 00388 00389 Explanation of the algorithm: 00390 00391 By using homogeneous coordinates the above affine transformation can be described as follows: 00392 00393 | t_11 t_12 0 t_14 | | source_x | | target_x | 00394 | t_21 t_22 0 t_24 | * | source_y | = | target_y | 00395 | t_31 t_32 0 t_34 | | source_z | = | target_z | 00396 | 0 0 0 1 | | 1 | | 1 | 00397 00398 The equation can be rewritten as: 00399 | t_11 | 00400 | t_12 | 00401 | t_14 | 00402 | source_x source_y 1 0 0 0 0 0 0 | | t_21 | | target_x | 00403 | 0 0 0 source_x source_y 1 0 0 0 | * | t_22 | = | target_y | 00404 | 0 0 0 0 0 0 source_x source_y 1 | | t_24 | | target_z | 00405 | t_31 | 00406 | t_32 | 00407 | t_34 | 00408 00409 Or in short (for the i-th point pair): A(i) * x = b(i) 00410 00411 The equation must hold for all point pairs. Thus we combine A(1) ... A(n) and 00412 b(1) ... b(n), respectively, to A = [A(1) ... A(n)]^T and b = [b(1) ... b(n)]^T. 00413 This leads to an over-determined system, which can be solved via the pseudo inverse: 00414 00415 <==> A * x = b 00416 <==> A^T * A * x = A^T * b 00417 <==> inverse(A^T * A) * A^T * A * x = x = inverse(A^T * A) * A^T * b 00418 00419 Thus: 00420 00421 x = pseudo_inverse * b = [ inverse(A^T * A) * A^T ] * b 00422 00423 */ 00424 00425 const int number_points = super::m_source_points.cols(); 00426 00427 // fill matrix A with the above described entries 00428 00429 MatrixXT A(3 * number_points, 9); 00430 00431 const VectorXT ones = VectorXT::Ones(number_points); 00432 00433 A.setZero(); 00434 A.block(0 * number_points, 0, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00435 A.block(0 * number_points, 1, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00436 A.block(0 * number_points, 2, number_points, 1) = ones; 00437 A.block(1 * number_points, 3, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00438 A.block(1 * number_points, 4, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00439 A.block(1 * number_points, 5, number_points, 1) = ones; 00440 A.block(2 * number_points, 6, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00441 A.block(2 * number_points, 7, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00442 A.block(2 * number_points, 8, number_points, 1) = ones; 00443 00444 // fill vector b 00445 00446 VectorXT b(3 * number_points, 1); 00447 b.block(0 * number_points, 0, number_points, 1) = super::m_target_points.block(0, 0, 1, number_points).transpose(); 00448 b.block(1 * number_points, 0, number_points, 1) = super::m_target_points.block(1, 0, 1, number_points).transpose(); 00449 b.block(2 * number_points, 0, number_points, 1) = super::m_target_points.block(2, 0, 1, number_points).transpose(); 00450 00451 // compute pseudo inverse 00452 00453 MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose(); 00454 00455 // compute vector x whose components are the sought transformation entries 00456 00457 VectorXT x = pseudo_inverse * b; 00458 00459 super::m_transformation_matrix(0, 0) = x(0); 00460 super::m_transformation_matrix(0, 1) = x(1); 00461 super::m_transformation_matrix(0, 2) = 0; 00462 super::m_transformation_matrix(0, 3) = x(2); 00463 00464 super::m_transformation_matrix(1, 0) = x(3); 00465 super::m_transformation_matrix(1, 1) = x(4); 00466 super::m_transformation_matrix(1, 2) = 0; 00467 super::m_transformation_matrix(1, 3) = x(5); 00468 00469 super::m_transformation_matrix(2, 0) = x(6); 00470 super::m_transformation_matrix(2, 1) = x(7); 00471 super::m_transformation_matrix(2, 2) = 0; 00472 super::m_transformation_matrix(2, 3) = x(8); 00473 00474 super::m_transformation_matrix(3, 0) = 0; 00475 super::m_transformation_matrix(3, 1) = 0; 00476 super::m_transformation_matrix(3, 2) = 0; 00477 super::m_transformation_matrix(3, 3) = 1; 00478 } 00479 00480 00488 template <class T> 00489 typename EstimateAffineTransformationFromPlaneTo3D<T>::Matrix3T EstimateAffineTransformationFromPlaneTo3D<T>::getLinearTransformationMatrix() const 00490 { 00491 return super::m_transformation_matrix.block(0, 0, 3, 3); 00492 } 00493 00494 00502 template <class T> 00503 typename EstimateAffineTransformationFromPlaneTo3D<T>::Vector3T EstimateAffineTransformationFromPlaneTo3D<T>::getTranslationVector() const 00504 { 00505 return super::m_transformation_matrix.block(0, 3, 3, 1); 00506 } 00507 00508 00509 } // namespace TRTK 00510 00511 00512 #endif // ESTIMATE_AFFINE_TRANSFORMATION_FROM_PLANE_TO_3D_HPP_8970243897
Documentation generated by Doxygen