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
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
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
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 ¤t_timestamp)
00219 {
00220 NDTFrame<pcl::PointXYZ> *frame = new NDTFrame<pcl::PointXYZ>();
00221
00222
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
00243
00244
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
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
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
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
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
00320 cv::Mat rgb_img(_imBridge.imgMsgToCv(msg_rgb, "bgr8"));
00321 cv::Mat depth_img(_imBridge.imgMsgToCv(msg_depth));
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
00331
00332 };
00333
00334 int main(int argc, char **argv)
00335 {
00336 ros::init(argc, argv, "ndt_feature_reg_node");
00337
00338 ros::NodeHandle param_nh ("~");
00339
00340 NDTFeatureRegNode n(param_nh);
00341 ros::spin();
00342
00343 return 0;
00344 }