void callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::ImageConstPtr& imageDepth, const sensor_msgs::CameraInfoConstPtr& camInfo) { if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 || image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 || image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) && imageDepth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0) { ROS_ERROR("Input type must be image=mono8,rgb8,bgr8 and image_depth=32FC1"); return; } cv_bridge::CvImageConstPtr imgPtr = cv_bridge::toCvShare(image); cv::Mat imageMono2; // convert to grayscale if(image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0) { cv::cvtColor(imgPtr->image, imageMono2, cv::COLOR_BGR2GRAY); } else if(image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) { cv::cvtColor(imgPtr->image, imageMono2, cv::COLOR_RGB2GRAY); } else { imageMono2 = imgPtr->image.clone(); } cv::Mat imageDepth2 = cv_bridge::toCvShare(imageDepth)->image.clone(); if(!lastImages_.first.empty()) { cv::Mat imageMono1 = lastImages_.first; cv::Mat imageDepth1 = lastImages_.second; if( imageMono1.cols != imageMono2.cols || imageMono1.rows != imageMono2.rows || imageDepth1.cols != imageDepth2.cols || imageDepth1.rows != imageDepth2.rows) { lastImages_.first = imageMono2; lastImages_.second = imageDepth2; ROS_ERROR("clouds must be the same size!\n"); return; } ros::WallTime time = ros::WallTime::now(); // Find 2d correspondences std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences = findCorrespondences(imageMono1, imageMono2); // Extract XYZ point clouds from correspondences pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); extractXYZCorrespondences( correspondences, imageDepth1, imageDepth2, *camInfo, *inliers1, *inliers2); if(correspondences.size() < 15) { ROS_WARN("Low correspondences -> %d", correspondences.size()); } pcl::PointCloud<pcl::PointXYZ>::Ptr inliersTransformed1; pcl::PointCloud<pcl::PointXYZ>::Ptr inliersTransformed2; if(frameId_.size()) { inliers1->header.frame_id = image->header.frame_id; inliers2->header.frame_id = image->header.frame_id; inliersTransformed1.reset(new pcl::PointCloud<pcl::PointXYZ>); inliersTransformed2.reset(new pcl::PointCloud<pcl::PointXYZ>); pcl_ros::transformPointCloud(frameId_, *inliers1, *inliersTransformed1, tfListener_); pcl_ros::transformPointCloud(frameId_, *inliers2, *inliersTransformed2, tfListener_); } else { inliersTransformed1 = inliers1; inliersTransformed2 = inliers2; } // Compute transform tf::Transform transform = transformFromXYZCorrespondences(inliersTransformed1, inliersTransformed2); if(std::isfinite(transform.getOrigin().x())) { // from optical frame to world frame tf::Transform motion = transform; //Update pose pose_ *= motion; double roll, pitch, yaw; pose_.getBasis().getEulerYPR(yaw,pitch,roll); ROS_INFO("Odom xyz(%f,%f,%f) rpy(%f,%f,%f) update time(%f s)", pose_.getOrigin().x(), pose_.getOrigin().y(), pose_.getOrigin().z(), roll, pitch, yaw, (ros::WallTime::now()-time).toSec()); } else { ROS_WARN("transform nan"); } } lastImages_.first = imageMono2; lastImages_.second = imageDepth2; //********************* // Publish odometry //********************* //first, we'll publish the transform over tf geometry_msgs::TransformStamped trans; trans.header.stamp = image->header.stamp; trans.header.frame_id = "odom"; trans.child_frame_id = frameId_; tf::transformTFToMsg(pose_, trans.transform); //send the transform tfBroadcaster_.sendTransform(trans); if(odomPub_.getNumSubscribers()) { //next, we'll publish the odometry message over ROS nav_msgs::Odometry odom; odom.header.stamp = image->header.stamp; // use corresponding time stamp to cloud odom.header.frame_id = "odom"; //set the position odom.pose.pose.position.x = pose_.getOrigin().x(); odom.pose.pose.position.y = pose_.getOrigin().y(); odom.pose.pose.position.z = pose_.getOrigin().z(); tf::quaternionTFToMsg(pose_.getRotation(), odom.pose.pose.orientation); //publish the message odomPub_.publish(odom); } }
void pcl_process::plane_fitting(const PointCloudRGB::ConstPtr& pcl_in) { PointCloudRGB cloud_input = *pcl_in; ROS_INFO("total points: %d", cloud_input.points.size()); //perform downsampling for the input pointcloud; pcl::VoxelGrid<pcl::PointXYZRGB> sor; PointCloudRGB::Ptr input_msg_filtered (new PointCloudRGB ()); float downsample_size_ = 0.05; sor.setInputCloud(cloud_input.makeShared()); sor.setLeafSize (downsample_size_, downsample_size_, downsample_size_); sor.filter (*input_msg_filtered); cloud_input = * input_msg_filtered; ROS_INFO("remained points: %d", cloud_input.points.size()); //at most 3 planes in the pool; pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers1 (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients2 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers2 (new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients3 (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers3 (new pcl::PointIndices); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane1 (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane2 (new pcl::PointCloud<pcl::PointXYZRGB> ()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr surface_plane3 (new pcl::PointCloud<pcl::PointXYZRGB> ()); // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZRGB> seg; seg.setOptimizeCoefficients (true); seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.1); pcl::ExtractIndices<pcl::PointXYZRGB> extract; seg.setInputCloud (cloud_input.makeShared ()); seg.segment (*inliers1, *coefficients1); extract.setInputCloud (cloud_input.makeShared()); extract.setIndices (inliers1); extract.setNegative (false); extract.filter (*surface_plane1); pcl::PointCloud<pcl::PointXYZRGB>::Ptr remained_points (new pcl::PointCloud<pcl::PointXYZRGB> ()); extract.setInputCloud (cloud_input.makeShared()); extract.setIndices (inliers1); extract.setNegative (true); extract.filter (*remained_points); double remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size()); if(remained_ratio > 0.2) { seg.setInputCloud (remained_points->makeShared ()); seg.segment (*inliers2, *coefficients2); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers2); extract.setNegative (false); extract.filter (*surface_plane2); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers2); extract.setNegative (true); extract.filter (*remained_points); remained_ratio = ((double)remained_points->points.size())/((double)cloud_input.points.size()); if(remained_ratio > 0.2) { seg.setInputCloud (remained_points->makeShared ()); seg.segment (*inliers3, *coefficients3); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers3); extract.setNegative (false); extract.filter (*surface_plane3); extract.setInputCloud (remained_points->makeShared ()); extract.setIndices (inliers3); extract.setNegative (true); extract.filter (*remained_points); } } int plane1_type, plane2_type, plane3_type; plane1_type = plane_classication(*inliers1, *coefficients1, cloud_input); plane2_type = plane_classication(*inliers2, *coefficients2, cloud_input); plane3_type = plane_classication(*inliers3, *coefficients3, cloud_input); if(plane1_type ==1) pcl_vis_pub_.publish(*surface_plane1); else if(plane2_type ==1) pcl_vis_pub_.publish(*surface_plane2); else if(plane3_type ==1) pcl_vis_pub_.publish(*surface_plane3); }