dynamic_trails.cpp
Go to the documentation of this file.
00001 #include <fstream>
00002 
00003 #include <boost/version.hpp>
00004 #include <deque>
00005 
00006 
00007 #include "ros/ros.h"
00008 #include "ros/console.h"
00009 #include "pointmatcher/PointMatcher.h"
00010 #include "pointmatcher/Timer.h"
00011 
00012 #include "nabo/nabo.h"
00013 
00014 #include "pointmatcher_ros/point_cloud.h"
00015 #include "pointmatcher_ros/transform.h"
00016 #include "pointmatcher_ros/get_params_from_server.h"
00017 #include "pointmatcher_ros/ros_logger.h"
00018 
00019 #include "nav_msgs/Odometry.h"
00020 #include "tf/transform_broadcaster.h"
00021 #include "tf_conversions/tf_eigen.h"
00022 #include "tf/transform_listener.h"
00023 
00024 #include <message_filters/subscriber.h>
00025 #include <message_filters/synchronizer.h>
00026 #include <message_filters/sync_policies/exact_time.h>
00027 
00028 #include <visualization_msgs/Marker.h>
00029 
00030 #include "std_msgs/String.h"
00031 #include "std_srvs/Empty.h"
00032 
00033 using namespace std;
00034 using namespace PointMatcherSupport;
00035 using namespace visualization_msgs;
00036 
00037 typedef PointMatcher<float> PM;
00038 typedef PM::DataPoints DP;
00039 
00040 class DynamicTrails
00041 {
00042         typedef PM::TransformationParameters TP;
00043         typedef PM::Matches Matches;
00044         
00045         typedef typename Nabo::NearestNeighbourSearch<float> NNS;
00046         typedef typename NNS::SearchType NNSearchType;
00047 
00048         ros::NodeHandle& n;
00049         ros::NodeHandle& pn;
00050 
00051         //message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub;
00052         //message_filters::Subscriber<nav_msgs::Odometry> odom_sub;
00053 
00054         //typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> MySyncPolicy;
00055         //message_filters::Synchronizer<MySyncPolicy> sync;
00056 
00057         ros::Subscriber scanSub;
00058         ros::Subscriber cloudSub;
00059         ros::Subscriber mapSub;
00060         ros::Publisher lastPub;
00061         ros::Publisher dynPub;
00062         ros::Publisher lastProjectedPub;
00063         ros::Publisher dynProjectedPub;
00064         ros::Publisher markerPub;
00065 
00066         PM::DataPointsFilters inputFilters;
00067         unique_ptr<PM::Transformation> transformation;
00068         shared_ptr<DP> lastPointCloud;
00069         shared_ptr<DP> trailPointCloud;
00070         PM::DataPoints globalMap;
00071         
00072         std::shared_ptr<NNS> globalNNS;
00073         std::shared_ptr<NNS> lastNNS;
00074 
00075         boost::mutex usingGlobalMap;
00076         
00077         std::deque<shared_ptr<DP>> priors;
00078 
00079         // tf
00080         tf::TransformListener tfListener;
00081 
00082         boost::mutex publishLock;
00083         
00084         const float eps;
00085         ros::Time lastInputTime;
00086 
00087         // Parameters
00088         const string p_mapFrame;
00089         const float p_minDynamicRatio;
00090         const float p_normalSpaceFactor;
00091         const float p_maxVelocity;
00092         const float p_inputRate;
00093         const int p_windowSize;
00094         const string p_trailFilepName;
00095         int trailCount;
00096         
00097 public:
00098         DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn);
00099         ~DynamicTrails();
00100         
00101 protected:
00102         //void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn, const nav_msgs::OdometryConstPtr& odom);
00103         void gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn);
00104         void gotMap(const sensor_msgs::PointCloud2& cloudMsgIn);
00105         void processCloud(DP cloud, const TP TScannerToMap);
00106         PM::DataPoints extractDynamicPoints(const PM::DataPoints input, const float minDynamicRatio);
00107         void computeVelocity(shared_ptr<DP> reference, shared_ptr<NNS> ref_tree, shared_ptr<DP> reading, shared_ptr<NNS> read_tree, const float maxRadius, const int knn, ros::Publisher debugPub);
00108         float angleWeight(const Eigen::Vector3f &normal, const Eigen::Vector3f &velocity);
00109         
00110         void averageWithReference(shared_ptr<DP> reference, shared_ptr<DP> reading, const float dir);
00111 
00112 };
00113 
00114 DynamicTrails::DynamicTrails(ros::NodeHandle& n, ros::NodeHandle& pn):
00115         n(n),
00116         pn(pn),
00117         //cloud_sub(n, "cloud_in", 1),
00118         //odom_sub(n, "icp_odom", 1),
00119         //sync(MySyncPolicy(10), cloud_sub, odom_sub),
00120         transformation(PM::get().REG(Transformation).create("RigidTransformation")),
00121         lastPointCloud(0),
00122         trailPointCloud(0),
00123   tfListener(ros::Duration(30)),
00124         eps(0.0001),
00125         p_mapFrame(getParam<string>("map_frame", "map")),
00126         p_minDynamicRatio(getParam<double>("min_dynamic_ratio", 0.75)),
00127         p_normalSpaceFactor(getParam<double>("normal_space_factor", 1.0)),
00128         p_maxVelocity(getParam<double>("max_velocity", 12.0)),
00129         p_inputRate(getParam<double>("input_rate", 1.5)),
00130         p_windowSize(getParam<int>("window_size", 3)),
00131         p_trailFilepName(getParam<string>("trail_filename", "trailMap.vtk")),
00132         trailCount(0)
00133 {
00134         
00135         // memory for last velocities
00136         //priors.resize(p_windowSize);
00137 
00138 
00139         // load configs
00140         string configFileName;
00141 
00142         if (ros::param::get("~inputFiltersConfig", configFileName))
00143         {
00144                 ifstream ifs(configFileName.c_str());
00145                 if (ifs.good())
00146                 {
00147                         inputFilters = PM::DataPointsFilters(ifs);
00148                 }
00149                 else
00150                 {
00151                         ROS_ERROR_STREAM("Cannot load input filters config from YAML file " << configFileName);
00152                 }
00153         }
00154         else
00155         {
00156                 ROS_INFO_STREAM("No input filters config file given, not using these filters");
00157         }
00158 
00159         
00160         // topics and services initialization
00161         //sync.registerCallback(boost::bind(&DynamicTrails::gotCloud, this, _1, _2));
00162         
00163         cloudSub = n.subscribe("cloud_in", 10, &DynamicTrails::gotCloud, this);
00164         mapSub = n.subscribe("point_map", 1, &DynamicTrails::gotMap, this);
00165         lastPub = n.advertise<sensor_msgs::PointCloud2>("last_dynamic_points", 2, true);
00166         dynPub = n.advertise<sensor_msgs::PointCloud2>("current_dynamic_points", 2, true);
00167         lastProjectedPub= n.advertise<sensor_msgs::PointCloud2>("last_projected_points", 2, true);
00168         dynProjectedPub= n.advertise<sensor_msgs::PointCloud2>("dyn_projected_points", 2, true);
00169         markerPub = n.advertise<visualization_msgs::Marker>("dynamic_trail", 1);
00170                 
00171 }
00172 
00173 DynamicTrails::~DynamicTrails()
00174 {
00175         // save point cloud
00176         
00177         if (trailPointCloud)
00178         {
00179                 trailPointCloud->save(p_trailFilepName);
00180         }
00181 
00182 }
00183 
00184 // Callback
00185 void DynamicTrails::gotMap(const sensor_msgs::PointCloud2& cloudMsgIn)
00186 {
00187         cout << "Map received" << endl;
00188         usingGlobalMap.lock();
00189         // Convert pointcloud2 to DataPoint
00190         globalMap = PointMatcher_ros::rosMsgToPointMatcherCloud<float>(cloudMsgIn);
00191         
00192         // Build a kd-tree
00193         globalNNS.reset( NNS::create(globalMap.features, globalMap.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
00194         usingGlobalMap.unlock();
00195 }
00196 
00197 
00198 
00199 void DynamicTrails::gotCloud(const sensor_msgs::PointCloud2ConstPtr& cloudMsgIn)
00200 {
00201         // IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)
00202 
00203         const ros::Time currentTime = ros::Time::now();
00204         const float currentRate = 1.0/(currentTime - lastInputTime).toSec();
00205         
00206         if(currentRate < p_inputRate )
00207         {
00208                 lastInputTime = currentTime;
00209 
00210                 // Convert pointcloud2 to DataPoint
00211                 DP cloud(PointMatcher_ros::rosMsgToPointMatcherCloud<float>(*cloudMsgIn));
00212                 
00213                 // Convert odometry msg to TransformationParameters
00214                 //TP TScannerToMap =    PointMatcher_ros::odomMsgToEigenMatrix<float>(*odom);
00215                 TP TScannerToMap;
00216 
00217                 try
00218                 {
00219                         tfListener.waitForTransform(p_mapFrame, cloudMsgIn->header.frame_id, cloudMsgIn->header.stamp, ros::Duration(0.5));
00220                         TScannerToMap = PointMatcher_ros::transformListenerToEigenMatrix<float>(
00221                                         tfListener,
00222                                         p_mapFrame,
00223                                         cloudMsgIn->header.frame_id,
00224                                         cloudMsgIn->header.stamp
00225                                 );
00226                         processCloud(cloud, TScannerToMap);
00227                 }
00228                 catch(tf::ExtrapolationException e)
00229                 {
00230                         ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< cloudMsgIn->header.stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - cloudMsgIn->header.stamp);
00231                         return;
00232                 }
00233         
00234         }
00235 
00236 }
00237 
00238 // Point cloud processing
00239 void DynamicTrails::processCloud(DP inputPointCloud, const TP TScannerToMap)
00240 {
00241 
00242         ROS_INFO("Processing new point cloud");
00243         
00244         timer t; // Print how long take the algo
00245                 
00246 
00247                 
00248         ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
00249         
00250         // Correct new points using ICP result and move them in their own frame
00251         inputPointCloud = transformation->compute(inputPointCloud, TScannerToMap); 
00252 
00253         cerr << "inputPointCloud.features.cols() " << inputPointCloud.features.cols() << endl;
00254         // filter to keep only dynamic points
00255         shared_ptr<DP> dynamicPointCloud( new DP(extractDynamicPoints(inputPointCloud, p_minDynamicRatio)));
00256 
00257         
00258         
00259         // Apply filters to incoming cloud, in global coordinates!
00260         inputFilters.apply(*dynamicPointCloud);
00261         
00262         const int dynamicPtsCount = dynamicPointCloud->features.cols();
00263 
00264         cerr << "dynamicPointCloud->features.cols() " << dynamicPtsCount << endl;
00265         
00266         // Error verification
00267         if(dynamicPtsCount == 0)
00268         {
00269                 ROS_WARN_STREAM("No dynamic points could be found");
00270                 return;
00271         }
00272                 
00273         //PM::Matrix unitVec(3,dynamicPtsCount);
00274         //unitVec.row(0) = PM::Matrix::Ones(1,dynamicPtsCount);
00275         //unitVec.bottomRows(2) = PM::Matrix::Zero(2,dynamicPtsCount);
00276 
00277 
00278         dynamicPointCloud->addDescriptor("velocity_dir", PM::Matrix::Zero(3,dynamicPtsCount));
00279         dynamicPointCloud->addDescriptor("velocity_mag", PM::Matrix::Zero(1,dynamicPtsCount));
00280         dynamicPointCloud->addDescriptor("velocity_dt", PM::Matrix::Zero(1,dynamicPtsCount));
00281 
00282 
00283         // fill the lastPointCloud for the firs time
00284         if(!lastPointCloud)
00285         {
00286                 lastPointCloud = dynamicPointCloud;
00287                 cerr << lastPointCloud->features.cols() << endl;
00288 
00289 
00290                 DP::View viewOn_normals_last = lastPointCloud->getDescriptorViewByName("normals");
00291 
00292                 // Build the kd-tree of the last points
00293                 PM::Matrix lastEuclNormal(6, dynamicPtsCount);
00294                 lastEuclNormal.topRows(3) = lastPointCloud->features.topRows(3);
00295                 lastEuclNormal.bottomRows(3) = viewOn_normals_last*p_normalSpaceFactor;
00296 
00297                 //lastNNS.reset( NNS::create(lastEuclNormal, 6, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
00298                 lastNNS.reset( NNS::create(lastPointCloud->features, lastPointCloud->features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
00299 
00300                 ROS_INFO_STREAM("Initializing last point cloud");
00301 
00302                 return;
00303         }
00304 
00305         const int lastPtsCount = lastPointCloud->features.cols();
00306         ROS_INFO_STREAM("lastPtsCount: " << lastPtsCount);
00307 
00308         // Error check on dimensions
00309         if (dynamicPointCloud->features.rows() != lastPointCloud->features.rows())
00310         {
00311                 ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << dynamicPointCloud->features.rows()-1 << " while lastPointCloud is " << lastPointCloud->features.rows()-1);
00312                 return;
00313         }
00314 
00315         
00316 
00317         // Copy the last speed direction for temporal smoothing
00318         PM::Matrix priorOn_velocityDir = lastPointCloud->getDescriptorViewByName("velocity_dir");
00319         PM::Matrix priorOn_velocityDt = lastPointCloud->getDescriptorViewByName("velocity_dt");
00320         
00321         DP::View v_normals_input = dynamicPointCloud->getDescriptorViewByName("normals");
00322         PM::Matrix inputEuclNormal(6, dynamicPtsCount);
00323         inputEuclNormal.topRows(3) = dynamicPointCloud->features.topRows(3);
00324         inputEuclNormal.bottomRows(3) = v_normals_input*p_normalSpaceFactor;
00325         
00326 
00327         // Build kd-tree of the input points
00328         std::shared_ptr<NNS> inputNNS;
00329         //inputNNS.reset( NNS::create(inputEuclNormal, 6, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
00330         inputNNS.reset( NNS::create(dynamicPointCloud->features, 3, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS));
00331 
00332 
00333         // FIXME: those are parameters
00334         const float maxClusterDistance = 0.75;
00335         const int nbIter = 5;
00336         for(int i=0; i<nbIter; i++)
00337         {
00338                 computeVelocity(lastPointCloud, lastNNS, dynamicPointCloud, inputNNS, maxClusterDistance, nbIter-i, dynProjectedPub);
00339                 computeVelocity(dynamicPointCloud, inputNNS, lastPointCloud, lastNNS, maxClusterDistance, nbIter-i, lastProjectedPub);
00340 
00341                 averageWithReference(lastPointCloud, dynamicPointCloud, 1);
00342                 averageWithReference(dynamicPointCloud, lastPointCloud, 1);
00343         }
00344         
00345 
00346         DP::View v_matchId_dyn = dynamicPointCloud->getDescriptorViewByName("match_id");
00347         //DP::View v_velocityMag_dyn = dynamicPointCloud->getDescriptorViewByName("velocity_mag");
00348         DP::View v_velocityDir_dyn = dynamicPointCloud->getDescriptorViewByName("velocity_dir");
00349         DP::View v_velocityDt_dyn = dynamicPointCloud->getDescriptorViewByName("velocity_dt");
00350         DP::View v_normal_dyn = dynamicPointCloud->getDescriptorViewByName("normals");
00351 
00352         //DP::View v_velocityMag_last = lastPointCloud->getDescriptorViewByName("velocity_mag");
00353         DP::View v_velocityDir_last = lastPointCloud->getDescriptorViewByName("velocity_dir");
00354         DP::View v_velocityDt_last = lastPointCloud->getDescriptorViewByName("velocity_dt");
00355         
00356         // Project into the future
00357         v_velocityDir_dyn *= -1;
00358         
00359         // Markers
00360         visualization_msgs::Marker marker;
00361 
00362         marker.header.frame_id = p_mapFrame;
00363         marker.header.stamp = ros::Time::now();
00364         marker.ns = "basic_shapes";
00365         marker.id = 0;
00366         //marker.id = trailCount;
00367         trailCount++;
00368         marker.type = visualization_msgs::Marker::LINE_LIST;
00369         
00370         marker.scale.x = 0.01;
00371         marker.scale.y = 0.1;
00372         marker.scale.z = 0.1;
00373 
00374         marker.color.r = 0.0f;
00375         marker.color.g = 1.0f;
00376         marker.color.b = 0.0f;
00377         //marker.color.a = 0.20;
00378         marker.color.a = 1.0;
00379 
00380         marker.lifetime = ros::Duration();
00381 
00382         DP currenttTrails = dynamicPointCloud->createSimilarEmpty();
00383         int currentTrailPtCount = 0;
00384 
00385         // Filter outliers and build markers
00386         for(int i=0; i<dynamicPtsCount; i++)
00387         {
00388                 const int id = v_matchId_dyn(0,i);
00389 
00390                 const Eigen::Vector3f pt = dynamicPointCloud->features.col(i).head(3);
00391                 const Eigen::Vector3f pt_last = lastPointCloud->features.col(id).head(3);
00392                 
00393                 const Eigen::Vector3f normal = v_normal_dyn.col(i);
00394                 const float w_ang = angleWeight(normal, v_velocityDir_dyn.col(i));
00395                 
00396                 const Eigen::Vector3f lastVelDir = priorOn_velocityDir.col(id);
00397                 const Eigen::Vector3f currentVelDir = v_velocityDir_dyn.col(i);
00398                 const float w_diffAngle =  1 - acos(lastVelDir.normalized().dot(currentVelDir.normalized()))/M_PI;
00399                 
00400                 
00401                 int lastID = id;
00402 
00403                 // FIXME: finish here
00404                 //for(int i = 0; i < priors.size(); i++)
00405                 //{
00406                 //      const shared_ptr<DP> lastPrior = priors[i];
00407                 //      cerr << lastPrior << endl;
00408                 //      cerr << "lastID " << lastID << endl;
00409                 //      cerr << "last size " << lastPrior->features.cols() << endl;
00410                 //      DP::View match_id = priors[i]->getDescriptorViewByName("match_id");
00411 
00412                 //      lastID = match_id(0, lastID);
00413                 //      
00414                 //}
00415 
00416                 //cerr << "----------" << endl;
00417                 
00418                 const float lastDt = priorOn_velocityDt(0,id);
00419                 const float delta = (pt_last - pt).norm();
00420                 const float delta_projected = (pt_last + lastVelDir*lastDt - pt).norm();
00421                 const float w_proj = delta_projected/delta;
00422 
00423 
00424                 const float mag = currentVelDir.norm();
00425                 //if(w_ang > 0.75  && mag > eps && mag < p_maxVelocity)
00426                 if(mag > eps && mag < p_maxVelocity)
00427                 {
00428                         if(w_diffAngle > 0.5 && w_proj < 0.5)
00429                         {
00430                                 // Temporal smoothing
00431 
00432                                 v_velocityDir_dyn.col(i) = (lastVelDir + currentVelDir)/2;
00433 
00434                                 geometry_msgs::Point p;
00435                                 p.x = dynamicPointCloud->features(0,i);
00436                                 p.y = dynamicPointCloud->features(1,i);
00437                                 p.z = dynamicPointCloud->features(2,i);
00438                                 marker.points.push_back(p);
00439                                 
00440                                 p.x += v_velocityDir_dyn(0,i);
00441                                 p.y += v_velocityDir_dyn(1,i);
00442                                 p.z += v_velocityDir_dyn(2,i);
00443 
00444                                 marker.points.push_back(p);
00445 
00446                                 currenttTrails.setColFrom(currentTrailPtCount, *dynamicPointCloud, i);
00447                                 currentTrailPtCount++;
00448                         }
00449                 }
00450                 else
00451                 {
00452                         v_velocityDir_dyn.col(i) = Eigen::Vector3f::Zero();
00453                         v_velocityDt_dyn(0,i) = 0;
00454                 }
00455                 
00456         }
00457 
00458         currenttTrails.conservativeResize(currentTrailPtCount);
00459 
00460         if(currentTrailPtCount > 0)
00461         {
00462                 if(trailPointCloud == false)
00463                 {
00464                         trailPointCloud.reset(new DP(currenttTrails));
00465                 }
00466                 else
00467                 {
00468                         trailPointCloud->concatenate(currenttTrails);
00469                 }
00470         }
00471 
00472         markerPub.publish(marker);
00473 
00474         dynPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*dynamicPointCloud.get(), p_mapFrame, ros::Time::now()));
00475         lastPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*lastPointCloud.get(), p_mapFrame, ros::Time::now()));
00476 
00477 
00478         lastPointCloud = dynamicPointCloud;
00479         lastNNS = inputNNS;
00480 
00481 
00482         if(priors.size() >= p_windowSize)
00483         {
00484                 priors.pop_back();
00485         }
00486 
00487         priors.push_front(dynamicPointCloud);
00488 
00489 
00490         ROS_INFO_STREAM("Total trail computation: " << t.elapsed() << " [s]");
00491 
00492         //ros::Rate r(2);
00493         //r.sleep();
00494 }
00495 
00496 
00497 PM::DataPoints DynamicTrails::extractDynamicPoints (const PM::DataPoints input, const float minDynamicRatio)
00498 {
00499         const int inputPtsCount(input.features.cols());
00500 
00501         DP output(input.createSimilarEmpty());
00502 
00503         const int knn = 1;
00504         Matches::Dists dists_input(knn, inputPtsCount);
00505         Matches::Ids ids_input(knn, inputPtsCount);
00506         
00507         usingGlobalMap.lock();
00508         // FIXME: this is a parameter
00509         const float maxGlobalDist = 0.5;
00510         globalNNS->knn(input.features, ids_input, dists_input, knn, 0, 0, maxGlobalDist);
00511 
00512         if(globalMap.descriptorExists("probabilityStatic") == false ||
00513                         globalMap.descriptorExists("probabilityDynamic") == false ||
00514                         globalMap.descriptorExists("dynamic_ratio") == false
00515                 )
00516         {
00517                 ROS_WARN_STREAM("Map descriptors not found");
00518                 usingGlobalMap.unlock();
00519                 return output;
00520         }
00521 
00522         //DP::View viewOnDynamicRatio = globalMap.getDescriptorViewByName("dynamic_ratio");
00523         DP::View viewOnProbabilityStatic = globalMap.getDescriptorViewByName("probabilityStatic");
00524         DP::View viewOnProbabilityDynamic = globalMap.getDescriptorViewByName("probabilityDynamic");
00525 
00526         int dynamicPtsCount=0;
00527         for(int i=0; i<inputPtsCount; i++)
00528         {
00529                 float sumObs = 0;
00530                 float dist = numeric_limits<float>::infinity();
00531                 float dynRatio = 0;
00532 
00533                 for(int k=0; k<knn; k++)
00534                 {
00535                         const int mapId = ids_input(k,i);
00536                         sumObs = max(sumObs, viewOnProbabilityDynamic(0,mapId) + viewOnProbabilityStatic(0,mapId));
00537                         dist = min(dist, dists_input(k,i));
00538                         dynRatio = max(dynRatio, viewOnProbabilityDynamic(0, mapId));
00539                 }
00540 
00541                 {
00542                         //if(sumObs == 0 || dynRatio > minDynamicRatio || dist == numeric_limits<float>::infinity())
00543                         if(dynRatio > minDynamicRatio || dist == numeric_limits<float>::infinity())
00544                         {
00545                                 output.setColFrom(dynamicPtsCount, input, i);
00546                                 dynamicPtsCount++;
00547                         }
00548                 }
00549         }
00550 
00551         usingGlobalMap.unlock();
00552 
00553         output.conservativeResize(dynamicPtsCount);
00554         return output;
00555 
00556 }
00557 
00558 void DynamicTrails::computeVelocity(shared_ptr<DP> reference, shared_ptr<NNS> ref_tree, shared_ptr<DP> reading, shared_ptr<NNS> read_tree, const float maxRadius, const int knn, ros::Publisher debugPub)
00559 {
00560 
00561         const int readPtsCount = reading->features.cols();
00562         const int refPtsCount = reference->features.cols();
00563 
00564         // Prepare useful views on descriptors
00565         DP::View v_velocityDir_read = reading->getDescriptorViewByName("velocity_dir");
00566         DP::View v_velocityDt_read = reading->getDescriptorViewByName("velocity_dt");
00567         DP::View v_normal_read = reading->getDescriptorViewByName("normals");
00568 
00569         DP::View v_Msec_read = reading->getDescriptorViewByName("stamps_Msec");
00570         DP::View v_sec_read = reading->getDescriptorViewByName("stamps_sec");
00571         DP::View v_nsec_read = reading->getDescriptorViewByName("stamps_nsec");
00572         
00573         // Ensure that we don't modify the reference
00574         PM::Matrix velocityDir_ref = PM::Matrix::Zero(3,refPtsCount);
00575         PM::Matrix velocityDt_ref = PM::Matrix::Zero(1,refPtsCount);
00576 
00577         DP::View v_Msec_ref = reference->getDescriptorViewByName("stamps_Msec");
00578         DP::View v_sec_ref = reference->getDescriptorViewByName("stamps_sec");
00579         DP::View v_nsec_ref = reference->getDescriptorViewByName("stamps_nsec");
00580         
00581 
00582         // Move points based on velocity
00583         PM::Matrix moved_read(3, readPtsCount);
00584         //PM::Matrix moved_read(6, readPtsCount);
00585 
00586         const PM::Matrix static_read = reading->features.topRows(3);
00587         //PM::Matrix static_read(6, readPtsCount);
00588         //static_read.topRows(3) = reading->features.topRows(3);
00589         //static_read.bottomRows(3) = v_normal_read;
00590         
00591         //if (knn == 1)
00592         //      abort();
00593 
00594         for(int i=0; i<readPtsCount; i++)
00595         {
00596                 moved_read.col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i)*v_velocityDt_read(0,i);
00597                 //moved_read.col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i);
00598                 
00599                 //moved_read.topRows(3).col(i)= reading->features.col(i).head(3) + v_velocityDir_read.col(i)*v_velocityMag_read(0,i);
00600                 //moved_read.bottomRows(3).col(i) = v_normal_read.col(i);
00601                 
00602                 
00603                 assert(!isnan(moved_read(0,i)));
00604                 assert(!isnan(moved_read(1,i)));
00605                 assert(!isnan(moved_read(2,i)));
00606                 assert(!isinf(moved_read(0,i)));
00607                 assert(!isinf(moved_read(1,i)));
00608                 assert(!isinf(moved_read(2,i)));
00609         }
00610 
00611         //------------------
00612         // Search for nearest points
00613         Matches::Dists dists(knn, readPtsCount);
00614         Matches::Ids ids(knn, readPtsCount);
00615 
00616         ref_tree->knn(moved_read, ids, dists, knn, 0, 2.0);
00617 
00618         
00619         PM::Matrix sumVec_ref = PM::Matrix::Zero(3,refPtsCount);
00620         //PM::Matrix sumVec_ref(3,refPtsCount);
00621         //sumVec_ref.row(0) = PM::Matrix::Ones(1,refPtsCount);
00622         //sumVec_ref.bottomRows(2) = PM::Matrix::Zero(2,refPtsCount);
00623 
00624         PM::Matrix closeId_read = PM::Matrix::Constant(1,readPtsCount, -1);
00625         PM::Matrix weight_ref = PM::Matrix::Zero(1,refPtsCount);
00626         PM::Matrix sumDt_ref = PM::Matrix::Zero(1,refPtsCount);
00627 
00628         for(int i=0; i<readPtsCount; i++)
00629         {
00630                 const Eigen::Vector3f pt = reading->features.col(i).head(3);
00631                 const double timeRead = v_Msec_read(0,i)*1e6 + v_sec_read(0,i) + v_nsec_read(0,i)*1e-9;
00632 
00633                 // Counters for reading
00634                 float sumWeight = 0;
00635                 float sumDt = 0;
00636                 Eigen::Vector3f sumVec = Eigen::Vector3f::Zero(3,1);
00637 
00638                 for(int k=0; k<knn; k++)
00639                 {
00640                         if(dists(k,i) != numeric_limits<float>::infinity())
00641                         {
00642                                 const int id = ids(k,i);
00643                                 const Eigen::Vector3f pt_nn = reference->features.col(id).head(3);
00644                                 const double timeRef = v_Msec_ref(0,id)*1e6 + v_sec_ref(0,id) + v_nsec_ref(0,id)*1e-9;
00645                                 
00646                                 const Eigen::Vector3f delta = pt_nn - pt;
00647                                 const Eigen::Vector3f normal_read = v_normal_read.col(i);
00648                                 const double dt = abs(timeRead - timeRef);
00649 
00650                                 const float distRatio = sqrt(dists(k,i))/delta.norm();
00651                                 const float w_dist = 1 + eps - std::min(1.0f, distRatio);
00652                                 const float w_ang = angleWeight(normal_read, delta);
00653                                 //const float w = w_ang*w_dist;
00654                                 const float w = w_dist;
00655                                 //const float w = 1;
00656 
00657                                 assert(!isnan(w_dist));
00658                                 assert(!isnan(w_ang));
00659                                 assert(dt != 0);
00660 
00661                         
00662                                 weight_ref(id)     += w;
00663                                 sumDt_ref(id)      += w*dt;
00664                                 sumVec_ref.col(id) += w*delta/dt;
00665                                 //sumVec_ref.col(id) += w*delta;
00666 
00667                                 sumWeight += w;
00668                                 sumDt     += w*dt;
00669                                 sumVec    += w*delta/dt;
00670                                 //sumVec    += w*delta;
00671                                 
00672                                 closeId_read(i) = id;
00673                         }
00674                 }
00675 
00676                 // Weighted mean
00677                 if(sumWeight > eps)
00678                 {
00679                         sumDt /= sumWeight;
00680                         sumVec /= sumWeight;
00681 
00682                         const float mag = sumVec.norm();
00683                         if( mag < p_maxVelocity)
00684                         {
00685                                 // Store in the descriptors
00686                                 v_velocityDir_read.col(i) = sumVec;
00687                                 v_velocityDt_read(0,i) = sumDt;
00688                         }
00689                 }
00690 
00691 
00692 
00693         }
00694 
00695         //------------------
00696         // Average points that received multiple matches on the reference
00697         for(int i=0; i<refPtsCount; i++)
00698         {
00699                 // Avoid division per zero
00700                 const float mag = sumVec_ref.col(i).norm();
00701                 if(mag > eps)
00702                 {
00703                         // Weighted mean
00704                         sumDt_ref(i) /= weight_ref(i);
00705                         sumVec_ref.col(i) /= weight_ref(i);
00706                         
00707                         if(mag < p_maxVelocity)
00708                         {
00709                                 // Store in the descriptors
00710                                 velocityDir_ref.col(i) = sumVec_ref.col(i);
00711                                 velocityDt_ref(0,i) = sumDt_ref(i);
00712                         }       
00713                 }
00714         }
00715 
00716 
00717         //------------------
00718         // Average all points with dual match
00719         for(int i=0; i<readPtsCount; i++)
00720         {
00721                 assert(!isnan(v_velocityDir_read(0,i)));
00722                 const int id = closeId_read(i);
00723 
00724                 if(id != -1)
00725                 {
00726                         const float mag = velocityDir_ref.col(id).norm();
00727                         if(mag > eps)
00728                         {
00729                                 // We only keep the reference velocity, if only one match it will be the same
00730                                 v_velocityDir_read.col(i) = velocityDir_ref.col(id);
00731                                 v_velocityDt_read(0,i) = velocityDt_ref(0,id);
00732                         }
00733                 }
00734                 
00735         }
00736 
00737         //------------------
00738         // Apply local constrains on velocity
00739         const int maxNN = 100;
00740         Matches::Dists dists_local(maxNN, readPtsCount);
00741         Matches::Ids ids_local(maxNN, readPtsCount);
00742 
00743         // Search
00744         read_tree->knn(static_read, ids_local, dists_local, maxNN, 0, 0, maxRadius);
00745         //PM::Matrix dists_nosqrt = dists_local.cwiseSqrt();
00746         //dists_local = dists_local.cwiseSqrt();
00747         
00748         // Initialize counters
00749         
00750         PM::Matrix sumVec_read = PM::Matrix::Zero(3,readPtsCount);
00751 
00752         PM::Matrix weight_read = PM::Matrix::Zero(1,readPtsCount);
00753         PM::Matrix sumMag_read = PM::Matrix::Zero(1,readPtsCount);
00754         PM::Matrix sumDt_read = PM::Matrix::Zero(1,readPtsCount);
00755         
00756         for(int i=0; i<readPtsCount; i++)
00757         {
00758                 const Eigen::Vector3f normal_read = v_normal_read.col(i);
00759                 
00760                 for(int k=0; k<maxNN; k++)
00761                 {
00762                         if(dists_local(k,i) != numeric_limits<float>::infinity())
00763                         {
00764                                 const int id = ids_local(k,i);
00765                                 const Eigen::Vector3f velocityDir = v_velocityDir_read.col(id);
00766 
00767                                 const float w_dist = eps + (1 - eps)*(1 - sqrt(dists_local(k,i))/maxRadius);
00768                                 assert(!isnan(w_dist));
00769 
00770                                 //const float w_ang = angleWeight(normal_read, velocityDir);
00771                                 //assert(!isnan(w_ang));
00772 
00773                                 const float w = w_dist;
00774                                 //const float w = 1;
00775 
00776                                 weight_read(i)     += w;
00777                                 sumVec_read.col(i) += w*velocityDir;
00778                                 sumDt_read(i)      += w*v_velocityDt_read(0,id);
00779                         }
00780                 }
00781 
00782         }
00783         
00784         //------------------
00785         // Average points given their neighborgs
00786         for(int i=0; i<readPtsCount; i++)
00787         {
00788                 assert(!isnan(v_velocityDir_read(0,i)));
00789                 // Avoid division per zero
00790                 if(weight_read(i) > eps)
00791                 {
00792                         // Weighted mean
00793                         sumDt_read(i) /= weight_read(i);
00794                         sumVec_read.col(i) /= weight_read(i);
00795 
00796                         const float mag = sumVec_read.col(i).norm();
00797                         if(mag > eps && mag < p_maxVelocity)
00798                         {
00799                                 // Store in the descriptors
00800                                 v_velocityDir_read.col(i) = sumVec_read.col(i);
00801                                 v_velocityDt_read(0,i) = sumDt_read(i);
00802                         }
00803                 }
00804                 
00805                 assert(isnan(v_velocityDir_read(0,i)) == false);
00806 
00807         }
00808         reading->addDescriptor("match_id", closeId_read);
00809 
00810 
00811         // Extra for debug
00812         DP projectedPointCloud = *reading;
00813         projectedPointCloud.features.topRows(3) = moved_read.topRows(3);
00814         debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(projectedPointCloud, p_mapFrame, ros::Time::now()));
00815 
00816         //usleep(100000);
00817 }
00818 
00819 float DynamicTrails::angleWeight(const Eigen::Vector3f &normal, const Eigen::Vector3f &velocity)
00820 {
00821         const float projDist = min(1.0f, fabs(normal.dot(velocity.normalized())));
00822         assert(projDist <= 1);
00823 
00824         return (eps + (1 - eps)*(1 - 2.0*acos(projDist)/M_PI));
00825 
00826         //if(projDist > 0)
00827         //{
00828         //      return (1 + eps - 2.0*fabs(acos(min(1.0f, projDist)))/M_PI);
00829         //}
00830         //else
00831         //{
00832         //      const float projDistFlip = normal.dot(-velocity.normalized());
00833         //      return (1 + eps - 2.0*fabs(acos(min(1.0f, projDistFlip)))/M_PI);
00834         //}
00835 }
00836 
00837 void DynamicTrails::averageWithReference(shared_ptr<DP> reference, shared_ptr<DP> reading, const float dir)
00838 {
00839         //const int readPtsCount = reading->features.cols();
00840         const int refPtsCount = reference->features.cols();
00841         
00842         //DP::View v_matchId_read = reading->getDescriptorViewByName("match_id");
00843         DP::View v_velocityDir_read = reading->getDescriptorViewByName("velocity_dir");
00844         DP::View v_velocityDt_read = reading->getDescriptorViewByName("velocity_dt");
00845         DP::View v_normal_read = reading->getDescriptorViewByName("normals");
00846         
00847         DP::View v_matchId_ref = reference->getDescriptorViewByName("match_id");
00848         DP::View v_velocityDir_ref = reference->getDescriptorViewByName("velocity_dir");
00849         DP::View v_velocityDt_ref = reference->getDescriptorViewByName("velocity_dt");
00850         
00851         for(int i=0; i<refPtsCount; i++)
00852         {
00853                 const int id = v_matchId_ref(0,i);
00854                 const float w_read = angleWeight(v_normal_read.col(id), v_velocityDir_read.col(id));
00855                 const float w_ref = angleWeight(v_normal_read.col(id), v_velocityDir_ref.col(i));
00856                 const float w_sum = w_read + w_ref;
00857                 
00858                 const float mag_ref = v_velocityDir_ref.col(i).norm();
00859                 if(w_sum > eps && mag_ref > eps)
00860                 {
00861                         v_velocityDir_read.col(id) = (w_read*dir*v_velocityDir_read.col(id) - w_ref*dir*v_velocityDir_ref.col(i))/(w_sum);
00862                         
00863                         v_velocityDt_read(0,id) = (w_read*v_velocityDt_read(0,id) + w_ref*v_velocityDt_ref(0,i))/(w_sum);
00864                 }
00865         }
00866 
00867         //for(int i=0; i<readPtsCount; i++)
00868         //{
00869         //      assert(!isnan(v_velocityDir_read(0,i)));
00870         //      const int id = v_matchId_read(0,i);
00871         //      const float w_read = angleWeight(v_normal_read.col(i), v_velocityDir_read.col(i));
00872         //      const float w_ref = angleWeight(v_normal_read.col(i), v_velocityDir_ref.col(id));
00873         //      const float w_sum = w_read + w_ref;
00874         //      
00875         //      const float mag_ref = v_velocityDir_ref.col(id).norm();
00876         //      if(w_sum > eps && mag_ref > eps)
00877         //      {
00878         //              assert((dir*v_velocityDir_read.col(i) - dir*v_velocityDir_ref.col(id)).norm() != 0);
00879 
00880         //              v_velocityDir_read.col(i) = (w_read*dir*v_velocityDir_read.col(i) - w_ref*dir*v_velocityDir_ref.col(id))/(w_sum);
00881         //              
00882         //              v_velocityDt_read(0,i) = (w_read*v_velocityDt_read(0,i) + w_ref*v_velocityDt_ref(0,id))/(w_sum);
00883         //      }
00884         //      assert(isnan(v_velocityDir_read(0,i)) == false);
00885         //}
00886 }
00887 
00888 
00889 // Main function supporting the DynamicTrails class
00890 int main(int argc, char **argv)
00891 {
00892         ros::init(argc, argv, "map_maintainer");
00893         ros::NodeHandle n;
00894         ros::NodeHandle pn("~");
00895         DynamicTrails trails(n, pn);
00896 
00897         ros::Rate r(50);
00898         while(ros::ok())
00899         {
00900                 ros::spinOnce();
00901                 r.sleep();
00902         }
00903         
00904         return 0;
00905 }


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Tue Mar 3 2015 15:28:44