D:/Programmierung/C/TRTK/include/TRTK/EstimateAffineTransformation2D.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_2D_HPP_2437243897 00020 #define ESTIMATE_AFFINE_TRANSFORMATION_2D_HPP_2437243897 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 "EstimateTransformation2D.hpp" 00033 00034 00035 namespace TRTK 00036 { 00037 00038 00188 template <class T> 00189 class EstimateAffineTransformation2D : public EstimateTransformation2D<T> 00190 { 00191 private: 00192 00193 typedef EstimateTransformation2D<T> super; 00194 00195 public: 00196 00197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00198 00199 typedef T value_type; 00200 00201 typedef typename super::VectorXT VectorXT; 00202 typedef typename super::MatrixXT MatrixXT; 00203 typedef typename super::Vector2T Vector2T; 00204 typedef typename super::Vector3T Vector3T; 00205 typedef typename super::Vector4T Vector4T; 00206 typedef typename super::RowVector2T RowVector2T; 00207 typedef typename super::RowVector3T RowVector3T; 00208 typedef typename super::Matrix2T Matrix2T; 00209 typedef typename super::Matrix3T Matrix3T; 00210 typedef typename super::Matrix4T Matrix4T; 00211 00212 using super::NOT_ENOUGH_POINTS; 00213 using super::UNEQUAL_NUMBER_OF_POINTS; 00214 using super::UNKNOWN_ERROR; 00215 using super::WRONG_POINT_SIZE; 00216 00217 EstimateAffineTransformation2D(); 00218 00219 EstimateAffineTransformation2D(const std::vector<Coordinate<T> > & source_points, 00220 const std::vector<Coordinate<T> > & target_points); 00221 00222 EstimateAffineTransformation2D(const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & source_points, 00223 const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & target_points); 00224 00225 EstimateAffineTransformation2D(const std::vector<Vector3T> & source_points, 00226 const std::vector<Vector3T> & target_points); 00227 00228 virtual ~EstimateAffineTransformation2D(); 00229 00230 virtual void compute(); 00231 00232 Matrix2T getLinearTransformationMatrix() const; 00233 Vector2T getTranslationVector() const; 00234 }; 00235 00236 00242 template <class T> 00243 EstimateAffineTransformation2D<T>::EstimateAffineTransformation2D() 00244 { 00245 } 00246 00247 00269 template <class T> 00270 EstimateAffineTransformation2D<T>::EstimateAffineTransformation2D(const std::vector<Coordinate<T> > & source_points, 00271 const std::vector<Coordinate<T> > & target_points) 00272 { 00273 super::setSourcePoints(source_points); 00274 super::setTargetPoints(target_points); 00275 } 00276 00277 00291 template <class T> 00292 EstimateAffineTransformation2D<T>::EstimateAffineTransformation2D(const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & source_points, 00293 const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & target_points) 00294 { 00295 super::setSourcePoints(source_points); 00296 super::setTargetPoints(target_points); 00297 } 00298 00299 00313 template <class T> 00314 EstimateAffineTransformation2D<T>::EstimateAffineTransformation2D(const std::vector<Vector3T> & source_points, 00315 const std::vector<Vector3T> & target_points) 00316 { 00317 super::setSourcePoints(source_points); 00318 super::setTargetPoints(target_points); 00319 } 00320 00321 00327 template <class T> 00328 EstimateAffineTransformation2D<T>::~EstimateAffineTransformation2D() 00329 { 00330 } 00331 00332 00353 template <class T> 00354 void EstimateAffineTransformation2D<T>::compute() 00355 { 00356 if (super::m_source_points.size() != super::m_target_points.size()) 00357 { 00358 ErrorObj error; 00359 error.setClassName("EstimateAffineTransformation2D<T>"); 00360 error.setFunctionName("compute"); 00361 error.setErrorMessage("Point sets must have the same cardinality."); 00362 error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS); 00363 throw error; 00364 } 00365 00366 if (super::m_source_points.cols() < 3 || super::m_target_points.cols() < 3) 00367 { 00368 ErrorObj error; 00369 error.setClassName("EstimateAffineTransformation2D<T>"); 00370 error.setFunctionName("compute"); 00371 error.setErrorMessage("Not enough points to compute transformation matrix."); 00372 error.setErrorCode(NOT_ENOUGH_POINTS); 00373 throw error; 00374 } 00375 00376 /* 00377 00378 Explanation of the algorithm: 00379 00380 By using homogeneous coordinates an affine transformation can be described as follows: 00381 00382 | m_11 m_12 t_11 | | source_x | | target_x | 00383 | m_21 m_22 t_21 | * | source_y | = | target_y | 00384 | 0 0 1 | | 1 | | 1 | 00385 00386 The equation can be rewritten as: 00387 | m_11 | 00388 | m_12 | 00389 | source_x source_y 1 0 0 0 | * | t_11 | = | target_x | 00390 | 0 0 0 source_x source_y 1 | | m_21 | | target_y | 00391 | m_22 | 00392 | t_21 | 00393 00394 Or in short: A * x = b 00395 00396 By using more and more points this leads to an over-determined system, which can 00397 be solved via the pseudo inverse: 00398 00399 <==> A * x = b 00400 <==> A^T * A * x = A^T * b 00401 <==> inverse(A^T * A) * A^T * A * x = x = inverse(A^T * A) * A^T * b 00402 00403 Thus: 00404 00405 x = pseudo_inverse * b = [ inverse(A^T * A) * A^T ] * b 00406 00407 */ 00408 00409 00410 const int number_points = super::m_source_points.cols(); 00411 00412 // fill matrix A with the above described entries 00413 00414 MatrixXT A(2 * number_points, 6); 00415 00416 const VectorXT ones = VectorXT::Ones(number_points); 00417 00418 A.setZero(); 00419 A.block(0 * number_points, 0, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); // source_x 00420 A.block(0 * number_points, 1, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); // source_y 00421 A.block(0 * number_points, 2, number_points, 1) = ones; 00422 A.block(1 * number_points, 3, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); // source_x 00423 A.block(1 * number_points, 4, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); // source_y 00424 A.block(1 * number_points, 5, number_points, 1) = ones; 00425 00426 // fill vector b 00427 00428 VectorXT b(2 * number_points, 1); 00429 b.block(0 * number_points, 0, number_points, 1) = super::m_target_points.block(0, 0, 1, number_points).transpose(); // target_x 00430 b.block(1 * number_points, 0, number_points, 1) = super::m_target_points.block(1, 0, 1, number_points).transpose(); // target_y 00431 00432 // compute pseudo inverse 00433 00434 MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose(); 00435 00436 // compute vector x whose components are the sought transformation entries 00437 00438 VectorXT x = pseudo_inverse * b; 00439 00440 super::m_transformation_matrix(0, 0) = x(0); 00441 super::m_transformation_matrix(0, 1) = x(1); 00442 super::m_transformation_matrix(0, 2) = x(2); 00443 super::m_transformation_matrix(1, 0) = x(3); 00444 super::m_transformation_matrix(1, 1) = x(4); 00445 super::m_transformation_matrix(1, 2) = x(5); 00446 super::m_transformation_matrix(2, 0) = 0; 00447 super::m_transformation_matrix(2, 1) = 0; 00448 super::m_transformation_matrix(2, 2) = 1; 00449 } 00450 00451 00459 template <class T> 00460 typename EstimateAffineTransformation2D<T>::Matrix2T EstimateAffineTransformation2D<T>::getLinearTransformationMatrix() const 00461 { 00462 return super::m_transformation_matrix.block(0, 0, 2, 2); 00463 } 00464 00465 00473 template <class T> 00474 typename EstimateAffineTransformation2D<T>::Vector2T EstimateAffineTransformation2D<T>::getTranslationVector() const 00475 { 00476 return super::m_transformation_matrix.block(0, 2, 2, 1); 00477 } 00478 00479 00480 } // namespace TRTK 00481 00482 00483 #endif // ESTIMATE_AFFINE_TRANSFORMATION_2D_HPP_2437243897