ndt_feature_reg_node.cc
Go to the documentation of this file.
00001 #include <cstdio>
00002 #include <fstream>
00003 #include <ros/ros.h>
00004 #include <pcl/point_cloud.h>
00005 #include <sensor_msgs/PointCloud2.h>
00006 #include <pcl/io/pcd_io.h>
00007 #include <pcl/features/feature.h>
00008 #include <Eigen/Eigen>
00009 #include <fstream>
00010 #include <image_transport/image_transport.h>
00011 #include <image_transport/subscriber_filter.h>
00012 #include <message_filters/subscriber.h>
00013 #include <message_filters/time_synchronizer.h>
00014 #include <tf/message_filter.h>
00015 #include <tf/transform_broadcaster.h>
00016 #include <tf_conversions/tf_eigen.h>
00017 
00018 #include <depth_camera.h>
00019 
00020 #include <ndt_feature_reg/ndt_frame.h>
00021 #include <ndt_feature_reg/ndt_frame_proc.h>
00022 #include <ndt_feature_reg/ndt_frame_viewer.h>
00023 
00024 #include <opencv/cv.h>
00025 #include <opencv/highgui.h>
00026 #include <boost/shared_ptr.hpp>
00027 #include <cv_bridge/CvBridge.h>
00028 #include <sensor_msgs/Image.h>
00029 #include <sensor_msgs/CameraInfo.h>
00030 #include <message_filters/time_synchronizer.h>
00031 #include <message_filters/sync_policies/approximate_time.h>
00032 
00033 using namespace lslgeneric;
00034 using namespace ndt_feature_reg;
00035 
00036 cv::Mat generateLookupTable(const pcl::PointCloud<pcl::PointXYZ> &pointcloud)
00037 {
00038     cv::Mat lookup_table(cv::Size(pointcloud.width, pointcloud.height), CV_64FC3);
00039     if (pointcloud.is_dense)
00040     {
00041 
00042     }
00043     else
00044     {
00045         assert(false);
00046     }
00047     return lookup_table;
00048 }
00049 
00050 void subsamplePointCloud(const pcl::PointCloud<pcl::PointXYZ> &orig, pcl::PointCloud<pcl::PointXYZ> &sub, int step)
00051 {
00052     uint32_t x = 0;
00053     while (x < orig.width)
00054     {
00055         uint32_t y = 0;
00056         while (y*step < orig.height)
00057         {
00058             sub.push_back(orig.at(x*step, y*step));
00059             y += step;
00060         }
00061         x += step;
00062     }
00063 }
00064 
00065 
00066 
00067 class NDTFeatureRegNode
00068 {
00069 protected:
00070     ros::NodeHandle nh_;
00071 
00072     message_filters::Subscriber<sensor_msgs::Image> rgb_sub_;
00073     message_filters::Subscriber<sensor_msgs::Image> depth_sub_;
00074     ros::Subscriber depth_param_sub_;
00075     message_filters::Subscriber<sensor_msgs::Image> intensity_sub_;
00076     message_filters::Subscriber<sensor_msgs::PointCloud2> points2_sub_;
00077 
00078     message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> > sync_;
00079     message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> > sync2_;
00080 
00081     sensor_msgs::CvBridge _imBridge;
00082     bool visualize_;
00083 
00084     tf::TransformBroadcaster tf_;
00085     tf::TransformListener tf_listener_;
00086 
00087     ros::Publisher pub_points2_;
00088 
00089     boost::mutex m_;
00090     lslgeneric::DepthCamera<pcl::PointXYZ> cameraparams_;
00091     bool camerasetup_;
00092     bool skip_matching_;
00093     bool estimate_di_;
00094     bool match_full_;
00095     bool match_no_association_;
00096     int support_size_;
00097     double max_var_;
00098     double current_res_;
00099     int max_nb_frames_;
00100 
00101     int camera_nb_;
00102     bool set_initial_pose_;
00103     bool publish_cloud_;
00104     int subsample_step_;
00105     std::string world_str;
00106 
00107     ros::Time prev_timestamp_;
00108     NDTFrameProc<pcl::PointXYZ>* proc;
00109     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> global_transform_;
00110     void TransformEigenToTF(const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &k, tf::Transform &t)
00111     {
00112         t.setOrigin(tf::Vector3(k.matrix()(0,3), k.matrix()(1,3), k.matrix()(2,3)));
00113         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)));
00114     };
00115 
00116     void setupCamera(const sensor_msgs::CameraInfoConstPtr & camera_info)
00117     {
00118 
00119         m_.lock();
00120         if(!camerasetup_) {
00121             //setup params from camera info msg
00122             std::cout<<"seting up camera params\n";
00123             double fx, fy, cx, cy, ds=1;
00124             std::vector<double> dist(5);
00125             fx = camera_info->K[0]; 
00126             fy = camera_info->K[4];
00127             cx = camera_info->K[2]; 
00128             cy = camera_info->K[5];
00129             if(camera_info->D.size() == 5) {
00130                dist = camera_info->D;
00131             } else {
00132                dist = std::vector<double>(5,0);
00133             }
00134             std::cout<<"Params: "<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<" "<<dist.size()<<std::endl;
00135             cameraparams_ = lslgeneric::DepthCamera<pcl::PointXYZ>(fx,fy,cx,cy,dist,ds,true);
00136             
00137             camerasetup_ = true;
00138         }
00139         m_.unlock();
00140 
00141     }
00142 
00143 public:
00144     // Constructor
00145     NDTFeatureRegNode(ros::NodeHandle param_nh) : sync_(2), sync2_(2)
00146     {
00147         bool xyzi_data;
00148         param_nh.param("xyzi_data", xyzi_data, false);
00149 
00150         if (!xyzi_data)
00151         {
00152             rgb_sub_.subscribe(nh_, "rgb", 2);
00153             depth_sub_.subscribe(nh_, "depth", 2);
00154             depth_param_sub_ = nh_.subscribe("depth_param", 1, &NDTFeatureRegNode::setupCamera, this);
00155 
00156             sync_.connectInput(rgb_sub_, depth_sub_);
00157             sync_.registerCallback( boost::bind(&NDTFeatureRegNode::rgbDepthCb, this, _1, _2) );
00158         }
00159         else
00160         {
00161             intensity_sub_.subscribe(nh_, "intensity", 1);
00162             points2_sub_.subscribe(nh_, "points2", 2);
00163             sync2_.connectInput(intensity_sub_, points2_sub_);
00164             sync2_.registerCallback( boost::bind(&NDTFeatureRegNode::intensityPoints2Cb, this, _1, _2) );
00165         }
00166 
00167         pub_points2_ = nh_.advertise<sensor_msgs::PointCloud2>("reg_points2", 15);
00168 
00169         param_nh.param("visualize", visualize_, false);
00170 
00171         param_nh.param("skip_matching", skip_matching_, false);
00172         param_nh.param("estimate_di", estimate_di_, false);
00173         param_nh.param("match_full", match_full_, false);
00174         param_nh.param("match_no_association", match_no_association_, false);
00175         param_nh.param("support_size", support_size_, 10);
00176         param_nh.param("max_var", max_var_, 0.3);
00177         param_nh.param("current_res", current_res_, 0.2);
00178         param_nh.param("max_nb_frames", max_nb_frames_, 10);
00179 
00180         param_nh.param("camera_nb", camera_nb_, 1);
00181 
00182         double max_inldist_xy, max_inldist_z;
00183         int nb_ransac;
00184 
00185         param_nh.param("max_inldist_xy", max_inldist_xy, 0.02);
00186         param_nh.param("max_inldist_z", max_inldist_z, 0.05);
00187         param_nh.param("nb_ransac", nb_ransac, 1000);
00188 
00189         proc = new NDTFrameProc<pcl::PointXYZ>(nb_ransac, max_inldist_xy, max_inldist_z);
00190 
00191         double detector_thresh;
00192         param_nh.param("detector_thresh", detector_thresh, 400.);
00193         //proc->detector = new cv::SurfFeatureDetector( detector_thresh );
00194         proc->detector = cv::FeatureDetector::create("SURF");
00195 
00196         param_nh.param("img_scale", proc->img_scale, 0.25);
00197         param_nh.param("trim_factor", proc->trim_factor, 1.);
00198         param_nh.param("non_mean", proc->non_mean, false);
00199         param_nh.param("windowed_matching2", proc->pe.windowed, false);
00200         param_nh.param("max_kp_dist", proc->pe.maxDist, 10.);
00201         param_nh.param("min_kp_dist", proc->pe.minDist, 0.);
00202         param_nh.param("set_initial_pose", set_initial_pose_, false);
00203         std::string default_world = "world";
00204         param_nh.param("world_frame", world_str, default_world);
00205 
00206         param_nh.param("publish_cloud", publish_cloud_, false);
00207         param_nh.param("subsample_step", subsample_step_, 5);
00208 
00209         camerasetup_ = false;
00210         global_transform_.setIdentity();
00211     }
00212 
00213     ~NDTFeatureRegNode()
00214     {
00215         delete proc;
00216     }
00217 
00218     void process(cv::Mat &rgb_img, cv::Mat &depth_img, const ros::Time &current_timestamp)
00219     {
00220         NDTFrame<pcl::PointXYZ> *frame = new NDTFrame<pcl::PointXYZ>();
00221 //        frame->img = rgb_img;
00222 //        frame->depth_img = depth_img;
00223         rgb_img.copyTo(frame->img);
00224         depth_img.copyTo(frame->depth_img);
00225         frame->supportSize = support_size_;
00226         frame->maxVar = max_var_;
00227         frame->current_res = current_res_;
00228         frame->cameraParams = cameraparams_;
00229         proc->addFrameIncremental(frame, skip_matching_, estimate_di_,match_full_,match_no_association_);
00230 
00231         if (visualize_)
00232             viewKeypointMatches(proc, 10);
00233 
00234 
00235         proc->trimNbFrames(max_nb_frames_);
00236 
00237         global_transform_ =  global_transform_ * proc->transformVector.back();
00238         tf::Transform transform;
00239         tf::Transform global_transform;
00240         TransformEigenToTF(proc->transformVector.back(), transform);
00241         TransformEigenToTF(global_transform_, global_transform);
00242         //             ros::Time current_timestamp = msg_rgb->header.stamp;
00243 
00244         //tf_.sendTransform(tf::StampedTransform(transform, current_timestamp, world_str, "ndt_feature_reg"));
00245         
00246         tf_.sendTransform(tf::StampedTransform(global_transform, current_timestamp, world_str, "global_ndt_feature_reg"));
00247 
00248         std::string gt_frame = "/camera1_rgb_optical_frame";
00249         if (publish_cloud_)
00250         {
00251             pcl::PointCloud<pcl::PointXYZ> depthcloud, sub_depthcloud;
00252             cameraparams_.convertDepthImageToPointCloud(frame->depth_img,depthcloud);
00253             // Subsample the pointcloud to be able to visualize longer sequences.
00254             subsamplePointCloud(depthcloud, sub_depthcloud, subsample_step_);
00255 
00256             sensor_msgs::PointCloud2 pcloud;
00257             pcl::toROSMsg(sub_depthcloud,pcloud);
00258             pcloud.header.stamp = ros::Time::now();
00259             pcloud.header.frame_id = "global_ndt_feature_reg";
00260             pub_points2_.publish(pcloud);
00261         }
00262 
00263         try
00264         {
00265             /*
00266             tf::StampedTransform gt_transform;
00267 
00268             tf_listener_.waitForTransform(gt_frame, prev_timestamp_,
00269                                           gt_frame, current_timestamp,
00270                                           world_str, ros::Duration(2.0));
00271 
00272             tf_listener_.lookupTransform(gt_frame, prev_timestamp_,
00273                                          gt_frame, current_timestamp,
00274                                          world_str, gt_transform);
00275 
00276             tf_.sendTransform(tf::StampedTransform(gt_transform, current_timestamp, world_str, "gt_reg"));
00277             */
00278 
00279             if (set_initial_pose_)
00280             {
00281                 tf::StampedTransform glb_gt_transform;
00282                 tf_listener_.lookupTransform(world_str, gt_frame, current_timestamp, glb_gt_transform);
00283                 TransformTFToEigen(glb_gt_transform, global_transform_);
00284                 set_initial_pose_ = false;
00285             }
00286 
00287             prev_timestamp_ = current_timestamp;
00288         }
00289         catch ( tf::TransformException & ex )
00290         {
00291             ROS_ERROR( "%s",
00292                        ex.what() );
00293         }
00294 
00295     }
00296 
00297     void intensityPoints2Cb(const sensor_msgs::ImageConstPtr& msg_img, const sensor_msgs::PointCloud2::ConstPtr& msg_pts)
00298     {
00299         m_.lock();
00300 
00301         ROS_INFO("Got intensity and points2 pair...");
00302         cv::Mat intensity(_imBridge.imgMsgToCv(msg_img, "mono8"));
00303         pcl::PointCloud<pcl::PointXYZ> pc;
00304         pcl::fromROSMsg (*msg_pts, pc);
00305 
00306         // Currently lacking support to handle pointcloud directly convert it to a depth image... also we don't have any calibration data from the Fotonic camera in the same format but can derrive this from the 3D depth data.
00307 
00308         if (!camerasetup_)
00309         {
00310             cameraparams_.setLookupTable(generateLookupTable(pc));
00311             camerasetup_ = true;
00312         }
00313         m_.unlock();
00314     }
00315 
00316     void rgbDepthCb(const sensor_msgs::ImageConstPtr& msg_rgb, const sensor_msgs::ImageConstPtr& msg_depth)
00317     {
00318         m_.lock();
00319         //ROS_INFO("Got image pair...");
00320         cv::Mat rgb_img(_imBridge.imgMsgToCv(msg_rgb, "bgr8")); // mono8
00321         cv::Mat depth_img(_imBridge.imgMsgToCv(msg_depth)); //, "mono16"));
00322 
00323         if (camerasetup_)
00324         {
00325             process(rgb_img, depth_img, msg_rgb->header.stamp);
00326         }
00327         m_.unlock();
00328     }
00329 
00330     EIGEN_MAKE_ALIGNED_OPERATOR_NEW // needed for 16B alignment
00331 
00332 };
00333 
00334 int main(int argc, char **argv)
00335 {
00336     ros::init(argc, argv, "ndt_feature_reg_node");
00337 //    ros::NodeHandle comm_nh ("camera"); // for topics, services
00338     ros::NodeHandle param_nh ("~");     // for parameters
00339 
00340     NDTFeatureRegNode n(param_nh);
00341     ros::spin();
00342 
00343     return 0;
00344 }


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov
autogenerated on Mon Jan 6 2014 11:36:18