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);
}
