MatrixLoader.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the LABUST nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *  Author: Dula Nad
00035  *  Created: 01.02.2013.
00036  *********************************************************************/
00037 #ifndef MATRIXLOADER_HPP_
00038 #define MATRIXLOADER_HPP_
00039 #include <ros/ros.h>
00040 #include <Eigen/Dense>
00041 
00042 #include <string>
00043 
00044 namespace labust
00045 {
00046         namespace tools
00047         {
00048                 template <class Derived, class XmlRpcVal>
00049                 inline typename Eigen::MatrixBase<Derived>::Scalar      xmlRpcConvert(XmlRpcVal& data, bool intType)
00050                 {
00051                         return static_cast< typename Eigen::MatrixBase<Derived>::Scalar >(
00052                                         intType?static_cast<int>(data):static_cast<double>(data));
00053                 }
00054 
00058                 template <class Derived>
00059                 std::pair<int,int>      getMatrixParam_fixedsize(const ros::NodeHandle& nh,
00060                                 const std::string& name,
00061                                 const Eigen::MatrixBase<Derived>& matrix)
00062                 {
00063                         if (!nh.hasParam(name))
00064                         {
00065                                 ROS_WARN("Configuration parameter %s not found.", name.c_str());
00066                                 return std::make_pair(-1,-1);
00067                         }
00068                         else
00069                         {
00070                                 ROS_INFO("Found configuration parameter %s.", name.c_str());
00071                         }
00072 
00073                         XmlRpc::XmlRpcValue data;
00074                         nh.getParam(name, data);
00075                         ROS_ASSERT(data.getType() == XmlRpc::XmlRpcValue::TypeArray);
00076 
00077                         size_t col(0),row(0);
00078                         for(size_t i=0; i<data.size(); ++i)
00079                         {
00080                                 //Check validity
00081                                 bool doubleType = data[i].getType() == XmlRpc::XmlRpcValue::TypeDouble;
00082                                 bool intType = data[i].getType() == XmlRpc::XmlRpcValue::TypeInt;
00083                                 bool vectorType = data[i].getType() == XmlRpc::XmlRpcValue::TypeArray;
00084                                 ROS_ASSERT(doubleType || intType || vectorType);
00085 
00086                                 if (doubleType || intType)
00087                                 {
00088                                         const_cast< Eigen::MatrixBase<Derived>& >
00089                                         (matrix)(row,col++) = xmlRpcConvert<Derived>(data[i], intType);
00090 
00091                                         if (col>=matrix.cols())
00092                                         {
00093                                                 ++row;
00094                                                 col = col%matrix.cols();
00095                                         }
00096                                 }
00097                                 else
00098                                 {
00099                                         row = i;
00100                                         col = 0;
00101                                         for(size_t j=0; j<data[i].size(); ++j)
00102                                         {
00103                                                 //ROS_DEBUG("Access element %d %d",i,j);
00104                                                 bool intType = data[i][j].getType() == XmlRpc::XmlRpcValue::TypeInt;
00105                                                 const_cast< Eigen::MatrixBase<Derived>& >
00106                                                 (matrix)(row,col++) = xmlRpcConvert<Derived>(data[i][j], intType);
00107                                         }
00108                                 }
00109                         }
00110 
00111                         return std::make_pair(row,col);
00112                 }
00113 
00117                 template <class Derived>
00118                 std::pair<int,int>      getMatrixParam_variablesize(const ros::NodeHandle& nh,
00119                                 const std::string& name,
00120                                 const Eigen::MatrixBase<Derived>& matrix)
00121                 {
00122                         if (!nh.hasParam(name))
00123                         {
00124                                 ROS_WARN("Configuration parameter %s not found.", name.c_str());
00125                                 return std::make_pair(-1,-1);
00126                         }
00127                         else
00128                         {
00129                                 ROS_INFO("Found configuration parameter %s.", name.c_str());
00130                         }
00131 
00132                         XmlRpc::XmlRpcValue data;
00133                         nh.getParam(name, data);
00134                         ROS_ASSERT(data.getType() == XmlRpc::XmlRpcValue::TypeArray);
00135 
00136                         std::vector<typename Derived::Scalar> temp;
00137 
00138                         size_t col(0),row(0);
00139                         bool expandDiagonal(false);
00140                         for(size_t i=0; i<data.size(); ++i)
00141                         {
00142                                 if (i==0 && (data[i].getType() == XmlRpc::XmlRpcValue::TypeString))
00143                                 {
00144                                         std::string matrix_type(static_cast<std::string>(data[i]));
00145                                         std::cout<<"Matrix type: "<<matrix_type<<std::endl;
00146                                         expandDiagonal = (matrix_type == "diagonal");
00147                                         continue;
00148                                 }
00149                                 //Check validity
00150                                 bool doubleType = data[i].getType() == XmlRpc::XmlRpcValue::TypeDouble;
00151                                 bool intType = data[i].getType() == XmlRpc::XmlRpcValue::TypeInt;
00152                                 bool vectorType = data[i].getType() == XmlRpc::XmlRpcValue::TypeArray;
00153                                 ROS_ASSERT(doubleType || intType || vectorType);
00154 
00155                                 if (doubleType || intType)
00156                                 {
00157                                         temp.push_back(xmlRpcConvert<Derived>(data[i], intType));
00158                                         col++;
00159                                 }
00160                                 else
00161                                 {
00162                                         row = i;
00163                                         col = 0;
00164                                         for(size_t j=0; j<data[i].size(); ++j)
00165                                         {
00166                                                 //ROS_DEBUG("Access element %d %d",i,j);
00167                                                 bool intType = data[i][j].getType() == XmlRpc::XmlRpcValue::TypeInt;
00168                                                 temp.push_back(xmlRpcConvert<Derived>(data[i][j], intType));
00169                                                 col++;
00170                                         }
00171                                 }
00172                         }
00173 
00174 
00175                         if (expandDiagonal)
00176                         {
00177                                 Eigen::Map< Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, 1> > vector(&temp[0],col);
00178                                 Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> max(vector.asDiagonal());
00179                           const_cast< Eigen::MatrixBase<Derived>& >(matrix) = max;
00180                         }
00181                         else
00182                         {
00183                                 const_cast< Eigen::MatrixBase<Derived>& >(matrix) = matrix.derived().Map(&temp[0],col,row+1).transpose();
00184                         }
00185 
00186                         return std::make_pair(row,col);
00187                 }
00188                 
00194                 template <class Derived>
00195                 inline std::pair<int,int>       getMatrixParam(const ros::NodeHandle& nh,
00196                                 const std::string& name,
00197                                 const Eigen::MatrixBase<Derived>& matrix)
00198                 {
00199                         if ((matrix.RowsAtCompileTime <= 0) ||
00200                                         (matrix.ColsAtCompileTime <=0))
00201                         {
00202                                 return getMatrixParam_variablesize(nh,name,matrix);
00203                         }
00204                         else
00205                         {
00206                                 return getMatrixParam_fixedsize(nh,name,matrix);
00207                         }
00208                 }
00209         }
00210 }
00211 
00212 /* MATRIXLOADER_HPP_ */
00213 #endif


snippets
Author(s): Gyula Nagy
autogenerated on Mon Oct 6 2014 01:39:41