ndt_matcher_node.cc
Go to the documentation of this file.
00001 #include <NDTMatcherF2F.hh>
00002 #include <NDTMatcher.hh>
00003 #include <NDTMap.hh>
00004 #include <OctTree.hh>
00005 #include <AdaptiveOctTree.hh>
00006 #include <PointCloudUtils.hh>
00007 
00008 #include "ros/ros.h"
00009 #include "pcl/point_cloud.h"
00010 #include "sensor_msgs/PointCloud2.h"
00011 #include "pcl/io/pcd_io.h"
00012 #include "pcl/features/feature.h"
00013 #include <cstdio>
00014 #include <Eigen/Eigen>
00015 #include <fstream>
00016 #include "message_filters/subscriber.h"
00017 #include "tf/message_filter.h"
00018 #include <tf/transform_broadcaster.h>
00019 #include <tf_conversions/tf_eigen.h>
00020 #include <boost/circular_buffer.hpp>
00021 
00022 class NDTMatcherNode
00023 {
00024 protected:
00025     // Our NodeHandle
00026     ros::NodeHandle nh_;
00027 
00028     // Components for tf::MessageFilter
00029     ros::Subscriber points2_sub_;
00030 
00031 
00032     // Components for publishing
00033     tf::TransformBroadcaster tf_;
00034     ros::Publisher output_pub_;
00035 
00036     // Use the vector as a cyclic buffer (increment with std::rotate).
00037     std::vector<pcl::PointCloud<pcl::PointXYZ>,Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZ> > > pcl_buffer_;
00038     unsigned int nb_added_clouds_;
00039     boost::mutex m;
00040     lslgeneric::NDTMatcherF2F *matcher;
00041 
00042     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> incremental_pose_;
00043     void TransformEigenToTF(const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &k, tf::Transform &t)
00044     {
00045         t.setOrigin(tf::Vector3(k.matrix()(0,3), k.matrix()(1,3), k.matrix()(2,3)));
00046         t.setBasis(btMatrix3x3(k.matrix()(0,0), k.matrix()(0,1),k.matrix()(0,2),k.matrix()(1,0), k.matrix()(1,1),k.matrix()(1,2),k.matrix()(2,0), k.matrix()(2,1),k.matrix()(2,2)));
00047     };
00048 
00049 
00050 public:
00051     // Constructor
00052     NDTMatcherNode() : nb_added_clouds_(0)
00053 
00054     {
00055         double __res[] = {0.2, 0.4, 1, 2};
00056         std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00057         matcher = new lslgeneric::NDTMatcherF2F(false, false, false, resolutions);
00058         points2_sub_ = nh_.subscribe("kinect_head/camera/rgb/points", 10, &NDTMatcherNode::points2Callback, this);
00059         pcl_buffer_.resize(2);
00060         incremental_pose_.setIdentity();
00061     }
00062 
00063     ~NDTMatcherNode()
00064     {
00065         delete matcher;
00066     }
00067 
00068 
00069     // Callback
00070     void points2Callback(const sensor_msgs::PointCloud2::ConstPtr& msg_in)
00071     {
00072         // Add to a queue
00073 
00074         ROS_INFO("Got points");
00075         m.lock ();
00076         pcl::fromROSMsg (*msg_in, pcl_buffer_[0]);
00077         m.unlock ();
00078 
00079         if (nb_added_clouds_ < pcl_buffer_.size())
00080         {
00081             nb_added_clouds_++;
00082             std::rotate(pcl_buffer_.begin(), pcl_buffer_.begin()+1, pcl_buffer_.end());
00083             return;
00084         }
00085 
00086         // The most recent cloud is in [0], the secound in [1], etc.
00087         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
00088         bool ret = matcher->match(pcl_buffer_[1], pcl_buffer_[0],T);
00089         if (!ret)
00090             ROS_INFO("Registration failed!");
00091 
00092         incremental_pose_ = T * incremental_pose_; // * T;
00093 
00094         // Publish the transformation...
00095         tf::Transform transform;
00096         TransformEigenToTF(T, transform);
00097         //tf::TransformEigenToTF(incremental_pose_, transform);
00098         tf_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "test"));
00099 
00100 //      std::rotate(pcl_buffer_.begin(), pcl_buffer_.begin()+1, pcl_buffer_.end());
00101     }
00102 public:
00103     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00104 };
00105 
00106 int main(int argc, char **argv)
00107 {
00108     ros::init(argc, argv, "ndt_matcher_node");
00109 
00110     NDTMatcherNode t;
00111     ros::spin();
00112 
00113     return 0;
00114 }


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:32:03