matcher_service.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 
00003 #include "ros/ros.h"
00004 
00005 #include "pointmatcher/PointMatcher.h"
00006 
00007 #include "pointmatcher_ros/point_cloud.h"
00008 #include "pointmatcher_ros/transform.h"
00009 #include "pointmatcher_ros/get_params_from_server.h"
00010 #include "pointmatcher_ros/ros_logger.h"
00011 
00012 #include "ethzasl_icp_mapper/MatchClouds.h"
00013 
00014 #include "tf/tf.h"
00015 
00016 using namespace std;
00017 
00018 class CloudMatcher
00019 {
00020         typedef PointMatcher<float> PM;
00021         typedef PM::DataPoints DP;
00022 
00023         ros::NodeHandle& n;
00024         
00025         PM::ICP icp;
00026         
00027         ros::ServiceServer service;
00028         
00029 public:
00030         CloudMatcher(ros::NodeHandle& n);
00031         bool match(ethzasl_icp_mapper::MatchClouds::Request& req, ethzasl_icp_mapper::MatchClouds::Response& res);
00032 };
00033 
00034 CloudMatcher::CloudMatcher(ros::NodeHandle& n):
00035         n(n),
00036         service(n.advertiseService("match_clouds", &CloudMatcher::match, this))
00037 {
00038         // load config
00039         string configFileName;
00040         if (ros::param::get("~config", configFileName))
00041         {
00042                 ifstream ifs(configFileName.c_str());
00043                 if (ifs.good())
00044                 {
00045                         icp.loadFromYaml(ifs);
00046                 }
00047                 else
00048                 {
00049                         ROS_ERROR_STREAM("Cannot load config from YAML file " << configFileName);
00050                         icp.setDefault();
00051                 }
00052         }
00053         else
00054         {
00055                 ROS_WARN_STREAM("No config file specified, using default ICP chain.");
00056                 icp.setDefault();
00057         }
00058         
00059         // replace logger
00060         if (getParam<bool>("useROSLogger", false))
00061                 PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
00062 }
00063 
00064 bool CloudMatcher::match(ethzasl_icp_mapper::MatchClouds::Request& req, ethzasl_icp_mapper::MatchClouds::Response& res)
00065 {
00066         // get and check reference
00067         const DP referenceCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.reference));
00068         const unsigned referenceGoodCount(referenceCloud.features.cols());
00069         const unsigned referencePointCount(req.reference.width * req.reference.height);
00070         const double referenceGoodRatio(double(referenceGoodCount) / double(referencePointCount));
00071         
00072         if (referenceGoodCount == 0)
00073         {
00074                 ROS_ERROR("I found no good points in the reference cloud");
00075                 return false;
00076         }
00077         if (referenceGoodRatio < 0.5)
00078         {
00079                 ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - referenceGoodRatio*100.0 << "% of the cloud (received " << referenceGoodCount << ")");
00080         }
00081         
00082         // get and check reading
00083         const DP readingCloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(req.readings));
00084         const unsigned readingGoodCount(referenceCloud.features.cols());
00085         const unsigned readingPointCount(req.readings.width * req.readings.height);
00086         const double readingGoodRatio(double(readingGoodCount) / double(readingPointCount));
00087         
00088         if (readingGoodCount == 0)
00089         {
00090                 ROS_ERROR("I found no good points in the reading cloud");
00091                 return false;
00092         }
00093         if (readingGoodRatio < 0.5)
00094         {
00095                 ROS_WARN_STREAM("Partial reference cloud! Missing " << 100 - readingGoodRatio*100.0 << "% of the cloud (received " << readingGoodCount << ")");
00096         }
00097         
00098         // check dimensions
00099         if (referenceCloud.features.rows() != readingCloud.features.rows())
00100         {
00101                 ROS_ERROR_STREAM("Dimensionality missmatch: reference cloud is " << referenceCloud.features.rows()-1 << " while reading cloud is " << readingCloud.features.rows()-1);
00102                 return false;
00103         }
00104         
00105         // call ICP
00106         try 
00107         {
00108                 const PM::TransformationParameters transform(icp(readingCloud, referenceCloud));
00109                 tf::transformTFToMsg(PointMatcher_ros::eigenMatrixToTransform<float>(transform), res.transform);
00110                 ROS_INFO_STREAM("match ratio: " << icp.errorMinimizer->getWeightedPointUsedRatio() << endl);
00111         }
00112         catch (PM::ConvergenceError error)
00113         {
00114                 ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
00115                 return false;
00116         }
00117         
00118         return true;
00119 }
00120 
00121 
00122 int main(int argc, char **argv)
00123 {
00124         ros::init(argc, argv, "cloud_matcher_service");
00125         ros::NodeHandle n;
00126         
00127         CloudMatcher matcher(n);
00128         
00129         ros::spin();
00130         
00131         return 0;
00132 }
00133 


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:28:23