contrib/rpl/rgrl/rgrl_internal_util.cxx Source File

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