contrib/rpl/rgrl/rgrl_internal_util.cxx Source File

Go to the documentation of this file.
00001 #include <rgrl/rgrl_trans_rigid.h>
00002 #include <rgrl/rgrl_trans_translation.h>
00003 #include <rgrl/rgrl_trans_similarity.h>
00004 #include <rgrl/rgrl_trans_affine.h>
00005 #include <rgrl/rgrl_trans_homography2d.h>
00006 
00007 #include <vnl/vnl_matrix_fixed.h>
00008 #include <vnl/vnl_vector.h>
00009 
00010 
00011 // CAUTION: NO boundary check for the purpose of efficiency
00012 static
00013 void
00014 copy_matrix_at( vnl_matrix_fixed<double, 3, 3>& dest, unsigned int rp, unsigned int cp,
00015                 vnl_matrix<double>const& src )
00016 {
00017   for ( unsigned i=0; i<src.rows(); ++i )
00018     for ( unsigned j=0; j<src.cols(); ++j )
00019       dest(rp+i, cp+j) = src(i,j);
00020 }
00021 
00022 // CAUTION: NO boundary check for the purpose of efficiency
00023 static
00024 void
00025 copy_column_vector_at( vnl_matrix_fixed<double, 3, 3>& dest, unsigned int rp, unsigned int cp,
00026                        vnl_vector<double>const& src )
00027 {
00028   for ( unsigned i=0; i<src.size(); ++i )
00029     dest(rp+i, cp) = src(i);
00030 }
00031 
00032 
00033 bool
00034 rgrl_internal_util_upgrade_to_homography2D( vnl_matrix_fixed<double, 3, 3>& init_H,
00035                                             rgrl_transformation const& cur_transform )
00036 {
00037 
00038   // get initialization
00039   init_H.set_identity();
00040 
00041   if ( cur_transform.is_type( rgrl_trans_homography2d::type_id() ) ) {
00042 
00043     rgrl_trans_homography2d const& trans = static_cast<rgrl_trans_homography2d const&>( cur_transform );
00044     init_H = trans.H();
00045     return true;
00046 
00047   } else if ( cur_transform.is_type( rgrl_trans_affine::type_id() ) ) {
00048     rgrl_trans_affine const& trans = static_cast<rgrl_trans_affine const&>( cur_transform );
00049     if( trans.t().size() != 2 )
00050       return false;
00051     copy_matrix_at( init_H, 0, 0, trans.A() );
00052     copy_column_vector_at( init_H, 0, 2, trans.t() );
00053     return true;
00054 
00055   } else if ( cur_transform.is_type( rgrl_trans_similarity::type_id() ) ) {
00056     rgrl_trans_similarity const& trans = static_cast<rgrl_trans_similarity const&>( cur_transform );
00057     if( trans.t().size() != 2 )
00058       return false;
00059     copy_matrix_at( init_H, 0, 0, trans.A() );
00060     copy_column_vector_at( init_H, 0, 2, trans.t() );
00061     return true;
00062 
00063   } else if ( cur_transform.is_type( rgrl_trans_rigid::type_id() ) ) {
00064     rgrl_trans_rigid const& trans = static_cast<rgrl_trans_rigid const&>( cur_transform );
00065     if( trans.t().size() != 2 )
00066       return false;
00067     copy_matrix_at( init_H, 0, 0, trans.R() );
00068     copy_column_vector_at( init_H, 0, 2, trans.t() );
00069     return true;
00070 
00071   } else if ( cur_transform.is_type( rgrl_trans_translation::type_id() ) ) {
00072     rgrl_trans_translation const& trans = static_cast<rgrl_trans_translation const&>( cur_transform );
00073     if( trans.t().size() != 2 )
00074       return false;
00075     copy_column_vector_at( init_H, 0, 2, trans.t() );
00076     return true;
00077 
00078   } else {
00079     return false;
00080   }
00081 }