D:/Programmierung/C/TRTK/include/TRTK/EstimateRigidTransformation2D.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_RIGID_TRANSFORMATION_2D_HPP_4587327344 00020 #define ESTIMATE_RIGID_TRANSFORMATION_2D_HPP_4587327344 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 EstimateRigidTransformation2D : 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 EstimateRigidTransformation2D(); 00218 00219 EstimateRigidTransformation2D(const std::vector<Coordinate<T> > & source_points, 00220 const std::vector<Coordinate<T> > & target_points); 00221 00222 EstimateRigidTransformation2D(const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & source_points, 00223 const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & target_points); 00224 00225 EstimateRigidTransformation2D(const std::vector<Vector3T> & source_points, 00226 const std::vector<Vector3T> & target_points); 00227 00228 virtual ~EstimateRigidTransformation2D(); 00229 00230 virtual void compute(); 00231 00232 Matrix2T getRotationMatrix() const; 00233 Vector2T getTranslationVector() const; 00234 }; 00235 00236 00242 template <class T> 00243 EstimateRigidTransformation2D<T>::EstimateRigidTransformation2D() 00244 { 00245 } 00246 00247 00269 template <class T> 00270 EstimateRigidTransformation2D<T>::EstimateRigidTransformation2D(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 EstimateRigidTransformation2D<T>::EstimateRigidTransformation2D(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 EstimateRigidTransformation2D<T>::EstimateRigidTransformation2D(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 EstimateRigidTransformation2D<T>::~EstimateRigidTransformation2D() 00329 { 00330 } 00331 00332 00353 template <class T> 00354 void EstimateRigidTransformation2D<T>::compute() 00355 { 00356 if (super::m_source_points.size() != super::m_target_points.size()) 00357 { 00358 ErrorObj error; 00359 error.setClassName("EstimateRigidTransformation2D<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() < 2 || super::m_target_points.cols() < 2) 00367 { 00368 ErrorObj error; 00369 error.setClassName("EstimateRigidTransformation2D<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 a rigid transformation can be described as follows: 00381 00382 | cos phi -sin phi t_x | | source_x | | target_x | 00383 | sin phi cos phi t_y | * | source_y | = | target_y | 00384 | 0 0 1 | | 1 | | 1 | 00385 00386 The equation can be rewritten as: 00387 00388 | cos phi | 00389 | source_x -source_y 1 0 | * | sin phi | = | target_x | 00390 | source_y source_x 0 1 | | t_x | | target_y | 00391 | t_Y | 00392 00393 Or in short: A * x = b 00394 00395 By using more and more points this leads to an over-determined system, which can 00396 be solved via the pseudo inverse (i.e. A becomes A = [A1 A2 A3 ...]^T): 00397 00398 <==> A * x = b 00399 <==> A^T * A * x = A^T * b 00400 <==> inverse(A^T * A) * A^T * A * x = x = inverse(A^T * A) * A^T * b 00401 00402 Thus: 00403 00404 x = pseudo_inverse * b = [inverse(A^T * A) * A^T] * b 00405 00406 */ 00407 00408 const int number_points = super::m_source_points.cols(); 00409 00410 // fill matrix A with the above described entries 00411 00412 MatrixXT A(2*number_points, 4); 00413 00414 A.setZero(); 00415 A.block(0*number_points, 0, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00416 A.block(0*number_points, 1, number_points, 1) = -super::m_source_points.block(1, 0, 1, number_points).transpose(); 00417 A.block(0*number_points, 2, number_points, 1) = VectorXT::Ones(number_points); 00418 A.block(1*number_points, 0, number_points, 1) = super::m_source_points.block(1, 0, 1, number_points).transpose(); 00419 A.block(1*number_points, 1, number_points, 1) = super::m_source_points.block(0, 0, 1, number_points).transpose(); 00420 A.block(1*number_points, 3, number_points, 1) = VectorXT::Ones(number_points); 00421 00422 // fill vector b 00423 00424 VectorXT b(2*number_points, 1); 00425 b.block(0*number_points, 0, number_points, 1) = super::m_target_points.block(0, 0, 1, number_points).transpose(); 00426 b.block(1*number_points, 0, number_points, 1) = super::m_target_points.block(1, 0, 1, number_points).transpose(); 00427 00428 // compute pseudo inverse 00429 00430 MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose(); 00431 00432 // compute vector x whose components are/contain the sought transformation entries 00433 00434 VectorXT x = pseudo_inverse * b; 00435 00436 using std::atan2; 00437 using std::cos; 00438 using std::sin; 00439 00440 T phi = atan2(x(1), x(0)); // cos(phi) and sin(phi) by oneself might diverge from each other 00441 00442 super::m_transformation_matrix(0, 0) = cos(phi); 00443 super::m_transformation_matrix(0, 1) = -sin(phi); 00444 super::m_transformation_matrix(0, 2) = x(2); 00445 super::m_transformation_matrix(1, 0) = sin(phi); 00446 super::m_transformation_matrix(1, 1) = cos(phi); 00447 super::m_transformation_matrix(1, 2) = x(3); 00448 super::m_transformation_matrix(2, 0) = 0; 00449 super::m_transformation_matrix(2, 1) = 0; 00450 super::m_transformation_matrix(2, 2) = 1; 00451 } 00452 00453 00461 template <class T> 00462 typename EstimateRigidTransformation2D<T>::Matrix2T EstimateRigidTransformation2D<T>::getRotationMatrix() const 00463 { 00464 return super::m_transformation_matrix.block(0, 0, 2, 2); 00465 } 00466 00467 00475 template <class T> 00476 typename EstimateRigidTransformation2D<T>::Vector2T EstimateRigidTransformation2D<T>::getTranslationVector() const 00477 { 00478 return super::m_transformation_matrix.block(0, 2, 2, 1); 00479 } 00480 00481 00482 } // namespace TRTK 00483 00484 00485 #endif // ESTIMATE_RIGID_TRANSFORMATION_2D_HPP_4587327344