D:/Programmierung/C/TRTK/include/TRTK/EstimateAffineTransformation3D.hpp
Go to the documentation of this file.
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.2.3 (2011-11-10) 00011 */ 00012 00019 #ifndef ESTIMATE_AFFINE_TRANSFORMATION_3D_HPP_1790843183 00020 #define ESTIMATE_AFFINE_TRANSFORMATION_3D_HPP_1790843183 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 00195 template <class T> 00196 class EstimateAffineTransformation3D : public EstimateTransformation3D<T> 00197 { 00198 private: 00199 00200 typedef EstimateTransformation3D<T> super; 00201 00202 public: 00203 00204 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00205 00206 typedef T value_type; 00207 00208 typedef typename super::VectorXT VectorXT; 00209 typedef typename super::MatrixXT MatrixXT; 00210 typedef typename super::Vector2T Vector2T; 00211 typedef typename super::Vector3T Vector3T; 00212 typedef typename super::Vector4T Vector4T; 00213 typedef typename super::RowVector2T RowVector2T; 00214 typedef typename super::RowVector3T RowVector3T; 00215 typedef typename super::Matrix2T Matrix2T; 00216 typedef typename super::Matrix3T Matrix3T; 00217 typedef typename super::Matrix4T Matrix4T; 00218 00219 using super::NOT_ENOUGH_POINTS; 00220 using super::UNEQUAL_NUMBER_OF_POINTS; 00221 using super::UNKNOWN_ERROR; 00222 using super::WRONG_POINT_SIZE; 00223 00224 EstimateAffineTransformation3D(); 00225 00226 EstimateAffineTransformation3D(const std::vector<Coordinate<T> > & source_points, 00227 const std::vector<Coordinate<T> > & target_points); 00228 00229 EstimateAffineTransformation3D(const std::vector<Vector3T> & source_points, 00230 const std::vector<Vector3T> & target_points); 00231 00232 EstimateAffineTransformation3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points, 00233 const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points); 00234 00235 virtual ~EstimateAffineTransformation3D(); 00236 00237 virtual void compute(); 00238 00239 Matrix3T getLinearTransformationMatrix() const; 00240 Vector3T getTranslationVector() const; 00241 }; 00242 00243 00249 template <class T> 00250 EstimateAffineTransformation3D<T>::EstimateAffineTransformation3D() 00251 { 00252 } 00253 00254 00276 template <class T> 00277 EstimateAffineTransformation3D<T>::EstimateAffineTransformation3D(const std::vector<Coordinate<T> > & source_points, 00278 const std::vector<Coordinate<T> > & target_points) 00279 { 00280 super::setSourcePoints(source_points); 00281 super::setTargetPoints(target_points); 00282 } 00283 00284 00298 template <class T> 00299 EstimateAffineTransformation3D<T>::EstimateAffineTransformation3D(const std::vector<Vector3T> & source_points, 00300 const std::vector<Vector3T> & target_points) 00301 { 00302 super::setSourcePoints(source_points); 00303 super::setTargetPoints(target_points); 00304 } 00305 00306 00320 template <class T> 00321 EstimateAffineTransformation3D<T>::EstimateAffineTransformation3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points, 00322 const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points) 00323 { 00324 super::setSourcePoints(source_points); 00325 super::setTargetPoints(target_points); 00326 } 00327 00328 00334 template <class T> 00335 EstimateAffineTransformation3D<T>::~EstimateAffineTransformation3D() 00336 { 00337 } 00338 00339 00360 template <class T> 00361 void EstimateAffineTransformation3D<T>::compute() 00362 { 00363 if (super::m_source_points.size() != super::m_target_points.size()) 00364 { 00365 ErrorObj error; 00366 error.setClassName("EstimateAffineTransformation3D<T>"); 00367 error.setFunctionName("compute"); 00368 error.setErrorMessage("Point sets must have the same cardinality."); 00369 error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS); 00370 throw error; 00371 } 00372 00373 if (super::m_source_points.cols() < 4 || super::m_target_points.cols() < 4) 00374 { 00375 ErrorObj error; 00376 error.setClassName("EstimateAffineTransformation3D<T>"); 00377 error.setFunctionName("compute"); 00378 error.setErrorMessage("Not enough points to compute transformation matrix."); 00379 error.setErrorCode(NOT_ENOUGH_POINTS); 00380 throw error; 00381 } 00382 00383 /* 00384 00385 Explanation of the algorithm (in 2D): 00386 00387 By using homogeneous coordinates an affine transformation can be described as follows: 00388 00389 | t_11 t_12 t_13 | | source_x | | target_x | 00390 | t_21 t_22 t_23 | * | source_y | = | target_y | 00391 | 0 0 1 | | 1 | | 1 | 00392 00393 The equation can be rewritten as: 00394 | t_11 | 00395 | t_12 | 00396 | source_x source_y 1 0 0 0 | * | t_13 | = | target_x | 00397 | 0 0 0 source_x source_y 1 | | t_21 | | target_y | 00398 | t_22 | 00399 | t_23 | 00400 00401 Or in short: A * x = b 00402 00403 By using more and more points this leads to an over-determined system, which can 00404 be solved via the pseudo inverse: 00405 00406 <==> A * x = b 00407 <==> A^T * A * x = A^T * b 00408 <==> inverse(A^T * A) * A^T * A * x = x = inverse(A^T * A) * A^T * b 00409 00410 Thus: 00411 00412 x = pseudo_inverse * b = [ inverse(A^T * A) * A^T ] * b 00413 00414 */ 00415 00416 const int number_points = super::m_source_points.cols(); 00417 00418 // fill matrix A with the above described entries 00419 00420 MatrixXT A(3 * number_points, 12); 00421 00422 const VectorXT ones = VectorXT::Ones(number_points); 00423 00424 A.setZero(); 00425 A.block(0 * number_points, 0, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00426 A.block(0 * number_points, 1, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00427 A.block(0 * number_points, 2, number_points, 1) = super::m_source_points.block(2, 0, 1, number_points).transpose(); 00428 A.block(0 * number_points, 3, number_points, 1) = ones; 00429 A.block(1 * number_points, 4, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00430 A.block(1 * number_points, 5, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00431 A.block(1 * number_points, 6, number_points, 1) = super::m_source_points.block(2, 0, 1, number_points).transpose(); 00432 A.block(1 * number_points, 7, number_points, 1) = ones; 00433 A.block(2 * number_points, 8, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00434 A.block(2 * number_points, 9, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00435 A.block(2 * number_points, 10, number_points, 1) = super::m_source_points.block(2, 0, 1, number_points).transpose(); 00436 A.block(2 * number_points, 11, number_points, 1) = ones; 00437 00438 // fill vector b 00439 00440 VectorXT b(3 * number_points, 1); 00441 b.block(0 * number_points, 0, number_points, 1) = super::m_target_points.block(0, 0, 1, number_points).transpose(); 00442 b.block(1 * number_points, 0, number_points, 1) = super::m_target_points.block(1, 0, 1, number_points).transpose(); 00443 b.block(2 * number_points, 0, number_points, 1) = super::m_target_points.block(2, 0, 1, number_points).transpose(); 00444 00445 // compute pseudo inverse 00446 00447 MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose(); 00448 00449 // compute vector x whose components are the sought transformation entries 00450 00451 VectorXT x = pseudo_inverse * b; 00452 00453 super::m_transformation_matrix(0, 0) = x(0); 00454 super::m_transformation_matrix(0, 1) = x(1); 00455 super::m_transformation_matrix(0, 2) = x(2); 00456 super::m_transformation_matrix(0, 3) = x(3); 00457 super::m_transformation_matrix(1, 0) = x(4); 00458 super::m_transformation_matrix(1, 1) = x(5); 00459 super::m_transformation_matrix(1, 2) = x(6); 00460 super::m_transformation_matrix(1, 3) = x(7); 00461 super::m_transformation_matrix(2, 0) = x(8); 00462 super::m_transformation_matrix(2, 1) = x(9); 00463 super::m_transformation_matrix(2, 2) = x(10); 00464 super::m_transformation_matrix(2, 3) = x(11); 00465 super::m_transformation_matrix(3, 0) = 0; 00466 super::m_transformation_matrix(3, 1) = 0; 00467 super::m_transformation_matrix(3, 2) = 0; 00468 super::m_transformation_matrix(3, 3) = 1; 00469 } 00470 00471 00479 template <class T> 00480 typename EstimateAffineTransformation3D<T>::Matrix3T EstimateAffineTransformation3D<T>::getLinearTransformationMatrix() const 00481 { 00482 return super::m_transformation_matrix.block(0, 0, 3, 3); 00483 } 00484 00485 00493 template <class T> 00494 typename EstimateAffineTransformation3D<T>::Vector3T EstimateAffineTransformation3D<T>::getTranslationVector() const 00495 { 00496 return super::m_transformation_matrix.block(0, 3, 3, 1); 00497 } 00498 00499 00500 } // namespace TRTK 00501 00502 00503 #endif // ESTIMATE_AFFINE_TRANSFORMATION_3D_HPP_1790843183