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
00026 ros::NodeHandle nh_;
00027
00028
00029 ros::Subscriber points2_sub_;
00030
00031
00032
00033 tf::TransformBroadcaster tf_;
00034 ros::Publisher output_pub_;
00035
00036
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
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
00070 void points2Callback(const sensor_msgs::PointCloud2::ConstPtr& msg_in)
00071 {
00072
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
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_;
00093
00094
00095 tf::Transform transform;
00096 TransformEigenToTF(T, transform);
00097
00098 tf_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "test"));
00099
00100
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 }