1 #include "rgrl_internal_util.h"
2 //
3 #include <rgrl/rgrl_trans_rigid.h>
4 #include <rgrl/rgrl_trans_translation.h>
5 #include <rgrl/rgrl_trans_similarity.h>
6 #include <rgrl/rgrl_trans_affine.h>
7 #include <rgrl/rgrl_trans_homography2d.h>
8
9 #include "vnl/vnl_matrix_fixed.h"
10 #include "vnl/vnl_vector.h"
11
12
13 // CAUTION: NO boundary check for the purpose of efficiency
14 static
15 void
copy_matrix_at(vnl_matrix_fixed<double,3,3> & dest,unsigned int rp,unsigned int cp,vnl_matrix<double> const & src)16 copy_matrix_at( vnl_matrix_fixed<double, 3, 3>& dest, unsigned int rp, unsigned int cp,
17 vnl_matrix<double>const& src )
18 {
19 for ( unsigned i=0; i<src.rows(); ++i )
20 for ( unsigned j=0; j<src.cols(); ++j )
21 dest(rp+i, cp+j) = src(i,j);
22 }
23
24 // CAUTION: NO boundary check for the purpose of efficiency
25 static
26 void
copy_column_vector_at(vnl_matrix_fixed<double,3,3> & dest,unsigned int rp,unsigned int cp,vnl_vector<double> const & src)27 copy_column_vector_at( vnl_matrix_fixed<double, 3, 3>& dest, unsigned int rp, unsigned int cp,
28 vnl_vector<double>const& src )
29 {
30 for ( unsigned i=0; i<src.size(); ++i )
31 dest(rp+i, cp) = src(i);
32 }
33
34
35 bool
rgrl_internal_util_upgrade_to_homography2D(vnl_matrix_fixed<double,3,3> & init_H,rgrl_transformation const & cur_transform)36 rgrl_internal_util_upgrade_to_homography2D( vnl_matrix_fixed<double, 3, 3>& init_H,
37 rgrl_transformation const& cur_transform )
38 {
39 // get initialization
40 init_H.set_identity();
41
42 if ( cur_transform.is_type( rgrl_trans_homography2d::type_id() ) )
43 {
44 auto const& trans = static_cast<rgrl_trans_homography2d const&>( cur_transform );
45 init_H = trans.H();
46 return true;
47 }
48 else if ( cur_transform.is_type( rgrl_trans_affine::type_id() ) ) {
49 auto const& trans = static_cast<rgrl_trans_affine const&>( cur_transform );
50 if ( trans.t().size() != 2 )
51 return false;
52 copy_matrix_at( init_H, 0, 0, trans.A() );
53 copy_column_vector_at( init_H, 0, 2, trans.t() );
54 return true;
55 }
56 else if ( cur_transform.is_type( rgrl_trans_similarity::type_id() ) ) {
57 auto const& trans = static_cast<rgrl_trans_similarity const&>( cur_transform );
58 if ( trans.t().size() != 2 )
59 return false;
60 copy_matrix_at( init_H, 0, 0, trans.A() );
61 copy_column_vector_at( init_H, 0, 2, trans.t() );
62 return true;
63 }
64 else if ( cur_transform.is_type( rgrl_trans_rigid::type_id() ) ) {
65 auto const& trans = static_cast<rgrl_trans_rigid const&>( cur_transform );
66 if ( trans.t().size() != 2 )
67 return false;
68 copy_matrix_at( init_H, 0, 0, trans.R() );
69 copy_column_vector_at( init_H, 0, 2, trans.t() );
70 return true;
71 }
72 else if ( cur_transform.is_type( rgrl_trans_translation::type_id() ) ) {
73 auto const& trans = static_cast<rgrl_trans_translation const&>( cur_transform );
74 if ( trans.t().size() != 2 )
75 return false;
76 copy_column_vector_at( init_H, 0, 2, trans.t() );
77 return true;
78 }
79 else {
80 return false;
81 }
82 }
83