00001 #include <agile_grasp/localization.h>
00002
00003 std::vector<GraspHypothesis> Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
00004 const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering)
00005 {
00006 double t0 = omp_get_wtime();
00007 std::vector<GraspHypothesis> hand_list;
00008
00009 if (size_left == 0 || cloud_in->size() == 0)
00010 {
00011 std::cout << "Input cloud is empty!\n";
00012 std::cout << size_left << std::endl;
00013 hand_list.resize(0);
00014 return hand_list;
00015 }
00016
00017
00018 std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n";
00019 Eigen::VectorXi pts_cam_source(cloud_in->size());
00020 if (size_left == cloud_in->size())
00021 pts_cam_source << Eigen::VectorXi::Zero(size_left);
00022 else
00023 pts_cam_source << Eigen::VectorXi::Zero(size_left), Eigen::VectorXi::Ones(cloud_in->size() - size_left);
00024
00025
00026 std::vector<int> nan_indices;
00027 pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, nan_indices);
00028
00029
00030 std::cout << "Filtering workspace ...\n";
00031 PointCloud::Ptr cloud(new PointCloud);
00032 filterWorkspace(cloud_in, pts_cam_source, cloud, pts_cam_source);
00033 std::cout << " " << cloud->size() << " points left\n";
00034
00035
00036 PointCloud::Ptr cloud_plot(new PointCloud);
00037 *cloud_plot = *cloud;
00038 *cloud_ = *cloud;
00039
00040
00041 std::cout << "Voxelizing point cloud\n";
00042 double t1_voxels = omp_get_wtime();
00043 voxelizeCloud(cloud, pts_cam_source, cloud, pts_cam_source, 0.003);
00044 double t2_voxels = omp_get_wtime() - t1_voxels;
00045 std::cout << " Created " << cloud->points.size() << " voxels in " << t2_voxels << " sec\n";
00046
00047
00048 if (plots_camera_sources_)
00049 plot_.plotCameraSource(pts_cam_source, cloud);
00050
00051 if (uses_clustering)
00052 {
00053 std::cout << "Finding point cloud clusters ... \n";
00054
00055
00056 pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
00057 pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
00058 pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
00059 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGBA>());
00060 seg.setOptimizeCoefficients(true);
00061 seg.setModelType(pcl::SACMODEL_PLANE);
00062 seg.setMethodType(pcl::SAC_RANSAC);
00063 seg.setMaxIterations(100);
00064 seg.setDistanceThreshold(0.01);
00065
00066
00067 seg.setInputCloud(cloud);
00068 seg.segment(*inliers, *coefficients);
00069 if (inliers->indices.size() == 0)
00070 {
00071 std::cout << " Could not estimate a planar model for the given dataset." << std::endl;
00072 hand_list.resize(0);
00073 return hand_list;
00074 }
00075
00076 std::cout << " PointCloud representing the planar component: " << inliers->indices.size()
00077 << " data points." << std::endl;
00078
00079
00080 pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
00081 extract.setInputCloud(cloud);
00082 extract.setIndices(inliers);
00083 extract.setNegative(true);
00084 std::vector<int> indices_cluster;
00085 extract.filter(indices_cluster);
00086 PointCloud::Ptr cloud_cluster(new PointCloud);
00087 cloud_cluster->points.resize(indices_cluster.size());
00088 Eigen::VectorXi cluster_cam_source(indices_cluster.size());
00089 for (int i = 0; i < indices_cluster.size(); i++)
00090 {
00091 cloud_cluster->points[i] = cloud->points[indices_cluster[i]];
00092 cluster_cam_source[i] = pts_cam_source[indices_cluster[i]];
00093 }
00094 cloud = cloud_cluster;
00095 *cloud_plot = *cloud;
00096 std::cout << " PointCloud representing the non-planar component: " << cloud->points.size()
00097 << " data points." << std::endl;
00098 }
00099
00100
00101 cloud_plot = cloud;
00102
00103
00104 bool plots_hands;
00105 if (plotting_mode_ == PCL_PLOTTING)
00106 plots_hands = true;
00107 else
00108 plots_hands = false;
00109
00110
00111 HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, hand_height_, init_bite_, num_threads_,
00112 num_samples_, cam_tf_left_, plots_hands);
00113 hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, calculates_antipodal, uses_clustering);
00114
00115
00116 if (filters_boundaries_)
00117 {
00118 std::cout << "Filtering out hands close to workspace boundaries ...\n";
00119 hand_list = filterHands(hand_list);
00120 std::cout << " # hands left: " << hand_list.size() << "\n";
00121 }
00122
00123 double t2 = omp_get_wtime();
00124 std::cout << "Hand localization done in " << t2 - t0 << " sec\n";
00125
00126 if (plotting_mode_ == PCL_PLOTTING)
00127 {
00128 plot_.plotHands(hand_list, cloud_plot, "Hands");
00129 }
00130 else if (plotting_mode_ == PCL_PLOTTING_FINGERS)
00131 {
00132 plot_.plotFingers(hand_list, cloud_plot, "Hands");
00133 }
00134 else if (plotting_mode_ == RVIZ_PLOTTING)
00135 {
00136 plot_.plotGraspsRviz(hand_list, visuals_frame_);
00137 }
00138
00139 return hand_list;
00140 }
00141
00142 std::vector<GraspHypothesis> Localization::predictAntipodalHands(const std::vector<GraspHypothesis>& hand_list,
00143 const std::string& svm_filename)
00144 {
00145 double t0 = omp_get_wtime();
00146 std::vector<GraspHypothesis> antipodal_hands;
00147 Learning learn(num_threads_);
00148 Eigen::Matrix<double, 3, 2> cams_mat;
00149 cams_mat.col(0) = cam_tf_left_.block<3, 1>(0, 3);
00150 cams_mat.col(1) = cam_tf_right_.block<3, 1>(0, 3);
00151 antipodal_hands = learn.classify(hand_list, svm_filename, cams_mat);
00152 std::cout << " runtime: " << omp_get_wtime() - t0 << " sec\n";
00153 std::cout << antipodal_hands.size() << " antipodal hand configurations found\n";
00154 if (plotting_mode_ == PCL_PLOTTING)
00155 {
00156 plot_.plotHands(hand_list, antipodal_hands, cloud_, "Antipodal Hands");
00157 }
00158 else if (plotting_mode_ == PCL_PLOTTING_FINGERS)
00159 {
00160 plot_.plotFingers(antipodal_hands, cloud_, "Antipodal Hands");
00161 }
00162 else if (plotting_mode_ == RVIZ_PLOTTING)
00163 {
00164 plot_.plotGraspsRviz(antipodal_hands, visuals_frame_, true);
00165 }
00166 return antipodal_hands;
00167 }
00168
00169 std::vector<GraspHypothesis> Localization::localizeHands(const std::string& pcd_filename_left,
00170 const std::string& pcd_filename_right, bool calculates_antipodal, bool uses_clustering)
00171 {
00172 std::vector<int> indices(0);
00173 return localizeHands(pcd_filename_left, pcd_filename_right, indices, calculates_antipodal, uses_clustering);
00174 }
00175
00176 std::vector<GraspHypothesis> Localization::localizeHands(const std::string& pcd_filename_left,
00177 const std::string& pcd_filename_right, const std::vector<int>& indices, bool calculates_antipodal,
00178 bool uses_clustering)
00179 {
00180 double t0 = omp_get_wtime();
00181
00182
00183 PointCloud::Ptr cloud_left(new PointCloud);
00184 if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(pcd_filename_left, *cloud_left) == -1)
00185 {
00186 std::cout << "Couldn't read pcd_filename_left file: " << pcd_filename_left << " \n";
00187 std::vector<GraspHypothesis> hand_list(0);
00188 return hand_list;
00189 }
00190 if (pcd_filename_right.length() > 0)
00191 std::cout << "Loaded left point cloud with " << cloud_left->width * cloud_left->height << " data points.\n";
00192 else
00193 std::cout << "Loaded point cloud with " << cloud_left->width * cloud_left->height << " data points.\n";
00194
00195 PointCloud::Ptr cloud_right(new PointCloud);
00196 if (pcd_filename_right.length() > 0)
00197 {
00198 if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(pcd_filename_right, *cloud_right) == -1)
00199 {
00200 std::cout << "Couldn't read pcd_filename_left file: " << pcd_filename_right << " \n";
00201 std::vector<GraspHypothesis> hand_list(0);
00202 return hand_list;
00203 }
00204 std::cout << "Loaded right point cloud with " << cloud_right->width * cloud_right->height << " data points.\n";
00205 std::cout << "Loaded both clouds in " << omp_get_wtime() - t0 << " sec\n";
00206 }
00207
00208
00209 std::cout << "Concatenating point clouds ...\n";
00210 PointCloud::Ptr cloud(new PointCloud);
00211 *cloud = *cloud_left + *cloud_right;
00212
00213 return localizeHands(cloud, cloud_left->size(), indices, calculates_antipodal, uses_clustering);
00214 }
00215
00216 void Localization::filterWorkspace(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
00217 PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out)
00218 {
00219 std::vector<int> indices(cloud_in->points.size());
00220 int c = 0;
00221 PointCloud::Ptr cloud(new PointCloud);
00222
00223
00224 for (int i = 0; i < cloud_in->points.size(); i++)
00225 {
00226
00227 const pcl::PointXYZRGBA& p = cloud_in->points[i];
00228 if (p.x >= workspace_(0) && p.x <= workspace_(1) && p.y >= workspace_(2) && p.y <= workspace_(3)
00229 && p.z >= workspace_(4) && p.z <= workspace_(5))
00230 {
00231 cloud->points.push_back(p);
00232 indices[c] = i;
00233 c++;
00234 }
00235 }
00236
00237 Eigen::VectorXi pts_cam_source(c);
00238 for (int i = 0; i < pts_cam_source.size(); i++)
00239 {
00240 pts_cam_source(i) = pts_cam_source_in(indices[i]);
00241 }
00242
00243 cloud_out = cloud;
00244 pts_cam_source_out = pts_cam_source;
00245 }
00246
00247 void Localization::voxelizeCloud(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
00248 PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out, double cell_size)
00249 {
00250 Eigen::Vector3d min_left, min_right;
00251 min_left << 10000, 10000, 10000;
00252 min_right << 10000, 10000, 10000;
00253 Eigen::Matrix3Xd pts(3, cloud_in->points.size());
00254 int num_left = 0;
00255 int num_right = 0;
00256 for (int i = 0; i < cloud_in->points.size(); i++)
00257 {
00258 if (pts_cam_source_in(i) == 0)
00259 {
00260 if (cloud_in->points[i].x < min_left(0))
00261 min_left(0) = cloud_in->points[i].x;
00262 if (cloud_in->points[i].y < min_left(1))
00263 min_left(1) = cloud_in->points[i].y;
00264 if (cloud_in->points[i].z < min_left(2))
00265 min_left(2) = cloud_in->points[i].z;
00266 num_left++;
00267 }
00268 else if (pts_cam_source_in(i) == 1)
00269 {
00270 if (cloud_in->points[i].x < min_right(0))
00271 min_right(0) = cloud_in->points[i].x;
00272 if (cloud_in->points[i].y < min_right(1))
00273 min_right(1) = cloud_in->points[i].y;
00274 if (cloud_in->points[i].z < min_right(2))
00275 min_right(2) = cloud_in->points[i].z;
00276 num_right++;
00277 }
00278 pts.col(i) = cloud_in->points[i].getVector3fMap().cast<double>();
00279 }
00280
00281
00282 std::set<Eigen::Vector3i, Localization::UniqueVectorComparator> bins_left;
00283 std::set<Eigen::Vector3i, Localization::UniqueVectorComparator> bins_right;
00284 int prev;
00285 for (int i = 0; i < pts.cols(); i++)
00286 {
00287 if (pts_cam_source_in(i) == 0)
00288 {
00289 Eigen::Vector3i v = floorVector((pts.col(i) - min_left) / cell_size);
00290 bins_left.insert(v);
00291 prev = bins_left.size();
00292 }
00293 else if (pts_cam_source_in(i) == 1)
00294 {
00295 Eigen::Vector3i v = floorVector((pts.col(i) - min_right) / cell_size);
00296 bins_right.insert(v);
00297 }
00298 }
00299
00300
00301 Eigen::Matrix3Xd voxels_left(3, bins_left.size());
00302 Eigen::Matrix3Xd voxels_right(3, bins_right.size());
00303 int i = 0;
00304 for (std::set<Eigen::Vector3i, Localization::UniqueVectorComparator>::iterator it = bins_left.begin();
00305 it != bins_left.end(); it++)
00306 {
00307 voxels_left.col(i) = (*it).cast<double>();
00308 i++;
00309 }
00310 i = 0;
00311 for (std::set<Eigen::Vector3i, Localization::UniqueVectorComparator>::iterator it = bins_right.begin();
00312 it != bins_right.end(); it++)
00313 {
00314 voxels_right.col(i) = (*it).cast<double>();
00315 i++;
00316 }
00317
00318 voxels_left.row(0) = voxels_left.row(0) * cell_size
00319 + Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(0);
00320 voxels_left.row(1) = voxels_left.row(1) * cell_size
00321 + Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(1);
00322 voxels_left.row(2) = voxels_left.row(2) * cell_size
00323 + Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(2);
00324 voxels_right.row(0) = voxels_right.row(0) * cell_size
00325 + Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(0);
00326 voxels_right.row(1) = voxels_right.row(1) * cell_size
00327 + Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(1);
00328 voxels_right.row(2) = voxels_right.row(2) * cell_size
00329 + Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(2);
00330
00331 PointCloud::Ptr cloud(new PointCloud);
00332 cloud->resize(bins_left.size() + bins_right.size());
00333 Eigen::VectorXi pts_cam_source(bins_left.size() + bins_right.size());
00334 for (int i = 0; i < bins_left.size(); i++)
00335 {
00336 pcl::PointXYZRGBA p;
00337 p.x = voxels_left(0, i);
00338 p.y = voxels_left(1, i);
00339 p.z = voxels_left(2, i);
00340 cloud->points[i] = p;
00341 pts_cam_source(i) = 0;
00342 }
00343 for (int i = 0; i < bins_right.size(); i++)
00344 {
00345 pcl::PointXYZRGBA p;
00346 p.x = voxels_right(0, i);
00347 p.y = voxels_right(1, i);
00348 p.z = voxels_right(2, i);
00349 cloud->points[bins_left.size() + i] = p;
00350 pts_cam_source(bins_left.size() + i) = 1;
00351 }
00352
00353 cloud_out = cloud;
00354 pts_cam_source_out = pts_cam_source;
00355 }
00356
00357 Eigen::Vector3i Localization::floorVector(const Eigen::Vector3d& a)
00358 {
00359 Eigen::Vector3i b;
00360 b << floor(a(0)), floor(a(1)), floor(a(2));
00361 return b;
00362 }
00363
00364 std::vector<GraspHypothesis> Localization::filterHands(const std::vector<GraspHypothesis>& hand_list)
00365 {
00366 const double MIN_DIST = 0.02;
00367
00368 std::vector<GraspHypothesis> filtered_hand_list;
00369
00370 for (int i = 0; i < hand_list.size(); i++)
00371 {
00372 const Eigen::Vector3d& center = hand_list[i].getGraspSurface();
00373 int k;
00374 for (k = 0; k < workspace_.size(); k++)
00375 {
00376 if (fabs((center(floor(k / 2.0)) - workspace_(k))) < MIN_DIST)
00377 {
00378 break;
00379 }
00380 }
00381 if (k == workspace_.size())
00382 {
00383 filtered_hand_list.push_back(hand_list[i]);
00384 }
00385 }
00386
00387 return filtered_hand_list;
00388 }
00389
00390 std::vector<Handle> Localization::findHandles(const std::vector<GraspHypothesis>& hand_list, int min_inliers,
00391 double min_length)
00392 {
00393 HandleSearch handle_search;
00394 std::vector<Handle> handles = handle_search.findHandles(hand_list, min_inliers, min_length);
00395 if (plotting_mode_ == PCL_PLOTTING)
00396 {
00397 plot_.plotHandles(handles, cloud_, "Handles");
00398 }
00399 else if (plotting_mode_ == PCL_PLOTTING_FINGERS)
00400 {
00401 plot_.plotFingers(handles, cloud_, "Handles");
00402 }
00403 else if (plotting_mode_ == RVIZ_PLOTTING)
00404 {
00405 plot_.plotHandlesRviz(handles, visuals_frame_);
00406 }
00407 return handles;
00408 }