void MapBuilder::onCloudFrame(pcl::PointCloud<pcl::PointXYZ>::Ptr frame, pcl::PointXY sensorOffset)
{
RobotPosition pose = poseTracker->GetPosition();
//Transform current cloud to match robot's position
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
double mx = pose.X + sensorOffset.x;
double my = pose.Y + sensorOffset.y;
double mt = AngleUtils::degToRads(pose.Heading);
Eigen::Vector3f translation(mx, my, 0);
Eigen::Quaternionf rotation(Eigen::AngleAxisf(-mt, Eigen::Vector3f::UnitZ()));
pcl::transformPointCloud(*frame, *cloud_transformed, translation, rotation);
// ICP to refine cloud alignment
if(false)//!cloud->empty())
{
//Use ICP to fix errors in transformation
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setMaxCorrespondenceDistance(2);
icp.setMaximumIterations(30);
icp.setInputSource(cloud_transformed);
icp.setInputTarget(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_aligned (new pcl::PointCloud<pcl::PointXYZ>);
icp.align(*cloud_aligned);
if(icp.hasConverged())
{
//Add aligned cloud points to map cloud
(*cloud) += *cloud_aligned;
}
else
{
//Ignore alignment atempt
std::cout << "No convergence." << std::endl;
(*cloud) += *cloud_transformed;
}
}
else
{
//Add first cloud's points to map cloud
(*cloud) += *cloud_transformed;
}
//Run map cloud through a VoxelGrid to remove any duplicate points
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_removedDuplicates (new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> duplicateRemover;
duplicateRemover.setInputCloud(cloud);
float leafSize = ConfigManager::Instance().getValue("MapBuilder", "VoxelLeafSize", 0.25);
duplicateRemover.setLeafSize (leafSize, leafSize, leafSize);
duplicateRemover.filter(*cloud_removedDuplicates);
cloud.swap(cloud_removedDuplicates);
onNewMap(cloud);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr ObjectDetector::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
// Create rotation + translation matrix
Eigen::Affine3f transform = Eigen::Affine3f::Identity();
transform.translation() << 0.0, -y_translation, 0.0;
transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Perform transformation on point cloud
pcl::transformPointCloud(*cloud, *cloud_transformed, transform);
return cloud_transformed;
}
void SlamEngine::RegisterNext(const cv::Mat &imgRGB, const cv::Mat &imgDepth, double timestamp)
{
timestamps.push_back(timestamp);
PointCloudPtr cloud_new = ConvertToPointCloudWithoutMissingData(imgDepth, imgRGB, timestamp, frame_id);
#ifdef SHOW_Z_INDEX
cout << now_min_z << ' ' << now_max_z << endl;
#endif
PointCloudPtr cloud_downsampled = DownSamplingByVoxelGrid(cloud_new, downsample_rate, downsample_rate, downsample_rate);
std::cout << "Frame " << frame_id << ": ";
if (frame_id == 0)
{
total_start = clock();
last_cloud = cloud_new;
if (using_downsampling)
{
last_cloud = cloud_downsampled;
}
point_clouds.push_back(cloud_downsampled);
int m_size = last_cloud->size();
if (m_size < min_pt_count) min_pt_count = m_size;
if (m_size > max_pt_count) max_pt_count = m_size;
std::cout << "size: " << m_size;
if (using_icpcuda || using_feature)
{
imgDepth.copyTo(last_depth);
}
Frame *frame;
if (using_feature)
{
frame = new Frame(imgRGB, imgDepth, feature_type, Eigen::Matrix4f::Identity());
frame->relative_tran = Eigen::Matrix4f::Identity();
if (feature_type != "ORB")
frame->f->buildFlannIndex();
last_feature_frame = frame;
last_feature_keyframe = frame;
last_feature_frame_is_keyframe = true;
}
if (using_optimizer)
{
frame = new Frame(imgRGB, imgDepth, graph_feature_type, Eigen::Matrix4f::Identity());
frame->relative_tran = Eigen::Matrix4f::Identity();
frame->tran = frame->relative_tran;
string inliers, exists;
bool is_in_quadtree = false;
if (using_hogman_optimizer)
{
is_in_quadtree = hogman_manager.addNode(frame, 1.0, true, &inliers, &exists);
}
else if (using_srba_optimizer)
{
// is_in_quadtree = srba_manager.addNode(frame, 1.0, true, &inliers, &exists);
}
else if (using_robust_optimizer)
{
is_in_quadtree = robust_manager.addNode(frame, true);
}
if (!is_in_quadtree)
{
delete frame->f;
frame->f = nullptr;
}
#ifdef SAVE_TEST_INFOS
keyframe_candidates_id.push_back(frame_id);
keyframe_candidates.push_back(pair<cv::Mat, cv::Mat>(imgRGB, imgDepth));
if (last_keyframe_detect_lc)
{
keyframes_id.push_back(frame_id);
keyframes.push_back(pair<cv::Mat, cv::Mat>(imgRGB, imgDepth));
keyframes_inliers_sig.push_back(inliers);
keyframes_exists_sig.push_back(exists);
}
#endif
}
else
{
transformation_matrix.push_back(Eigen::Matrix4f::Identity());
}
accumulated_frame_count = 0;
accumulated_transformation = Eigen::Matrix4f::Identity();
last_transformation = Eigen::Matrix4f::Identity();
last_keyframe_transformation = Eigen::Matrix4f::Identity();
}
else
{
PointCloudPtr cloud_for_registration = cloud_new;
PointCloudPtr cloud_transformed(new PointCloudT);
Eigen::Matrix4f relative_tran = Eigen::Matrix4f::Identity();
Eigen::Matrix4f global_tran = Eigen::Matrix4f::Identity();
float weight = 1.0;
clock_t step_start = 0;
float step_time;
if (using_downsampling)
{
cloud_for_registration = cloud_downsampled;
}
point_clouds.push_back(cloud_downsampled);
int m_size = cloud_for_registration->size();
if (m_size < min_pt_count) min_pt_count = m_size;
if (m_size > max_pt_count) max_pt_count = m_size;
std::cout << "size: " << m_size;
if (using_gicp)
{
step_start = clock();
pcl::transformPointCloud(*cloud_for_registration, *cloud_transformed, last_transformation);
gicp->setInputSource(cloud_transformed);
gicp->setInputTarget(last_cloud);
gicp->align(*cloud_transformed);
Eigen::Matrix4f tran = gicp->getFinalTransformation();
step_time = (clock() - step_start) / 1000.0;
if (step_time < min_icp_time) min_icp_time = step_time;
if (step_time > max_icp_time) max_icp_time = step_time;
total_icp_time += step_time;
std::cout << ", gicp time: " << fixed << setprecision(3) << step_time;
weight = sqrt(1.0 / gicp->getFitnessScore());
if (weight < min_fit) min_fit = weight;
if (weight > max_fit) max_fit = weight;
std::cout << ", Weight: " << fixed << setprecision(3) << weight;
relative_tran = last_transformation * tran;
}
if (using_icpcuda)
{
step_start = clock();
icpcuda->initICPModel((unsigned short *)last_depth.data, 20.0f, Eigen::Matrix4f::Identity());
icpcuda->initICP((unsigned short *)imgDepth.data, 20.0f);
Eigen::Vector3f t = relative_tran.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = relative_tran.topLeftCorner(3, 3);
Eigen::Matrix4f estimated_tran = Eigen::Matrix4f::Identity();
Eigen::Vector3f estimated_t = estimated_tran.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> estimated_rot = estimated_tran.topLeftCorner(3, 3);
icpcuda->getIncrementalTransformation(t, rot, estimated_t, estimated_rot, threads, blocks);
step_time = (clock() - step_start) / 1000.0;
if (step_time < min_icp_time) min_icp_time = step_time;
if (step_time > max_icp_time) max_icp_time = step_time;
total_icp_time += step_time;
std::cout << ", icpcuda time: " << fixed << setprecision(3) << step_time;
weight = icpcuda->lastICPError > 0 ? sqrt(1.0 / icpcuda->lastICPError) : sqrt(1000000);
if (weight < min_fit) min_fit = weight;
if (weight > max_fit) max_fit = weight;
std::cout << ", Weight: " << fixed << setprecision(3) << weight;
relative_tran.topLeftCorner(3, 3) = rot;
relative_tran.topRightCorner(3, 1) = t;
}
bool isKeyframe = false;
bool ransac_failed = false;
Frame *f_frame;
if (using_feature)
{
f_frame = new Frame(imgRGB, imgDepth, feature_type, Eigen::Matrix4f::Identity());
vector<cv::DMatch> matches, inliers;
Eigen::Matrix4f tran;
Eigen::Matrix<double, 6, 6> information;
float rmse;
int pc, pcorrc;
if (feature_type == "ORB")
last_feature_frame->f->findMatchedPairsBruteForce(matches, f_frame->f);
else
last_feature_frame->f->findMatchedPairs(matches, f_frame->f);
if (Feature::getTransformationByRANSAC(tran, information, pc, pcorrc, rmse, &inliers,
last_feature_frame->f, f_frame->f, nullptr, matches))
{
relative_tran = tran;
cout << ", " << matches.size() << ", " << inliers.size();
matches.clear();
inliers.clear();
if (feature_type == "ORB")
last_feature_keyframe->f->findMatchedPairsBruteForce(matches, f_frame->f);
else
last_feature_keyframe->f->findMatchedPairs(matches, f_frame->f);
Feature::computeInliersAndError(inliers, rmse, nullptr, matches,
accumulated_transformation * relative_tran,
last_feature_keyframe->f, f_frame->f);
// if (Feature::getTransformationByRANSAC(tran, information, coresp, rmse, &inliers,
// last_feature_keyframe->f, f_frame->f, nullptr, matches))
// {
float rrr = (float)inliers.size() / matches.size();
if (last_feature_frame_is_keyframe)
{
last_rational = rrr;
}
rrr /= last_rational;
if (rrr < Config::instance()->get<float>("keyframe_rational"))
{
isKeyframe = true;
}
cout << ", " << rrr;
// }
// else
// {
// cout << ", failed";
// isKeyframe = true;
// }
}
else
{
ransac_failed = true;
ransac_failed_frames.push_back(frame_id);
isKeyframe = true;
icpcuda->initICPModel((unsigned short *)last_depth.data, 20.0f, Eigen::Matrix4f::Identity());
icpcuda->initICP((unsigned short *)imgDepth.data, 20.0f);
Eigen::Matrix4f tran2;
Eigen::Vector3f t = tran2.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = tran2.topLeftCorner(3, 3);
Eigen::Matrix4f estimated_tran = Eigen::Matrix4f::Identity();
Eigen::Vector3f estimated_t = estimated_tran.topRightCorner(3, 1);
Eigen::Matrix<float, 3, 3, Eigen::RowMajor> estimated_rot = estimated_tran.topLeftCorner(3, 3);
icpcuda->getIncrementalTransformation(t, rot, estimated_t, estimated_rot, threads, blocks);
tran2.topLeftCorner(3, 3) = rot;
tran2.topRightCorner(3, 1) = t;
relative_tran = tran2;
}
}
last_transformation = relative_tran;
accumulated_frame_count++;
accumulated_transformation = accumulated_transformation * relative_tran;
relative_tran = accumulated_transformation;
if (accumulated_frame_count >= Config::instance()->get<int>("max_keyframe_interval"))
{
isKeyframe = true;
}
if (isKeyframe)
{
accumulated_frame_count = 0;
accumulated_transformation = Eigen::Matrix4f::Identity();
}
Frame *g_frame;
if (using_optimizer)
{
/* step_start = clock();*/
if (using_hogman_optimizer)
{
global_tran = hogman_manager.getLastKeyframeTransformation() * relative_tran;
}
else if (using_srba_optimizer)
{
// global_tran = srba_manager.getLastKeyframeTransformation() * relative_tran;
}
else if (using_robust_optimizer)
{
global_tran = robust_manager.getLastKeyframeTransformation() * relative_tran;
}
if (isKeyframe)
{
g_frame = new Frame(imgRGB, imgDepth, graph_feature_type, global_tran);
g_frame->relative_tran = relative_tran;
g_frame->tran = global_tran;
g_frame->ransac_failed = ransac_failed;
string inliers, exists;
bool is_in_quadtree = false;
if (using_hogman_optimizer)
{
is_in_quadtree = hogman_manager.addNode(g_frame, weight, true, &inliers, &exists);
}
else if (using_srba_optimizer)
{
// is_in_quadtree = srba_manager.addNode(g_frame, weight, true, &inliers, &exists);
}
else if (using_robust_optimizer)
{
is_in_quadtree = robust_manager.addNode(g_frame, true);
}
last_keyframe_detect_lc = is_in_quadtree;
if (!is_in_quadtree)
{
delete g_frame->f;
g_frame->f = nullptr;
}
// #ifdef SAVE_TEST_INFOS
// keyframe_candidates_id.push_back(frame_id);
// keyframe_candidates.push_back(pair<cv::Mat, cv::Mat>(imgRGB, imgDepth));
//
// if (isKeyframe)
// {
// keyframes_id.push_back(frame_id);
// keyframes.push_back(pair<cv::Mat, cv::Mat>(imgRGB, imgDepth));
// keyframes_inliers_sig.push_back(inliers);
// keyframes_exists_sig.push_back(exists);
// }
// #endif
}
else
{
g_frame = new Frame();
g_frame->relative_tran = relative_tran;
g_frame->tran = global_tran;
if (using_hogman_optimizer)
{
hogman_manager.addNode(g_frame, weight, false);
}
else if (using_srba_optimizer)
{
// srba_manager.addNode(g_frame, weight, false);
}
else if (using_robust_optimizer)
{
robust_manager.addNode(g_frame, false);
}
}
// step_time = (clock() - step_start) / 1000.0;
// std::cout << endl;
// std::cout << "Feature: " << fixed << setprecision(3) << step_time;
}
else
{
transformation_matrix.push_back(last_keyframe_transformation * relative_tran);
if (isKeyframe)
last_keyframe_transformation = last_keyframe_transformation * relative_tran;
}
last_cloud = cloud_for_registration;
if (using_icpcuda || using_feature)
imgDepth.copyTo(last_depth);
if (using_feature)
{
if (!last_feature_frame_is_keyframe)
{
delete last_feature_frame->f;
last_feature_frame->f = nullptr;
}
if (feature_type != "ORB")
f_frame->f->buildFlannIndex();
last_feature_frame = f_frame;
if (isKeyframe)
{
delete last_feature_keyframe->f;
last_feature_keyframe->f = nullptr;
last_feature_keyframe = f_frame;
last_feature_frame_is_keyframe = true;
}
else
{
last_feature_frame_is_keyframe = false;
}
}
}
std::cout << endl;
frame_id++;
}
Eigen::Matrix4f performJointOptimization (PointCloudConstPtr source_cloud_ptr,
PointCloudConstPtr target_cloud_ptr, std::vector<Eigen::Vector4f, Eigen::aligned_allocator<
Eigen::Vector4f> >& source_feature_3d_locations, std::vector<Eigen::Vector4f,
Eigen::aligned_allocator<Eigen::Vector4f> >& target_feature_3d_locations,
Eigen::Matrix4f& initial_transformation)
{
//ICP cannot handle points with NaN values
std::vector<int> removed_points;
PointCloudPtr source_cloud_noNaN_ptr (new PointCloud);
PointCloudPtr target_cloud_noNaN_ptr (new PointCloud);
pcl::removeNaNFromPointCloud (*source_cloud_ptr, *source_cloud_noNaN_ptr, removed_points);
pcl::removeNaNFromPointCloud (*target_cloud_ptr, *target_cloud_noNaN_ptr, removed_points);
//pointcloud normals are required for icp point to plane
ROS_INFO("[performJointOptimization] Calculating point cloud normals...");
PointCloudNormalsPtr source_cloud_normals_ptr (new PointCloudNormals);
PointCloudNormalsPtr target_cloud_normals_ptr (new PointCloudNormals);
calculatePointCloudNormals (source_cloud_noNaN_ptr, source_cloud_normals_ptr);
calculatePointCloudNormals (target_cloud_noNaN_ptr, target_cloud_normals_ptr);
// the indices of features are required by icp joint optimization
std::vector<int> source_indices, target_indices;
getIndicesFromMatches<PointNormal> (source_cloud_normals_ptr, source_feature_3d_locations,
source_indices);
getIndicesFromMatches<PointNormal> (target_cloud_normals_ptr, target_feature_3d_locations,
target_indices);
boost::shared_ptr<TransformationEstimationWDF<PointNormal, PointNormal> > initial_transform_WDF (
new TransformationEstimationWDF<PointNormal, PointNormal> ());
// Please see rgbd_registration.launch for an explanation of the following parameters
ParameterServer* ps = ParameterServer::instance ();
initial_transform_WDF->setAlpha (ps->get<double> ("alpha"));
initial_transform_WDF->setCorrespondecesDFP (source_indices, target_indices);
pcl::IterativeClosestPoint<PointNormal, PointNormal> icp_wdf;
icp_wdf.setMaxCorrespondenceDistance (ps->get<double> ("max_correspondence_dist"));
icp_wdf.setMaximumIterations (ps->get<int> ("max_iterations"));
icp_wdf.setTransformationEpsilon (ps->get<double> ("transformation_epsilon"));
icp_wdf.setEuclideanFitnessEpsilon (ps->get<double> ("euclidean_fitness_epsilon")); //1
// Set TransformationEstimationWDF as ICP transform estimator
icp_wdf.setTransformationEstimation (initial_transform_WDF);
checkforNaNs (source_cloud_normals_ptr);
checkforNaNs (target_cloud_normals_ptr);
icp_wdf.setInputCloud (source_cloud_normals_ptr);
icp_wdf.setInputTarget (target_cloud_normals_ptr);
if (ps->get<bool> ("enable_pcl_debug_verbosity"))
pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);
else
ROS_INFO(
"[performJointOptimization] Now Performing Joint Optimization. This could take up to several minutes.....");
PointCloudNormalsPtr cloud_transformed (new PointCloudNormals);
if (ParameterServer::instance ()->get<bool> ("use_ransac_to_initialize_icp"))
icp_wdf.align (*cloud_transformed, initial_transformation);
else
icp_wdf.align (*cloud_transformed);
ROS_INFO_STREAM(
"[performJointOptimization] Has converged? = " << icp_wdf.hasConverged () << std::endl << " fitness score (SSD): " << icp_wdf.getFitnessScore (1000) << " \n Final Transformation: \n" << icp_wdf.getFinalTransformation());
if (ps->get<bool> ("save_all_pointclouds"))
{
writePCDToFile ("source_cloud.pcd", source_cloud_normals_ptr);
writePCDToFile ("target_cloud.pcd", target_cloud_normals_ptr);
writePCDToFile ("source_cloud_features.pcd", source_cloud_normals_ptr, source_indices);
writePCDToFile ("target_cloud_features.pcd", target_cloud_normals_ptr, target_indices);
transformAndWriteToFile (source_cloud_normals_ptr, source_indices,
icp_wdf.getFinalTransformation ());
}
return icp_wdf.getFinalTransformation ();
}
// Proccess the point clouds
void processPointCloud( const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg )
{
ROS_INFO_NAMED("perception","\n\n\n");
ROS_INFO_STREAM_NAMED("perception","Processing new point cloud");
// ---------------------------------------------------------------------------------------------
// Start making result
result_.blocks.poses.clear(); // Clear last block perception result
result_.blocks.header.stamp = pointcloud_msg->header.stamp;
result_.blocks.header.frame_id = base_link;
// Basic point cloud conversions ---------------------------------------------------------------
// Convert from ROS to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg(*pointcloud_msg, cloud);
// Make new point cloud that is in our working frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
// Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
ROS_INFO_STREAM_NAMED("perception","Waiting for transform...");
ros::spinOnce();
tf_listener_.waitForTransform(base_link, cloud.header.frame_id, cloud.header.stamp, ros::Duration(2.0));
if(!pcl_ros::transformPointCloud(base_link, cloud, *cloud_transformed, tf_listener_))
{
if( process_count_ > 1 ) // the first time we can ignore it
ROS_ERROR_STREAM_NAMED("perception","Error converting to desired frame");
// Do this to speed up the next process attempt:
process_count_ = PROCESS_EVERY_NTH;
return;
}
// Limit to things we think are roughly at the table height ------------------------------------
// pcl::PointIndices::Ptr filtered_indices(new pcl::PointIndices); // hold things at table height
// std::vector<int>
boost::shared_ptr<std::vector<int> > filtered_indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05);
//pass.setFilterLimits(table_height - 0.01, table_height + block_size + 0.02); // DTC
pass.filter(*filtered_indices);
/*
// Limit to things in front of the robot ---------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
//pass.setInputCloud(cloud_filteredZ);
pass.setIndices(filtered_indices);
pass.setFilterFieldName("x");
pass.setFilterLimits(.1,.5);
pass.filter(*cloud_filtered);
*/
/*
// Check if any points remain
if( cloud_filtered->points.size() == 0 )
{
ROS_ERROR_STREAM_NAMED("perception","0 points left");
return;
}
else
{
ROS_INFO_STREAM_NAMED("perception","Filtered, %d points left", (int) cloud_filtered->points.size());
}
*/
// Segment components --------------------------------------------------------------------------
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
seg.setMaxIterations(200); // the maximum number of iterations the sample consensus method will run
seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
/*
int nr_points = cloud_filtered->points.size();
// Segment cloud until there are less than 30% of points left? not sure why this is necessary
while(cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud (find the table)
seg.setInputCloud(cloud_filtered);
// seg.setIndices();
seg.segment(*inliers, *model_coefficients);
if(inliers->indices.size() == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Could not estimate a planar model for the given dataset.");
return;
}
//std::cout << "Inliers: " << (inliers->indices.size()) << std::endl;
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Copy the extracted component (the table) to a seperate point cloud
extract.filter(*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_filtered); // remove table from cloud_filtered
// Debug output - DTC
// Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
ROS_INFO_STREAM_NAMED("perception", "Model coefficients: " << model_coefficients->values[0] << " "
<< model_coefficients->values[1] << " "
<< model_coefficients->values[2] << " "
<< model_coefficients->values[3] ); // TODO: turn this into an rviz marker somehow?
*/
// Show groups of recognized objects (the inliers)
/*std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i) {
std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;
}*/
// }
// DTC: Removed to make compatible with PCL 1.5
// Creating the KdTree object for the search method of the extraction
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTreeFLANN<pcl::PointXYZRGB>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
// tree->setInputCloud(cloud_filtered);
tree->setInputCloud(cloud_transformed, filtered_indices);
// Find the clusters (objects) on the table
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> cluster_extract;
//cluster_extract.setClusterTolerance(0.005); // 5mm - If you take a very small value, it can happen that an actual object can be seen as multiple clusters. On the other hand, if you set the value too high, it could happen, that multiple objects are seen as one cluster. So our recommendation is to just test and try out which value suits your dataset.
cluster_extract.setClusterTolerance(0.02); // 2cm
cluster_extract.setMinClusterSize(100);
cluster_extract.setMaxClusterSize(25000);
// cluster_extract.setSearchMethod(tree);
// cluster_extract.setInputCloud(cloud_filtered);
cluster_extract.setInputCloud(cloud_transformed);
cluster_extract.setIndices(filtered_indices);
ROS_INFO_STREAM_NAMED("perception","Extracting...");
cluster_extract.extract(cluster_indices);
ROS_INFO_STREAM_NAMED("perception","after cluster extract");
// Publish point cloud data
// filtered_pub_.publish(cloud_filtered);
// plane_pub_.publish(cloud_plane);
ROS_WARN_STREAM_NAMED("perception","Number indicies/clusters: " << cluster_indices.size() );
// processClusters( cluster_indices, pointcloud_msg, cloud_filtered );
processClusters( cluster_indices, cloud_transformed, cloud_filtered, cloud );
// ---------------------------------------------------------------------------------------------
// Final results
if(result_.blocks.poses.size() > 0)
{
// Change action state, if we the action is currently active
if(action_server_.isActive())
{
action_server_.setSucceeded(result_);
}
// Publish block poses
block_pose_pub_.publish(result_.blocks);
// Publish rviz markers of the blocks
publishBlockLocation();
ROS_INFO_STREAM_NAMED("perception","Finished ---------------------------------------------- ");
}
else
{
ROS_INFO_STREAM_NAMED("perception","Couldn't find any blocks this iteration!");
}
}
// Proccess the point clouds, called when subscriber gets msg
void cloudCallback( const sensor_msgs::PointCloud2ConstPtr& msg )
{
ROS_INFO_STREAM("Recieved callback");
// Basic point cloud conversions ---------------------------------------------------------------
// Convert from ROS to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg(*msg, cloud);
// Make new point cloud that is in our working frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
// Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
tf_listener_.waitForTransform(std::string(arm_link), cloud.header.frame_id,
cloud.header.stamp, ros::Duration(1.0));
if(!pcl_ros::transformPointCloud(std::string(arm_link), cloud, *cloud_transformed, tf_listener_))
{
ROS_ERROR("Error converting to desired frame");
return;
}
// Limit to things we think are roughly at the table height ------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredZ(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_cam_dist-.05,max_cam_dist+0.05);
pass.filter(*cloud_filteredZ);
// Limit to things in front of the robot ---------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pass.setInputCloud(cloud_filteredZ);
pass.setFilterFieldName("x");
pass.setFilterLimits(.1,.5);
pass.filter(*cloud_filtered);
// Check if any points remain
if( cloud_filtered->points.size() == 0 )
{
ROS_ERROR("0 points left");
return;
}
else
{
ROS_INFO("[block detection] Filtered, %d points left", (int) cloud_filtered->points.size());
}
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
int nr_points = cloud_filtered->points.size();
// Segment cloud until there are less than 30% of points left? not sure why this is necessary
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
seg.setMaxIterations(200);
seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
while(cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud (find the table)
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if(inliers->indices.size() == 0)
{
ROS_ERROR("[block detection] Could not estimate a planar model for the given dataset.");
return;
}
// Debug output - DTC
// Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
}//end while
// DTC: Removed to make compatible with PCL 1.5
// Creating the KdTree object for the search method of the extraction
//pcl::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
//tree->setInputCloud(cloud_filtered);
// Publish point cloud data
depth_pub_.publish(cloud_filtered);
}//end cloudCallBack
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
int main(int argc, char** argv) {
if (argc != 3) {
std::cerr << "please provide 2 point clouds as arguments: <source>.pcd <target>.pcd)" << std::endl;
exit(0);
}
PointCloud::Ptr cloudSource(new PointCloud);
PointCloud::Ptr cloudOut(new PointCloud);
PointCloud::Ptr cloudTarget(new PointCloud);
PointCloud::Ptr cloudSourceFiltered(new PointCloud);
PointCloud::Ptr cloudTargetFiltered(new PointCloud);
PointCloudNormal::Ptr cloudSourceNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesSource(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesTarget(new PointCloudNormal);
PointCloudNormal::Ptr cloudSourceNormalNoNaNs(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormalNoNaNs(new PointCloudNormal);
pcl::PCDWriter writer;
std::vector<int> indicesSource, indicesTarget;
//Fill in the cloud data
ROS_INFO("Reading files....");
pcl::PCDReader reader;
reader.read(argv[1], *cloudSource);
reader.read(argv[2], *cloudTarget);
//pcl::copyPointCloud (*cloudSourceNormal, *cloudSource);
//pcl::copyPointCloud (*cloudTargetNormal, *cloudTarget);
normalEstimation(cloudSource, cloudSourceNormal, 0.03);
normalEstimation(cloudTarget, cloudTargetNormal, 0.03);
std::vector<int> sourceHandleClusters;
std::vector<int> targetHandleClusters;
ROS_INFO("Extracting handles....");
extractHandles(cloudSource, cloudSourceFiltered, cloudSourceNormal, sourceHandleClusters);
extractHandles(cloudTarget, cloudTargetFiltered, cloudTargetNormal, targetHandleClusters);
std::vector<int> removedPoints;
removeNaNs(cloudSourceNormal, cloudSourceNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(sourceHandleClusters, removedPoints);
removeNaNs(cloudTargetNormal, cloudTargetNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(targetHandleClusters, removedPoints);
writer.write("handlesSource.pcd", *cloudSourceNormalNoNaNs, sourceHandleClusters, true);
writer.write("handlesTarget.pcd", *cloudTargetNormalNoNaNs, targetHandleClusters, true);
ROS_INFO("Calculating inital transformation from ICP SVD on handles....");
Eigen::Matrix4f guess;
guess << 1, 0, 0, 0.0,
0, 1, 0, 0.5,
0, 0, 1, 0.33,
0, 0, 0, 1;
pcl::IterativeClosestPoint<PointNormal, PointNormal> icp;
// set source and target clouds from indices of pointclouds
pcl::ExtractIndices<PointNormal> handlefilter;
pcl::PointIndices::Ptr sourceHandleIndices (new pcl::PointIndices);
sourceHandleIndices->indices = sourceHandleClusters;
handlefilter.setIndices(sourceHandleIndices);
handlefilter.setInputCloud(cloudSourceNormalNoNaNs);
handlefilter.filter(*cloudHandlesSource);
icp.setInputCloud(cloudHandlesSource);
pcl::PointIndices::Ptr targetHandleIndices (new pcl::PointIndices);
targetHandleIndices->indices = targetHandleClusters;
handlefilter.setIndices(targetHandleIndices);
handlefilter.setInputCloud(cloudTargetNormalNoNaNs);
handlefilter.filter(*cloudHandlesTarget);
icp.setInputTarget(cloudHandlesTarget);
PointCloudNormal Final;
icp.align(Final, guess);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
ROS_INFO("Initialize transformation estimation object....");
boost::shared_ptr< TransformationEstimationJointOptimize<PointNormal, PointNormal > >
transformationEstimation_(new TransformationEstimationJointOptimize<PointNormal, PointNormal>());
float denseCloudWeight = 1.0;
float visualFeatureWeight = 0.0;
float handleFeatureWeight = 0.25;
transformationEstimation_->setWeights(denseCloudWeight, visualFeatureWeight, handleFeatureWeight);
transformationEstimation_->setCorrespondecesDFP(indicesSource, indicesTarget);
// Eigen::Matrix4f guess;
// guess << 0.999203, 0.0337772, -0.0213298, 0.0110106,
// -0.0349403, 0.99778, -0.0567293, -0.00746282,
// 0.0193665, 0.0574294, 0.998162, 0.0141431,
// 0, 0, 0, 1;
// guess << 1, 0.2, 0.3, -0.3,
// -0.2, 1, -0.2, 0.9,
// -0.3, 0.2, 1, 0.4,
// 0, 0, 0, 1;
// guess << 1, 0, 0, 0.0,
// 0, 1, 0, 0.5,
// 0, 0, 1, 0.33,
// 0, 0, 0, 1;
// icp svd for handles node_1/node_91
// guess << 0.993523, 0.0152363, -0.112636, 0.138385,
// -0.0264756, 0.994739, -0.0989777, 0.615225,
// 0.110535, 0.101318, 0.988696, 0.347863,
// 0, 0, 0, 1;
// custom icp
ROS_INFO("Initialize icp object....");
pcl::IterativeClosestPointJointOptimize<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icpJointOptimize; //JointOptimize
icpJointOptimize.setMaximumIterations (20);
icpJointOptimize.setTransformationEpsilon (0);
icpJointOptimize.setMaxCorrespondenceDistance(0.1);
icpJointOptimize.setRANSACOutlierRejectionThreshold(0.03);
icpJointOptimize.setEuclideanFitnessEpsilon (0);
icpJointOptimize.setTransformationEstimation (transformationEstimation_);
icpJointOptimize.setHandleSourceIndices(sourceHandleClusters);
icpJointOptimize.setHandleTargetIndices(targetHandleClusters);
icpJointOptimize.setInputCloud(cloudSourceNormalNoNaNs);
icpJointOptimize.setInputTarget(cloudTargetNormalNoNaNs);
ROS_INFO("Running ICP....");
PointCloudNormal::Ptr cloud_transformed( new PointCloudNormal);
icpJointOptimize.align ( *cloud_transformed, icp.getFinalTransformation()); //init_tr );
std::cout << "[SIIMCloudMatch::runICPMatch] Has converged? = " << icpJointOptimize.hasConverged() << std::endl <<
" fitness score (SSD): " << icpJointOptimize.getFitnessScore (1000) << std::endl
<< icpJointOptimize.getFinalTransformation () << "\n";
transformPointCloud (*cloudSource, *cloudOut, icpJointOptimize.getFinalTransformation());
ROS_INFO("Writing output....");
writer.write("converged.pcd", *cloudOut, true);
}
