bfl_toolkit.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2009  Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
00002 
00003 // Author: Tinne De Laet 
00004 // Maintainer: Tinne De Laet
00005 
00006 
00007 // This library is free software; you can redistribute it and/or
00008 // modify it under the terms of the GNU Lesser General Public
00009 // License as published by the Free Software Foundation; either
00010 // version 2.1 of the License, or (at your option) any later version.
00011 
00012 // This library is distributed in the hope that it will be useful,
00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015 // Lesser General Public License for more details.
00016 
00017 // You should have received a copy of the GNU Lesser General Public
00018 // License along with this library; if not, write to the Free Software
00019 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00020 
00021 #include "bfl_toolkit.hpp"
00022 #include <rtt/Property.hpp>
00023 #include <rtt/PropertyBag.hpp>
00024 #include <rtt/TemplateTypeInfo.hpp>
00025 #include <rtt/Operators.hpp>
00026 #include <rtt/OperatorTypes.hpp>
00027 #include <rtt/Types.hpp>
00028 #include <rtt/Logger.hpp>
00029 #include <rtt/DataSources.hpp>
00030 #include <rtt/mystd.hpp>
00031 #include <rtt/os/StartStopManager.hpp>
00032 #include <rtt/Toolkit.hpp>
00033 
00034 #include "../../wrappers/matrix/vector_wrapper.h"
00035 #include "../../wrappers/matrix/matrix_wrapper.h"
00036 #include "../../bfl_constants.h"
00037 #include "../../sample/sample.h"
00038 
00039 #include <rtt/VectorTemplateComposition.hpp>
00040 #include "SampleComposition.hpp"
00041 
00042 
00043 /*
00044 std::ostream& operator<<(std::ostream& os, const Probability p) {
00045  return os << (double)p;
00046 }
00047 
00048 std::istream& operator>>(std::istream& is, Probability p) {
00049  char c;
00050  double p;
00051  return os >> p ;
00052 }
00053 */
00054 
00055 
00056 
00057 #ifndef __PROB_STREAM__
00058 #define __PROB_STREAM__
00059 namespace BFL
00060 {
00061   using namespace std;
00062   ostream & operator<< (ostream & stream, Probability& prob)
00063     {
00064       stream << prob.getValue() << endl;
00065       return stream;
00066     }
00067 
00068   istream & operator>> (istream & stream, Probability& prob)
00069     {
00070         double value;
00071         stream >> value;
00072         prob = Probability(value);
00073         return stream; 
00074     }
00075 };
00076 #endif //__PROBSTREAM__
00077 
00078 
00079 using namespace MatrixWrapper;
00080 namespace BFL{ 
00081 
00082     using namespace RTT;
00083     using namespace RTT::detail;
00084 
00085     bflToolkitPlugin bflToolkit;
00086 
00087     struct ProbabilityTypeInfo : public TemplateTypeInfo<Probability,true>
00088     {
00089         ProbabilityTypeInfo():TemplateTypeInfo<Probability,true>("Probability")
00090         {
00091         };
00092         virtual bool decomposeTypeImpl(const Probability& prob, PropertyBag& targetbag) const
00093         {
00094                 targetbag.setType("Probability");
00095                 double probDouble = prob.getValue();
00096                 targetbag.add( new Property<double>("Probability","Probability value", probDouble ) ); // Put variables in the bag
00097                 return true;
00098         };
00099         virtual bool composeTypeImpl(const PropertyBag& bag, Probability& prob) const
00100         {
00101                 Property<double>* probability = bag.getProperty<double>("Probability");
00102                 if (!probability)
00103                     return false;
00104                 prob = (probability)->get();
00105                 return true;
00106         };
00107     };
00108 
00109 
00110     
00111     struct VectorTypeInfo : public TemplateContainerTypeInfo<ColumnVector,int,double,ArrayIndexChecker<ColumnVector >, VectorAssignChecker<ColumnVector >,true>
00112     {
00113         VectorTypeInfo():TemplateContainerTypeInfo<ColumnVector,int,double,ArrayIndexChecker<ColumnVector>, VectorAssignChecker<ColumnVector >, true >("ColumnVector")
00114         {
00115         };
00116         
00117         bool decomposeTypeImpl(const ColumnVector& vec, PropertyBag& targetbag) const
00118         {
00119             targetbag.setType("ColumnVector");
00120             int dimension = vec.size();
00121             std::string str;
00122             
00123             for ( int i=1; i <= dimension ; i++){
00124                 std::stringstream out;
00125                 out << i;
00126                 str = out.str();
00127                 targetbag.add( new Property<double>(str, str +"th element of vector",vec(i)) ); // Put variables in the bag
00128             }
00129 
00130             return true;
00131         };
00132 
00133         bool composeTypeImpl(const PropertyBag& bag, ColumnVector& result) const{
00134 
00135             if ( bag.getType() == "ColumnVector" ) {
00136                 int dimension = bag.size();
00137                 result.resize( dimension );
00138                 
00139                 // Get values
00140                 for (int i = 1; i <= dimension ; i++) {
00141                     std::stringstream out;
00142                     out << i;
00143                     Property<double>* elem = bag.getProperty<double>(out.str());
00144                     if(elem->ready())
00145                         result(i) = elem->get();
00146                     else{
00147                         log(Error)<<"Could not read element "<<i<<endlog();
00148                         return false;
00149                     }
00150                 }
00151             }else{
00152                 log(Error) << "Composing Property< ColumnVector > :"
00153                            << " type mismatch, got type '"<< bag.getType()
00154                            << "', expected type "<<"ColumnVector."<<endlog();
00155                 return false;
00156             }
00157             return true;
00158         };
00159     };
00160 
00161     struct RVectorTypeInfo : public TemplateContainerTypeInfo<RowVector,int,double,ArrayIndexChecker<RowVector >, VectorAssignChecker<RowVector >,true>
00162     {
00163         RVectorTypeInfo():TemplateContainerTypeInfo<RowVector,int,double,ArrayIndexChecker<RowVector>, VectorAssignChecker<RowVector >, true >("RowVector")
00164         {
00165         };
00166 
00167         bool decomposeTypeImpl(const RowVector& vec, PropertyBag& targetbag) const
00168         {
00169             targetbag.setType("RowVector");
00170             int dimension = vec.size();
00171             std::string str;
00172 
00173 
00174             for ( int i=1; i <= dimension ; i++){
00175                 std::stringstream out;
00176                 out << i;
00177                 str = out.str();
00178                 targetbag.add( new Property<double>(str, str +"th element of vector",vec(i)) ); // Put variables in the bag
00179             }
00180 
00181             return true;
00182         };
00183 
00184         bool composeTypeImpl(const PropertyBag& bag, RowVector& result) const{
00185 
00186             if ( bag.getType() == "RowVector" ) {
00187                 int dimension = bag.size();
00188                 result.resize( dimension );
00189 
00190                 // Get values
00191                 for (int i = 1; i <= dimension ; i++) {
00192                     std::stringstream out;
00193                     out << i;
00194                     Property<double>* elem = bag.getProperty<double>(out.str());
00195                     if(elem->ready())
00196                         result(i) = elem->get();
00197                     else{
00198                         log(Error)<<"Could not read element "<<i<<endlog();
00199                         return false;
00200                     }
00201                 }
00202             }else{
00203                 log(Error) << "Composing Property< RowVector > :"
00204                            << " type mismatch, got type '"<< bag.getType()
00205                            << "', expected type "<<"RowVector."<<endlog();
00206                 return false;
00207             }
00208             return true;
00209         };
00210     };
00211 
00212     struct MatrixTypeInfo : public TemplateContainerTypeInfo<Matrix,int,RowVector,MatrixIndexChecker<Matrix>, MatrixAssignChecker<Matrix>, true>
00213     {
00214         MatrixTypeInfo():TemplateContainerTypeInfo<Matrix,int,RowVector,MatrixIndexChecker<Matrix>, MatrixAssignChecker<Matrix>,true>("Matrix"){
00215         };
00216 
00217         bool decomposeTypeImpl(const Matrix& mat, PropertyBag& targetbag) const{
00218             targetbag.setType("Matrix");
00219             unsigned int dimension = mat.rows();
00220 
00221             for ( unsigned int i=1; i <= dimension ; i++){
00222                 std::stringstream out;
00223                 out << i;
00224                 Property<PropertyBag>* row_bag = new Property<PropertyBag>(out.str(), out.str() +"th row of matrix"); 
00225                 Property<RowVector> row(out.str(), out.str() +"th row of matrix",mat.rowCopy(i))  ;
00226                 row.getTypeInfo()->decomposeType(row.getDataSource(),row_bag->value());
00227                 targetbag.add( row_bag ); // Put variables in the bag
00228             }
00229 
00230             return true;
00231         };
00232 
00233         bool composeTypeImpl(const PropertyBag& bag, Matrix& result) const{
00234             if ( bag.getType() == "Matrix" ) {
00235                 unsigned int rows = bag.size();
00236                 unsigned int cols = 0;
00237                 // Get values
00238                 for (unsigned int i = 1; i <= rows ; i++) {
00239                     std::stringstream out;
00240                     out << i;
00241                     Property<PropertyBag>* row_bag =  bag.getProperty<PropertyBag>(out.str());
00242                     if(row_bag==NULL){
00243                         log(Error)<<"Could not read row "<<i<<endlog();
00244                         return false;
00245                     }
00246                     Property<RowVector> row_p(row_bag->getName(),row_bag->getDescription());
00247                     if(!(row_p.getDataSource()->composeType(row_bag->getDataSource()))){
00248                         log(Error)<<"Could not decompose row "<<i<<endlog();
00249                         return false;
00250                     }
00251                     if(row_p.ready()){
00252                         if(i==1){
00253                             cols = row_p.get().size();
00254                             result.resize(rows,cols);
00255                         } else
00256                             if(row_p.get().size()!=cols){
00257                                 log(Error)<<"Row "<<i+1<<" size does not match matrix columns"<<endlog();
00258                                 return false;
00259                             }
00260                         for ( unsigned int j=1; j <= row_p.get().size() ; j++){
00261                             result(i,j)=row_p.get()(j);
00262                         }
00263                     }else{
00264                         log(Error)<<"Property of Row "<<i<<"was not ready for use"<<endlog();
00265                         return false;
00266                     }
00267                 }
00268             }else {
00269                 log(Error) << "Composing Property< Matrix > :"
00270                            << " type mismatch, got type '"<< bag.getType()
00271                            << "', expected type "<<"Matrix."<<endlog();
00272                 return false;
00273             }
00274             return true;
00275         };
00276     };
00277 
00278     struct SymmetricMatrixTypeInfo : public TemplateContainerTypeInfo<SymmetricMatrix,int,RowVector,MatrixIndexChecker<SymmetricMatrix>, MatrixAssignChecker<SymmetricMatrix>, true>
00279     {
00280         SymmetricMatrixTypeInfo():TemplateContainerTypeInfo<SymmetricMatrix,int,RowVector,MatrixIndexChecker<SymmetricMatrix>, MatrixAssignChecker<SymmetricMatrix>,true>("SymmetricMatrix"){
00281         };
00282 
00283         bool decomposeTypeImpl(const SymmetricMatrix& mat, PropertyBag& targetbag) const{
00284             targetbag.setType("SymmetricMatrix");
00285             unsigned int dimension = mat.rows();
00286 
00287             for ( unsigned int i=1; i <= dimension ; i++){
00288                 std::stringstream out;
00289                 out << i;
00290                 //targetbag.add( new Property<RowVector >(out.str(), out.str() +"th row of matrix",((Matrix)mat).rowCopy(i) )); // Put variables in the bag
00291                 Property<PropertyBag>* row_bag = new Property<PropertyBag>(out.str(), out.str() +"th row of matrix"); 
00292                 Property<RowVector> row(out.str(), out.str() +"th row of matrix",((Matrix)mat).rowCopy(i))  ;
00293                 row.getTypeInfo()->decomposeType(row.getDataSource(),row_bag->value());
00294                 targetbag.add( row_bag ); // Put variables in the bag
00295             }
00296 
00297             return true;
00298         };
00299 
00300         bool composeTypeImpl(const PropertyBag& bag, SymmetricMatrix& result) const{
00301             Matrix matrix;
00302             if ( bag.getType() == "SymmetricMatrix" ) {
00303                 unsigned int rows = bag.size();
00304                 unsigned int cols = 0;
00305                 // Get values
00306                 for (unsigned int i = 1; i <= rows ; i++) {
00307                     std::stringstream out;
00308                     out << i;
00309                     Property<PropertyBag>* row_bag =  bag.getProperty<PropertyBag>(out.str());
00310                     if(row_bag==NULL){
00311                         log(Error)<<"Could not read row "<<i<<endlog();
00312                         return false;
00313                     }
00314                     Property<RowVector > row_p(row_bag->getName(),row_bag->getDescription());
00315                     if(!(row_p.getDataSource()->composeType(row_bag->getDataSource()))){
00316                         log(Error)<<"Could not decompose row "<<i<<endlog();
00317                         return false;
00318                     }
00319                     if(row_p.ready()){
00320                         if(i==1){
00321                             cols = row_p.get().size();
00322                             matrix.resize(rows,cols);
00323                         } else
00324                             if(row_p.get().size()!=cols){
00325                                 log(Error)<<"Row "<<i+1<<" size does not match matrix columns"<<endlog();
00326                                 return false;
00327                             }
00328                         for ( unsigned int j=1; j <= row_p.get().size() ; j++){
00329                             matrix(i,j)=row_p.get()(j);
00330                         }
00331                     }else{
00332                         log(Error)<<"Property of Row "<<i<<"was not ready for use"<<endlog();
00333                         return false;
00334                     }
00335                 }
00336             }else {
00337                 log(Error) << "Composing Property< SymmetricMatrix > :"
00338                            << " type mismatch, got type '"<< bag.getType()
00339                            << "', expected type "<<"SymmetricMatrix."<<endlog();
00340                 return false;
00341             }
00342             matrix.convertToSymmetricMatrix(result);
00343             return true;
00344         };
00345     };
00346 
00347     struct vector_index
00348         : public std::binary_function<const ColumnVector&, int, double>
00349     {
00350         double operator()(const ColumnVector& v, int index) const
00351         {
00352             if ( index > (int)(v.size()) || index < 1)
00353                 return NAN;
00354             return v(index);
00355         }
00356     };
00357 
00358     struct rvector_index
00359         : public std::binary_function<const RowVector&, int, double>
00360     {
00361         double operator()(const RowVector& v, int index) const
00362         {
00363             if ( index > (int)(v.size()) || index < 1)
00364                 return NAN;
00365             return v(index);
00366         }
00367     };
00368 
00369     struct get_size
00370         : public std::unary_function<const ColumnVector&, int>
00371     {
00372         int operator()(const ColumnVector& cont ) const
00373         {
00374             return cont.size();
00375         }
00376     };
00377 
00378     struct rget_size
00379         : public std::unary_function<const RowVector&, int>
00380     {
00381         int operator()(const RowVector& cont ) const
00382         {
00383             return cont.size();
00384         }
00385     };
00386 
00387     struct vector_index_constructor
00388         : public std::unary_function<int,const ColumnVector&>
00389     {
00390         typedef const ColumnVector& (Signature)( int );
00391         mutable boost::shared_ptr< ColumnVector > ptr;
00392         vector_index_constructor() :
00393             ptr( new ColumnVector() ){}
00394         const ColumnVector& operator()(int size ) const
00395         {
00396             ptr->resize(size);
00397             return *(ptr);
00398         }
00399     };
00400 
00401     struct rvector_index_constructor
00402         : public std::unary_function<int,const RowVector&>
00403     {
00404         typedef const RowVector& (Signature)( int );
00405         mutable boost::shared_ptr< RowVector > ptr;
00406         rvector_index_constructor() :
00407             ptr( new RowVector() ){}
00408         const RowVector& operator()(int size ) const
00409         {
00410             ptr->resize(size);
00411             return *(ptr);
00412         }
00413     };
00414 
00415     struct matrix_index
00416         : public std::binary_function<const Matrix&, int, const RowVector&>
00417     {
00418         const RowVector& operator()(const Matrix& m, int index) const
00419         {
00420             if ( index > (int)(m.rows()) || index < 1)
00421                 {
00422                     log(Error) << "index error" << endlog();
00423                     return RowVector(0);
00424                 }
00425             return m.rowCopy(index);
00426         }
00427     };
00428 
00429     //struct matrix_index
00430     //    : public std::ternary_function<const Matrix&, int, int, double>
00431     //{
00432     //    double operator()(const Matrix& m, int i, int j) const{
00433     //        if ( i > (int)(m.rows()) || i < 1 || j<1 || j> (int)(m.columns()))
00434     //            return NAN;
00435     //        return m(i,j);
00436     //    }
00437     //};
00438 
00439     struct matrix_i_j_constructor
00440         : public std::binary_function<int,int,const Matrix&>
00441     {
00442         typedef const Matrix& (Signature)( int, int );
00443         mutable boost::shared_ptr< Matrix > ptr;
00444         matrix_i_j_constructor() :
00445             ptr( new Matrix() ){}
00446         const Matrix& operator()(int size1,int size2) const
00447         {
00448             ptr->resize(size1,size2);
00449             return *(ptr);
00450         }
00451     };
00452 
00453     struct symmetricMatrix_index_constructor
00454         : public std::unary_function<int,const SymmetricMatrix&>
00455     {
00456         typedef const SymmetricMatrix& (Signature)( int );
00457         mutable boost::shared_ptr< SymmetricMatrix > ptr;
00458         symmetricMatrix_index_constructor() :
00459             ptr( new SymmetricMatrix() ){}
00460         const SymmetricMatrix& operator()(int size) const
00461         {
00462             ptr->resize(size);
00463             return *(ptr);
00464         }
00465     };
00466 
00467     struct Probability_ctor
00468         : public std::unary_function<double, const Probability&>
00469     {
00470         typedef const Probability& (Signature)( double );
00471         mutable boost::shared_ptr< Probability > ptr;
00472         Probability_ctor()
00473             : ptr( new Probability() ) {}
00474         const Probability& operator()( double value ) const
00475         {
00476             //ptr = new Probability(value);
00477             //return *(ptr);
00478             return *(new Probability(value));
00479         }
00480     };
00481 
00482     std::string bflToolkitPlugin::getName()
00483     {
00484         return "bfl_toolkit";
00485     }
00486 
00487     bool bflToolkitPlugin::loadTypes()
00488     {
00489         RTT::TypeInfoRepository::Instance()->addType( new VectorTypeInfo() );
00490         RTT::TypeInfoRepository::Instance()->addType( new RVectorTypeInfo() );
00491         RTT::TypeInfoRepository::Instance()->addType( new MatrixTypeInfo() );
00492         RTT::TypeInfoRepository::Instance()->addType( new SymmetricMatrixTypeInfo() );
00493         RTT::TypeInfoRepository::Instance()->addType( new SampleTypeInfo<int>("SampleInt") );
00494         RTT::TypeInfoRepository::Instance()->addType( new SampleTypeInfo<double>("SampleDouble") );
00495         RTT::TypeInfoRepository::Instance()->addType( new SampleTypeInfo<ColumnVector>("SampleColumnVector") );
00496         RTT::TypeInfoRepository::Instance()->addType( new WeightedSampleTypeInfo<int>("WeightedSampleInt") );
00497         RTT::TypeInfoRepository::Instance()->addType( new WeightedSampleTypeInfo<double>("WeightedSampleDouble") );
00498         RTT::TypeInfoRepository::Instance()->addType( new WeightedSampleTypeInfo<ColumnVector>("WeightedSampleColumnVector") );
00499         RTT::TypeInfoRepository::Instance()->addType( new ProbabilityTypeInfo() );
00500 
00501         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<ColumnVector,true>("ColumnVectors") );
00502         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<RowVector,true>("RowVectors") );
00503         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<Matrix,true>("Matrixs") );
00504         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<SymmetricMatrix,true>("SymmetricMatrixs") );
00505         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<Sample<int>,false>("SampleInts") );
00506         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<Sample<double>,false>("SampleDoubles") );
00507         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<Sample<ColumnVector>,false>("SampleColumnVectors") );
00508         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<WeightedSample<int>,false>("WeightedSampleInts") );
00509         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<WeightedSample<double>,false>("WeightedSampleDoubles") );
00510         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<WeightedSample<ColumnVector>,false>("WeightedSampleColumnVectors") );
00511         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<Probability,false>("Probabilitys") );
00512 
00513         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<Sample<int> >,false>("VecSampleInts") );
00514         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<Sample<double> >,false>("VecSampleDoubles") );
00515         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<Sample<ColumnVector> >,false>("VecSampleColumnVectors") );
00516         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<WeightedSample<int> >,false>("VecWeightedSampleInts") );
00517         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<WeightedSample<double> >,false>("VecWeightedSampleDoubles") );
00518         RTT::TypeInfoRepository::Instance()->addType( new StdVectorTemplateTypeInfo<vector<WeightedSample<ColumnVector> >,false>("VecWeightedSampleColumnVectors") );
00519         return true;
00520     }
00521 
00522     bool bflToolkitPlugin::loadConstructors()
00523     {
00524         RTT::TypeInfoRepository::Instance()->type("ColumnVector")->addConstructor(newConstructor(vector_index_constructor()));
00525         RTT::TypeInfoRepository::Instance()->type("RowVector")->addConstructor(newConstructor(rvector_index_constructor()));
00526         RTT::TypeInfoRepository::Instance()->type("Matrix")->addConstructor(newConstructor(matrix_i_j_constructor()));
00527         RTT::TypeInfoRepository::Instance()->type("SymmetricMatrix")->addConstructor(newConstructor(symmetricMatrix_index_constructor()));
00528         //RTT::TypeInfoRepository::Instance()->type("ColumnVector")->addConstructor(newConstructor(vector_index_value_constructor()));
00529         //
00530         RTT::TypeInfoRepository::Instance()->type("ColumnVectors")->addConstructor(newConstructor(stdvector_ctor<ColumnVector>() ) );
00531         RTT::TypeInfoRepository::Instance()->type("ColumnVectors")->addConstructor(newConstructor(stdvector_ctor2<ColumnVector>() ) );
00532         RTT::TypeInfoRepository::Instance()->type("ColumnVectors")->addConstructor(new StdVectorBuilder<ColumnVector>() );
00533 
00534         RTT::TypeInfoRepository::Instance()->type("RowVectors")->addConstructor(newConstructor(stdvector_ctor<RowVector>() ) );
00535         RTT::TypeInfoRepository::Instance()->type("RowVectors")->addConstructor(newConstructor(stdvector_ctor2<RowVector>() ) );
00536         RTT::TypeInfoRepository::Instance()->type("RowVectors")->addConstructor(new StdVectorBuilder<RowVector>() );
00537 
00538         RTT::TypeInfoRepository::Instance()->type("Matrixs")->addConstructor(newConstructor(stdvector_ctor<Matrix>() ) );
00539         RTT::TypeInfoRepository::Instance()->type("Matrixs")->addConstructor(newConstructor(stdvector_ctor2<Matrix>() ) );
00540         RTT::TypeInfoRepository::Instance()->type("Matrixs")->addConstructor(new StdVectorBuilder<Matrix>() );
00541 
00542         RTT::TypeInfoRepository::Instance()->type("SymmetricMatrixs")->addConstructor(newConstructor(stdvector_ctor<SymmetricMatrix>() ) );
00543         RTT::TypeInfoRepository::Instance()->type("SymmetricMatrixs")->addConstructor(newConstructor(stdvector_ctor2<SymmetricMatrix>() ) );
00544         RTT::TypeInfoRepository::Instance()->type("SymmetricMatrixs")->addConstructor(new StdVectorBuilder<SymmetricMatrix>() );
00545 
00546         RTT::TypeInfoRepository::Instance()->type("SampleInts")->addConstructor(newConstructor(stdvector_ctor<Sample<int> >() ) );
00547         RTT::TypeInfoRepository::Instance()->type("SampleInts")->addConstructor(newConstructor(stdvector_ctor2<Sample<int> >() ) );
00548         RTT::TypeInfoRepository::Instance()->type("SampleInts")->addConstructor(new StdVectorBuilder<Sample<int> >() );
00549 
00550         RTT::TypeInfoRepository::Instance()->type("SampleDoubles")->addConstructor(newConstructor(stdvector_ctor<Sample<double> >() ) );
00551         RTT::TypeInfoRepository::Instance()->type("SampleDoubles")->addConstructor(newConstructor(stdvector_ctor2<Sample<double> >() ) );
00552         RTT::TypeInfoRepository::Instance()->type("SampleDoubles")->addConstructor(new StdVectorBuilder<Sample<double> >() );
00553 
00554         RTT::TypeInfoRepository::Instance()->type("SampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor<Sample<ColumnVector> >() ) );
00555         RTT::TypeInfoRepository::Instance()->type("SampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor2<Sample<ColumnVector> >() ) );
00556         RTT::TypeInfoRepository::Instance()->type("SampleColumnVectors")->addConstructor(new StdVectorBuilder<Sample<ColumnVector> >() );
00557 
00558 
00559         RTT::TypeInfoRepository::Instance()->type("WeightedSampleInts")->addConstructor(newConstructor(stdvector_ctor<WeightedSample<int> >() ) );
00560         RTT::TypeInfoRepository::Instance()->type("WeightedSampleInts")->addConstructor(newConstructor(stdvector_ctor2<WeightedSample<int> >() ) );
00561         RTT::TypeInfoRepository::Instance()->type("WeightedSampleInts")->addConstructor(new StdVectorBuilder<WeightedSample<int> >() );
00562 
00563         RTT::TypeInfoRepository::Instance()->type("WeightedSampleDoubles")->addConstructor(newConstructor(stdvector_ctor<WeightedSample<double> >() ) );
00564         RTT::TypeInfoRepository::Instance()->type("WeightedSampleDoubles")->addConstructor(newConstructor(stdvector_ctor2<WeightedSample<double> >() ) );
00565         RTT::TypeInfoRepository::Instance()->type("WeightedSampleDoubles")->addConstructor(new StdVectorBuilder<WeightedSample<double> >() );
00566 
00567         RTT::TypeInfoRepository::Instance()->type("WeightedSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor<WeightedSample<ColumnVector> >() ) );
00568         RTT::TypeInfoRepository::Instance()->type("WeightedSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor2<WeightedSample<ColumnVector> >() ) );
00569         RTT::TypeInfoRepository::Instance()->type("WeightedSampleColumnVectors")->addConstructor(new StdVectorBuilder<WeightedSample<ColumnVector> >() );
00570 
00571         RTT::TypeInfoRepository::Instance()->type("VecSampleInts")->addConstructor(newConstructor(stdvector_ctor<vector<Sample<int> > >() ) );
00572         RTT::TypeInfoRepository::Instance()->type("VecSampleInts")->addConstructor(newConstructor(stdvector_ctor2<vector<Sample<int> > >() ) );
00573         RTT::TypeInfoRepository::Instance()->type("VecSampleInts")->addConstructor(new StdVectorBuilder<vector<Sample<int> > >() );
00574 
00575         RTT::TypeInfoRepository::Instance()->type("VecSampleDoubles")->addConstructor(newConstructor(stdvector_ctor<vector<Sample<double> > >() ) );
00576         RTT::TypeInfoRepository::Instance()->type("VecSampleDoubles")->addConstructor(newConstructor(stdvector_ctor2<vector<Sample<double> > >() ) );
00577         RTT::TypeInfoRepository::Instance()->type("VecSampleDoubles")->addConstructor(new StdVectorBuilder<vector<Sample<double>  > >() );
00578 
00579         RTT::TypeInfoRepository::Instance()->type("VecSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor<vector<Sample<ColumnVector> > >() ) );
00580         RTT::TypeInfoRepository::Instance()->type("VecSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor2<vector<Sample<ColumnVector> > >() ) );
00581         RTT::TypeInfoRepository::Instance()->type("VecSampleColumnVectors")->addConstructor(new StdVectorBuilder<vector<Sample<ColumnVector> > >() );
00582 
00583         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleInts")->addConstructor(newConstructor(stdvector_ctor<vector<WeightedSample<int> > >() ) );
00584         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleInts")->addConstructor(newConstructor(stdvector_ctor2<vector<WeightedSample<int> > >() ) );
00585         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleInts")->addConstructor(new StdVectorBuilder<vector<WeightedSample<int> > >() );
00586 
00587         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleDoubles")->addConstructor(newConstructor(stdvector_ctor<vector<WeightedSample<double> > >() ) );
00588         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleDoubles")->addConstructor(newConstructor(stdvector_ctor2<vector<WeightedSample<double> > >() ) );
00589         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleDoubles")->addConstructor(new StdVectorBuilder<vector<WeightedSample<double>  > >() );
00590 
00591         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor<vector<WeightedSample<ColumnVector> > >() ) );
00592         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleColumnVectors")->addConstructor(newConstructor(stdvector_ctor2<vector<WeightedSample<ColumnVector> > >() ) );
00593         RTT::TypeInfoRepository::Instance()->type("VecWeightedSampleColumnVectors")->addConstructor(new StdVectorBuilder<vector<WeightedSample<ColumnVector> > >() );
00594 
00595         RTT::TypeInfoRepository::Instance()->type("Probabilitys")->addConstructor(newConstructor(stdvector_ctor<Probability>() ) );
00596         RTT::TypeInfoRepository::Instance()->type("Probabilitys")->addConstructor(newConstructor(stdvector_ctor2<Probability>() ) );
00597         RTT::TypeInfoRepository::Instance()->type("Probabilitys")->addConstructor(new StdVectorBuilder<Probability>() );
00598 
00599         RTT::OperatorRepository::Instance()->add( newBinaryOperator( "[]", stdvector_index<ColumnVector>() ) );
00600         RTT::OperatorRepository::Instance()->add( newDotOperator( "size", RTT::get_size<const std::vector<ColumnVector>&>() ) );
00601 
00602         RTT::TypeInfoRepository::Instance()->type("SampleInt")->addConstructor(newConstructor(Sample_ctor<int>() ) );
00603         RTT::TypeInfoRepository::Instance()->type("SampleDouble")->addConstructor(newConstructor(Sample_ctor<double>() ) );
00604         RTT::TypeInfoRepository::Instance()->type("SampleColumnVector")->addConstructor(newConstructor(Sample_ctor<ColumnVector>() ) );
00605         RTT::TypeInfoRepository::Instance()->type("WeightedSampleInt")->addConstructor(newConstructor(WeightedSample_ctor<int>() ) );
00606         RTT::TypeInfoRepository::Instance()->type("WeightedSampleDouble")->addConstructor(newConstructor(WeightedSample_ctor<double>() ) );
00607         RTT::TypeInfoRepository::Instance()->type("WeightedSampleColumnVector")->addConstructor(newConstructor(WeightedSample_ctor<ColumnVector>() ) );
00608 
00609         RTT::TypeInfoRepository::Instance()->type("Probability")->addConstructor(newConstructor(Probability_ctor() ) );
00610 
00611         return true;
00612     }
00613 
00614     bool bflToolkitPlugin::loadOperators()
00615     {
00616         RTT::OperatorRepository::Instance()->add( newBinaryOperator( "[]", vector_index() ) );
00617         RTT::OperatorRepository::Instance()->add( newBinaryOperator( "[]", rvector_index() ) );
00618         RTT::OperatorRepository::Instance()->add( newDotOperator( "size", get_size() ) );
00619         RTT::OperatorRepository::Instance()->add( newDotOperator( "size", rget_size() ) );
00620         RTT::OperatorRepository::Instance()->add( newBinaryOperator( "+", std::plus<ColumnVector>() ) );
00621         RTT::OperatorRepository::Instance()->add( newBinaryOperator( "+", std::plus<RowVector>() ) );
00622         //RTT::OperatorRepository::Instance()->add( newTernaryOperator( "[,]", matrix_index() ) );
00623         //RTT::OperatorRepository::Instance()->add( newBinaryOperator( "[]", matrix_index() ) );
00624         return true;
00625     }
00626 }
00627 
00628 ORO_TOOLKIT_PLUGIN(BFL::bflToolkit)


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Sun Oct 5 2014 22:29:52