std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud,
const Eigen::VectorXi& pts_cam_source, const std::vector<int>& indices,
const PointCloud::Ptr cloud_plot, bool calculates_antipodal,
bool uses_clustering)
{
// create KdTree for neighborhood search
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
cloud_normals_.resize(3, cloud->size());
cloud_normals_.setZero(3, cloud->size());
// calculate normals for all points
if (calculates_antipodal)
{
//std::cout << "Calculating normals for all points\n";
nn_radius_taubin_ = 0.01;
std::vector<int> indices_cloud(cloud->size());
for (int i = 0; i < indices_cloud.size(); i++)
indices_cloud[i] = i;
findQuadrics(cloud, pts_cam_source, kdtree, indices_cloud);
nn_radius_taubin_ = 0.03;
}
// draw samples from the point cloud uniformly
std::vector<int> indices_rand;
Eigen::VectorXi hands_cam_source;
if (indices.size() == 0)
{
double t_rand = omp_get_wtime();
//std::cout << "Generating uniform random indices ...\n";
indices_rand.resize(num_samples_);
pcl::RandomSample<pcl::PointXYZ> random_sample;
random_sample.setInputCloud(cloud);
random_sample.setSample(num_samples_);
random_sample.filter(indices_rand);
hands_cam_source.resize(num_samples_);
for (int i = 0; i < num_samples_; i++)
hands_cam_source(i) = pts_cam_source(indices_rand[i]);
//std::cout << " Done in " << omp_get_wtime() - t_rand << " sec\n";
}
else
indices_rand = indices;
if (plots_samples_)
plot_.plotSamples(indices_rand, cloud);
// find quadrics
//std::cout << "Estimating local axes ...\n";
std::vector<Quadric> quadric_list = findQuadrics(cloud, pts_cam_source, kdtree, indices_rand);
if (plots_local_axes_)
plot_.plotLocalAxes(quadric_list, cloud_plot);
// find hands
//std::cout << "Finding hand poses ...\n";
std::vector<GraspHypothesis> hand_list = findHands(cloud, pts_cam_source, quadric_list, hands_cam_source, kdtree);
return hand_list;
}
Eigen::Vector3d obj_axe_2_points_next(int i_head,int i_center){
pcl::PointXYZRGB center,head;
center = cloud->at(i_center); //x1 y1 z1
head = cloud->at(i_head); //x2 y2 z2
Eigen::Vector3d vector_l;
vector_l(0)=5*(head.x - center.x);
vector_l(1)=5*(head.y - center.y);
vector_l(2)=5*(head.z - center.z);
return vector_l;
}
bool RegionGrowing::seedRegionGrowing(
pcl::PointCloud<PointNormalT>::Ptr src_points,
const PointT seed_point, const PointCloud::Ptr cloud,
PointNormal::Ptr normals) {
if (cloud->empty() || normals->size() != cloud->size()) {
ROS_ERROR("- Region growing failed. Incorrect inputs sizes ");
return false;
}
if (isnan(seed_point.x) || isnan(seed_point.y) || isnan(seed_point.z)) {
ROS_ERROR("- Seed Point is Nan. Skipping");
return false;
}
this->kdtree_->setInputCloud(cloud);
std::vector<int> neigbor_indices;
this->getPointNeigbour<int>(neigbor_indices, seed_point, 1);
int seed_index = neigbor_indices[0];
const int in_dim = static_cast<int>(cloud->size());
int *labels = reinterpret_cast<int*>(malloc(sizeof(int) * in_dim));
#ifdef _OPENMP
#pragma omp parallel for num_threads(this->num_threads_)
#endif
for (int i = 0; i < in_dim; i++) {
if (i == seed_index) {
labels[i] = 1;
}
labels[i] = -1;
}
this->seedCorrespondingRegion(labels, cloud, normals,
seed_index, seed_index);
src_points->clear();
for (int i = 0; i < in_dim; i++) {
if (labels[i] != -1) {
PointNormalT pt;
pt.x = cloud->points[i].x;
pt.y = cloud->points[i].y;
pt.z = cloud->points[i].z;
pt.r = cloud->points[i].r;
pt.g = cloud->points[i].g;
pt.b = cloud->points[i].b;
pt.normal_x = normals->points[i].normal_x;
pt.normal_y = normals->points[i].normal_y;
pt.normal_z = normals->points[i].normal_z;
src_points->push_back(pt);
}
}
free(labels);
return true;
}
void App::VisZ(TUMDataSetVisualizer * viewer) {
float const maxZ = max_element(Input_->begin(), Input_->end(),
[] (PointType const& lft, PointType const& rgh) {
if (pcl_isfinite(lft.z) && pcl_isfinite(rgh.z)) {
return lft.z < rgh.z;
} else {
return ! pcl_isfinite(lft.z);
}
})->z;
viewer->EasyAdd(Input_, "input", [this, &maxZ] (int i) { // #6
float const relZ = Input_->at(i).z / maxZ;
return Color({static_cast<int>(relZ * 255), 0, static_cast<int>((1 - relZ) * 255)});
});
}
void Mapper::viewer()
{
pcl::visualization::CloudViewer viewer("viewer");
PointCloud::Ptr globalMap (new PointCloud);
pcl::VoxelGrid<PointT> voxel;
voxel.setLeafSize( resolution, resolution, resolution );
while (shutdownFlag == false)
{
static int cntGlobalUpdate = 0;
if ( poseGraph.keyframes.size() <= this->keyframe_size )
{
usleep(1000);
continue;
}
// keyframe is updated
PointCloud::Ptr tmp(new PointCloud());
if (cntGlobalUpdate % 15 == 0)
{
// update all frames
cout<<"redrawing frames"<<endl;
globalMap->clear();
for ( int i=0; i<poseGraph.keyframes.size(); i+=2 )
{
PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
*globalMap += *cloud;
}
}
else
{
for ( int i=poseGraph.keyframes.size()-1; i>=0 && i>poseGraph.keyframes.size()-6; i-- )
{
PointCloud::Ptr cloud = this->generatePointCloud(poseGraph.keyframes[i]);
*globalMap += *cloud;
}
}
cntGlobalUpdate ++ ;
//voxel
voxel.setInputCloud( globalMap );
voxel.filter( *tmp );
keyframe_size = poseGraph.keyframes.size();
globalMap->swap( *tmp );
viewer.showCloud( globalMap );
cout<<"points in global map: "<<globalMap->points.size()<<endl;
}
}
ViewExtract::Ptr ViewSampleExtractor::extract( cv::Rect& rct) const
{
const PointCloud::Ptr vpc = view_->points;
// Ensure rectangle falls within organised point cloud dimensions
const cv::Rect pcRect(0,0, vpc->cols(), vpc->rows());
rct &= pcRect; // Set intersection
ViewExtract::Ptr ve( new ViewExtract( vpc, rct));
ve->setModelName( modelName_);
ve->setPartName( partName_);
ve->setAspectInfo( aspectInfo_);
ve->setPosVec( view_->posVec);
ve->setDirVec( view_->focalVec);
ve->setUpVec( view_->upVec);
return ve;
} // end extract
int main(int argC,char **argV)
{
ros::init(argC,argV,"startPointCloud");
ros::NodeHandle n;
std::string serverAddress;
n.getParam("/serverNameOrIP",serverAddress);
Socket mySocket(serverAddress.c_str(),"9005",streamSize);
ros::Publisher pub = n.advertise<PointCloud>("point_cloud",1);
PointCloud::Ptr pc (new PointCloud);
pc->header.frame_id = ros::this_node::getNamespace().substr(1,std::string::npos) + "/kinect_pcl";
while(ros::ok())
{
// TODO(Somhtr): change to ROS' logging API
cout << "Reading data..." << endl;
mySocket.readData();
// TODO(Somhtr): change to ROS' logging API
cout << "Copying data..." << endl;
float* pt_coords = reinterpret_cast<float*>(mySocket.mBuffer);
for(uint64_t idx=0; idx<numberOfPoints; idx+=3)
{
pc->push_back(pcl::PointXYZ(
pt_coords[idx], pt_coords[idx+1], pt_coords[idx+2]
));
}
double utcTime;
memcpy(&utcTime,&mySocket.mBuffer[pointBufferSize],sizeof(double));
pc->header.stamp = ros::Time(utcTime).toSec();
pub.publish(pc);
pc->clear();
ros::spinOnce();
}
return 0;
}
void WorldDownloadManager::pclPointCloudToMessage(PointCloud::Ptr pcl_cloud,
std::vector<kinfu_msgs::KinfuCloudPoint> & message)
{
uint cloud_size = pcl_cloud->size();
message.resize(cloud_size);
for (uint i = 0; i < cloud_size; i++)
{
message[i].x = (*pcl_cloud)[i].x;
message[i].y = (*pcl_cloud)[i].y;
message[i].z = (*pcl_cloud)[i].z;
}
}
int ObjectAspect::calculate(const cv::Mat &image, const PointCloud::Ptr &pointcloud) {
cv::Mat gray,timg;
image.copyTo(this->image);
this->points = pointcloud;
#if 1
cv::cvtColor(image,gray,CV_BGR2HLS);
//gray = planes[2]-planes[1];
std::vector<cv::Mat> planes;
cv::split(gray,planes);
gray = planes[1];
cv::merge(&planes[0],(size_t)3,timg);
cv::cvtColor(timg,timg,CV_HSV2BGR);
#else
timg = image;
#endif
#ifdef DEBUG_VIS
cv::cvtColor(gray,timg,CV_GRAY2BGR);
#endif
const int HessianThreshold = 500;
cv::SURF bug(HessianThreshold);
bug.extended = false;
bug(gray,255*cv::Mat::ones(gray.rows,gray.cols,CV_8U),keypoints,descriptors);
for(size_t i = 0; i < keypoints.size(); i++) {
cv::KeyPoint kp = keypoints[i];
PointType pt3d = pointcloud->at(kp.pt.x,kp.pt.y);
pt3d.rgb = kp.response;
if(!isnan(pt3d.x)/*&&(kp.response/1000>3)*/) {
keypoints3D->points.push_back(pt3d);
map2D3D.insert(std::make_pair<int,int>(i,keypoints3D->points.size()-1));
map2D3Dinv.insert(std::make_pair<int,int>(keypoints3D->points.size()-1,i));
}
}
#ifdef DEBUG_VIS
plotKeypoints(timg);
cv::namedWindow("surf");
cv::imshow("surf",timg);
#endif
return 0;
}
int main( int argc, char* argv[] )
{
if( argc < 3 )
{
usage();
exit(0);
}
string asc_file = argv[1];
string ply_file = argv[2];
int sampling = 1;
float cell_size = 0;
if( argc >= 4 )
sampling = lexical_cast<int>( argv[3] );
if( argc >= 5 )
cell_size = lexical_cast<float>( argv[4] );
Environment env;
Pointcloud::Ptr pc = new Pointcloud();
env.attachItem( pc.get() );
env.setFrameNode( pc.get(), env.getRootNode() );
ifstream asc( asc_file.c_str() );
pc->readText( asc, sampling, Pointcloud::XYZR );
if( cell_size > 0 )
{
#ifdef ENVIRE_USE_CGAL
Pointcloud::Ptr pc_simp = new Pointcloud();
env.attachItem( pc_simp.get() );
env.setFrameNode( pc_simp.get(), env.getRootNode() );
SimplifyPointcloud::Ptr spc = new SimplifyPointcloud();
env.attachItem( spc.get() );
spc->addInput( pc.get() );
spc->addOutput( pc_simp.get() );
spc->setSimplifyCellSize( cell_size );
spc->updateAll();
pc = pc_simp;
#else
std::cout << "ignoring cell_size parameter since CGAL is not compiled in." << std::endl;
#endif
}
ofstream ply( ply_file.c_str() );
pc->writePly( ply_file, ply );
}
void WorldDownloadManager::mergePointCloudsAndMesh(std::vector<PointCloud::Ptr> &pointclouds,
PointCloud::Ptr out_cloud, std::vector<TrianglesPtr> * meshes,Triangles * out_mesh)
{
uint offset = 0;
const uint pointcloud_count = pointclouds.size();
out_cloud->clear();
if (out_mesh)
out_mesh->clear();
for (uint pointcloud_i = 0; pointcloud_i < pointcloud_count; pointcloud_i++)
{
const uint pointcloud_size = pointclouds[pointcloud_i]->size();
// copy the points
(*out_cloud) += *(pointclouds[pointcloud_i]);
if (out_mesh)
{
// copy the triangles, shifting vertex id by an offset
const uint mesh_size = (*meshes)[pointcloud_i]->size();
out_mesh->reserve(out_mesh->size() + mesh_size);
for (uint triangle_i = 0; triangle_i < mesh_size; triangle_i++)
{
kinfu_msgs::KinfuMeshTriangle tri;
const kinfu_msgs::KinfuMeshTriangle & v = (*(*meshes)[pointcloud_i])[triangle_i];
for (uint i = 0; i < 3; i++)
tri.vertex_id[i] = v.vertex_id[i] + offset;
out_mesh->push_back(tri);
}
offset += pointcloud_size;
}
}
}
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min,
const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud,
TrianglesConstPtr triangles,PointCloud::Ptr out_cloud,TrianglesPtr out_triangles)
{
const uint triangles_size = triangles->size();
const uint cloud_size = cloud->size();
std::vector<bool> valid_points(cloud_size,true);
std::vector<uint> valid_points_remap(cloud_size,0);
std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n";
uint offset;
// check the points
for (uint i = 0; i < cloud_size; i++)
{
const pcl::PointXYZ & pt = (*cloud)[i];
if (pt.x > max.x || pt.y > max.y || pt.z > max.z ||
pt.x < min.x || pt.y < min.y || pt.z < min.z)
valid_points[i] = false;
}
// discard invalid points
out_cloud->clear();
out_cloud->reserve(cloud_size);
offset = 0;
for (uint i = 0; i < cloud_size; i++)
if (valid_points[i])
{
out_cloud->push_back((*cloud)[i]);
// save new position for triangles remap
valid_points_remap[i] = offset;
offset++;
}
out_cloud->resize(offset);
// discard invalid triangles
out_triangles->clear();
out_triangles->reserve(triangles_size);
offset = 0;
for (uint i = 0; i < triangles_size; i++)
{
const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i];
bool is_valid = true;
// validate all the vertices
for (uint h = 0; h < 3; h++)
if (!valid_points[tri.vertex_id[h]])
{
is_valid = false;
break;
}
if (is_valid)
{
kinfu_msgs::KinfuMeshTriangle out_tri;
// remap the triangle
for (uint h = 0; h < 3; h++)
out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]];
out_triangles->push_back(out_tri);
offset++;
}
}
out_triangles->resize(offset);
std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n";
}
//--------------------------------------------------------------------------------------------------
int main( int argc, char** argv )
{
// Read in a configuration file
if ( argc < 2 )
{
fprintf( stderr, "No configuration file provided\n" );
showUsage( argv[ 0 ] );
return -1;
}
initialiseTestPositions();
//std::string configFilename = Utilities::getDataDir() + std::string( "/" ) + std::string( argv[ 1 ] );
std::string configFilename( argv[ 1 ] );
cv::FileStorage configFileStorage;
configFileStorage.open( configFilename, cv::FileStorage::READ );
if ( !configFileStorage.isOpened() )
{
fprintf( stderr, "Unable to open %s\n", configFilename.c_str() );
return -1;
}
cv::Mat kinectDepthCameraPos;
cv::Mat kinectDepthCameraRotXYZDeg;
double kinectDepthFocalLengthPixel;
int32_t kinectDepthImageWidth;
int32_t kinectDepthImageHeight;
cv::Mat kinectRGBCameraPos;
cv::Mat kinectRGBCameraRotXYZDeg;
double kinectRGBFocalLengthPixel;
int32_t kinectRGBImageWidth;
int32_t kinectRGBImageHeight;
cv::Mat highResCameraPos;
cv::Mat highResCameraRotXYZDeg;
double highResFocalLengthPixel;
int32_t highResImageWidth;
int32_t highResImageHeight;
// The camera poses are defined in a coordinate system with the Kinect depth camera at the
// origin looking down the +ve z-axis, so z values increase into the image. The x and y axes
// of the depth camera are aligned with its image plane, so +ve x points to its right, and
// +ve y points down through the base of the camera. To position the camera in world space, it
// is rotated 180 degrees about the z axis, the chessboard positions are defined in world space.
configFileStorage[ "kinectDepthFocalLengthPixel" ] >> kinectDepthFocalLengthPixel;
configFileStorage[ "kinectDepthImageWidth" ] >> kinectDepthImageWidth;
configFileStorage[ "kinectDepthImageHeight" ] >> kinectDepthImageHeight;
configFileStorage[ "kinectRGBCameraPos" ] >> kinectRGBCameraPos;
configFileStorage[ "kinectRGBCameraRotXYZDeg" ] >> kinectRGBCameraRotXYZDeg;
configFileStorage[ "kinectRGBFocalLengthPixel" ] >> kinectRGBFocalLengthPixel;
configFileStorage[ "kinectRGBImageWidth" ] >> kinectRGBImageWidth;
configFileStorage[ "kinectRGBImageHeight" ] >> kinectRGBImageHeight;
configFileStorage[ "highResCameraPos" ] >> highResCameraPos;
configFileStorage[ "highResCameraRotXYZDeg" ] >> highResCameraRotXYZDeg;
configFileStorage[ "highResFocalLengthPixel" ] >> highResFocalLengthPixel;
configFileStorage[ "highResImageWidth" ] >> highResImageWidth;
configFileStorage[ "highResImageHeight" ] >> highResImageHeight;
// Construct camera matrices
cv::Mat zeroVec = cv::Mat::zeros( 3, 1, CV_64FC1 );
cv::Mat kinectDepthCalibMtx = createCameraCalibrationMatrix(
kinectDepthFocalLengthPixel, kinectDepthImageWidth, kinectDepthImageHeight );
cv::Mat kinectDepthWorldMtx = createCameraWorldMatrix( zeroVec, zeroVec );
cv::Mat kinectRGBCalibMtx = createCameraCalibrationMatrix(
kinectRGBFocalLengthPixel, kinectRGBImageWidth, kinectRGBImageHeight );
cv::Mat kinectRGBWorldMtx = createCameraWorldMatrix(
kinectRGBCameraPos, kinectRGBCameraRotXYZDeg );
cv::Mat highResCalibMtx = createCameraCalibrationMatrix(
highResFocalLengthPixel, highResImageWidth, highResImageHeight );
cv::Mat highResWorldMtx = createCameraWorldMatrix(
highResCameraPos, highResCameraRotXYZDeg );
// First generate calibration images for the high resolution camera
for ( uint32_t testPosIdx = 0; testPosIdx < gHighResCalibrationPositions.size(); testPosIdx++ )
{
cv::Mat chessboardPoseMtx = createChessboardPoseMatrix( gHighResCalibrationPositions[ testPosIdx ] );
// Generate an image from the high resolution camera
cv::Mat highResRgbImage = generateRGBImageOfChessboard( highResWorldMtx, highResCalibMtx,
highResImageWidth, highResImageHeight, chessboardPoseMtx );
cv::imwrite( createOutputFilename( "chessboard_highres", testPosIdx + 1, ".png" ), highResRgbImage );
}
// Now generate images to identify the relative poses of the cameras
for ( uint32_t testPosIdx = 0; testPosIdx < gRelativeCalibrationPositions.size(); testPosIdx++ )
{
cv::Mat chessboardPoseMtx = createChessboardPoseMatrix( gRelativeCalibrationPositions[ testPosIdx ] );
// Generate a point cloud from the Kinect
PointCloud::Ptr pCloud = generatePointCloudOfChessboard( kinectDepthWorldMtx, kinectDepthCalibMtx,
kinectDepthImageWidth, kinectDepthImageHeight, chessboardPoseMtx );
pCloud->saveToSpcFile( createOutputFilename( "relative_chessboard", testPosIdx + 1, ".spc" ), true );
// Generate an image from the Kinect RGB camera
cv::Mat rgbImage = generateRGBImageOfChessboard( kinectRGBWorldMtx, kinectRGBCalibMtx,
kinectRGBImageWidth, kinectRGBImageHeight, chessboardPoseMtx );
cv::imwrite( createOutputFilename( "relative_chessboard", testPosIdx + 1, ".png" ), rgbImage );
// Generate an image from the high resolution camera
cv::Mat highResRgbImage = generateRGBImageOfChessboard( highResWorldMtx, highResCalibMtx,
highResImageWidth, highResImageHeight, chessboardPoseMtx );
cv::imwrite( createOutputFilename( "relative_chessboard_highres", testPosIdx + 1, ".png" ), highResRgbImage );
}
// Generate ideal calibration files
// First the kinect calibration
cv::FileStorage dataFile( "kinect_calib.yaml", cv::FileStorage::WRITE );
dataFile << "DepthCameraCalibrationMtx" << kinectDepthCalibMtx;
dataFile << "ColorCameraCalibrationMtx" << kinectRGBCalibMtx;
cv::Mat kinectRGBInKinectDepthSpace = (kinectDepthWorldMtx.inv())*kinectRGBWorldMtx;
dataFile << "DepthToColorCameraRotation" << cv::Mat( kinectRGBInKinectDepthSpace, cv::Rect( 0, 0, 3, 3 ) );
dataFile << "DepthToColorCameraTranslation" << cv::Mat( kinectRGBInKinectDepthSpace, cv::Rect( 3, 0, 1, 3 ) );
dataFile.release();
// Now the high resolution camera calibration
dataFile = cv::FileStorage( "high_res_calib.yaml", cv::FileStorage::WRITE );
dataFile << "cameraMatrix" << highResCalibMtx;
dataFile.release();
// Finally, the position of the high resolution colour camera in Kinect colour camera space
dataFile = cv::FileStorage( "colour_stereo_calib.yaml", cv::FileStorage::WRITE );
cv::Mat highResInKinectRGBSpace = (kinectRGBWorldMtx.inv())*highResWorldMtx;
dataFile << "R" << cv::Mat( highResInKinectRGBSpace, cv::Rect( 0, 0, 3, 3 ) );
dataFile << "T" << cv::Mat( highResInKinectRGBSpace, cv::Rect( 3, 0, 1, 3 ) );
dataFile.release();
//std::cout << kinectRGBWorldMtx << std::endl;
//std::cout << highResWorldMtx << std::endl;
//std::cout << highResInKinectRGBSpace << std::endl;
return 0;
}
void RegionGrowing::fastSeedRegionGrowing(
pcl::PointCloud<PointNormalT>::Ptr src_points,
cv::Point2i &seed_index2D, const PointCloud::Ptr cloud,
const PointNormal::Ptr normals, const PointT seed_pt) {
if (cloud->empty() || normals->size() != cloud->size()) {
return;
}
cv::Point2f image_index;
int seed_index = -1;
if (this->projectPoint3DTo2DIndex(image_index, seed_pt)) {
seed_index = (static_cast<int>(image_index.x) +
(static_cast<int>(image_index.y) * camera_info_->width));
seed_index2D = cv::Point2i(static_cast<int>(image_index.x),
static_cast<int>(image_index.y));
} else {
ROS_ERROR("INDEX IS NAN");
return;
}
#ifdef _DEBUG
cv::Mat test = cv::Mat::zeros(480, 640, CV_8UC3);
cv::circle(test, image_index, 3, cv::Scalar(0, 255, 0), -1);
cv::imshow("test", test);
cv::waitKey(3);
#endif
Eigen::Vector4f seed_point = cloud->points[seed_index].getVector4fMap();
Eigen::Vector4f seed_normal = normals->points[
seed_index].getNormalVector4fMap();
std::vector<int> processing_list;
std::vector<int> labels(static_cast<int>(cloud->size()), -1);
const int window_size = 3;
const int wsize = window_size * window_size;
const int lenght = std::floor(window_size/2);
processing_list.clear();
for (int j = -lenght; j <= lenght; j++) {
for (int i = -lenght; i <= lenght; i++) {
int index = (seed_index + (j * camera_info_->width)) + i;
if (index >= 0 && index < cloud->size()) {
processing_list.push_back(index);
}
}
}
std::vector<int> temp_list;
while (true) {
if (processing_list.empty()) {
break;
}
temp_list.clear();
for (int i = 0; i < processing_list.size(); i++) {
int idx = processing_list[i];
if (labels[idx] == -1) {
Eigen::Vector4f c = cloud->points[idx].getVector4fMap();
Eigen::Vector4f n = normals->points[idx].getNormalVector4fMap();
if (this->seedVoxelConvexityCriteria(
seed_point, seed_normal, seed_point, c, n, -0.01) == 1) {
labels[idx] = 1;
for (int j = -lenght; j <= lenght; j++) {
for (int k = -lenght; k <= lenght; k++) {
int index = (idx + (j * camera_info_->width)) + k;
if (index >= 0 && index < cloud->size()) {
temp_list.push_back(index);
}
}
}
}
}
}
processing_list.clear();
processing_list.insert(processing_list.end(), temp_list.begin(),
temp_list.end());
}
src_points->clear();
for (int i = 0; i < labels.size(); i++) {
if (labels[i] != -1) {
PointNormalT pt;
pt.x = cloud->points[i].x;
pt.y = cloud->points[i].y;
pt.z = cloud->points[i].z;
pt.r = cloud->points[i].r;
pt.g = cloud->points[i].g;
pt.b = cloud->points[i].b;
pt.normal_x = normals->points[i].normal_x;
pt.normal_y = normals->points[i].normal_y;
pt.normal_z = normals->points[i].normal_z;
src_points->push_back(pt);
}
}
}
bool send2(brio_assembly_vision_new::TrasformStampedRequest &req, brio_assembly_vision_new::TrasformStampedResponse &res)
{
request_a_new_cluster =true;
if(find_cloud_from_kinect_head==true && final ==false)
{
brio_assembly_vision_new::Container * cont = new brio_assembly_vision_new::Container();
while((cont=client_call())==NULL)
{
std::this_thread::sleep_for (std::chrono::seconds(1)); //wait for a good response
}
if(first_time==true)
{
initial_cluster_size=cont->date_container.size();
}
first_time=false;
if(cont->date_container.size()>0) //after moving objects size is decreased
{
int i=0; //start with the lowest index
std::string object_with_shape_requested = model[model_step];
while(cont->date_container[i].piece_type!=object_with_shape_requested && i<cont->date_container.size()-1) //
{
i++;
}
//end while when cluster_vector[i] has the requested shape or when the search index is bigger then cluster_vector
if(i<=cont->date_container.size()-1) // if i<=cluster_vector.size() then we did find requested object at indices i
{
Eigen::Vector3d z_axe,x_or_y_axe;
Eigen::Vector4d translation;
z_axe=estimate_plane_normals(cloud);
int i_center = cont->date_container[i].center_index;
int i_head = cont->date_container[i].head_conn_index;
x_or_y_axe= obj_axe_2_points_next(i_head,i_center);
pcl::PointXYZRGB center = cloud->at(cont->date_container[i].center_index);
translation[0] = center.x;
translation[1] = center.y;
translation[2] = center.z;
calculate_transformation(x_or_y_axe,z_axe ,translation);
ROS_INFO("Send TransformedPose");
Eigen::Quaternionf quat;
quat = createQuaternion();
res.msg.child_frame_id = "brio_piece_frame";
res.msg.header.frame_id = "head_mount_kinect_rgb_optical_frame";
res.msg.transform.translation.x = transformata_finala(0,3);
res.msg.transform.translation.y = transformata_finala(1,3);
res.msg.transform.translation.z = transformata_finala(2,3);
res.msg.transform.rotation.w = (double)quat.w();
res.msg.transform.rotation.x = (double)quat.x();
res.msg.transform.rotation.y = (double)quat.y();
res.msg.transform.rotation.z = (double)quat.z();
if(model_step<initial_cluster_size-1)
model_step++;
else
{ final=true;
//return false;
}
return true;
}
int main( int argc, char** argv )
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("./data/pose.txt");
if (!fin)
{
cerr<<"cannot find pose file"<<endl;
return 1;
}
for ( int i=0; i<5; i++ )
{
boost::format fmt( "./data/%s/%d.%s" ); //图像文件格式
colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像
double data[7] = {0};
for ( int i=0; i<7; i++ )
{
fin>>data[i];
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
Eigen::Isometry3d T(q);
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout<<"正在将图像转换为点云..."<<endl;
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云
PointCloud::Ptr pointCloud( new PointCloud );
for ( int i=0; i<5; i++ )
{
PointCloud::Ptr current( new PointCloud );
cout<<"转换图像中: "<<i+1<<endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
for ( int v=0; v<color.rows; v++ )
for ( int u=0; u<color.cols; u++ )
{
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
if ( d==0 ) continue; // 为0表示没有测量到
if ( d >= 7000 ) continue; // 深度太大时不稳定，去掉
Eigen::Vector3d point;
point[2] = double(d)/depthScale;
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy;
Eigen::Vector3d pointWorld = T*point;
PointT p ;
p.x = pointWorld[0];
p.y = pointWorld[1];
p.z = pointWorld[2];
p.b = color.data[ v*color.step+u*color.channels() ];
p.g = color.data[ v*color.step+u*color.channels()+1 ];
p.r = color.data[ v*color.step+u*color.channels()+2 ];
current->points.push_back( p );
}
// depth filter and statistical removal
PointCloud::Ptr tmp ( new PointCloud );
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThresh(1.0);
statistical_filter.setInputCloud(current);
statistical_filter.filter( *tmp );
(*pointCloud) += *tmp;
}
pointCloud->is_dense = false;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
// voxel filter
pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution
PointCloud::Ptr tmp ( new PointCloud );
voxel_filter.setInputCloud( pointCloud );
voxel_filter.filter( *tmp );
tmp->swap( *pointCloud );
cout<<"滤波之后，点云共有"<<pointCloud->size()<<"个点."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
return 0;
}
std::vector<GraspHypothesis> Localization::localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering)
{
double t0 = omp_get_wtime();
std::vector<GraspHypothesis> hand_list;
if (size_left == 0 || cloud_in->size() == 0)
{
//std::cout << "Input cloud is empty!\n";
//std::cout << size_left << std::endl;
hand_list.resize(0);
return hand_list;
}
// set camera source for all points (0 = left, 1 = right)
//std::cout << "Generating camera sources for " << cloud_in->size() << " points ...\n";
Eigen::VectorXi pts_cam_source(cloud_in->size());
if (size_left == cloud_in->size())
pts_cam_source << Eigen::VectorXi::Zero(size_left);
else
pts_cam_source << Eigen::VectorXi::Zero(size_left), Eigen::VectorXi::Ones(cloud_in->size() - size_left);
// remove NAN points from the cloud
std::vector<int> nan_indices;
pcl::removeNaNFromPointCloud(*cloud_in, *cloud_in, nan_indices);
// reduce point cloud to workspace
//std::cout << "Filtering workspace ...\n";
PointCloud::Ptr cloud(new PointCloud);
filterWorkspace(cloud_in, pts_cam_source, cloud, pts_cam_source);
//std::cout << " " << cloud->size() << " points left\n";
// store complete cloud for later plotting
PointCloud::Ptr cloud_plot(new PointCloud);
*cloud_plot = *cloud;
*cloud_ = *cloud;
// voxelize point cloud
//std::cout << "Voxelizing point cloud\n";
double t1_voxels = omp_get_wtime();
voxelizeCloud(cloud, pts_cam_source, cloud, pts_cam_source, 0.003);
double t2_voxels = omp_get_wtime() - t1_voxels;
//std::cout << " Created " << cloud->points.size() << " voxels in " << t2_voxels << " sec\n";
// plot camera source for each point in the cloud
if (plots_camera_sources_)
plot_.plotCameraSource(pts_cam_source, cloud);
if (uses_clustering)
{
//std::cout << "Finding point cloud clusters ... \n";
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.01);
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
//std::cout << " Could not estimate a planar model for the given dataset." << std::endl;
hand_list.resize(0);
return hand_list;
}
//std::cout << " PointCloud representing the planar component: " << inliers->indices.size()
// << " data points." << std::endl;
// Extract the nonplanar inliers
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
std::vector<int> indices_cluster;
extract.filter(indices_cluster);
PointCloud::Ptr cloud_cluster(new PointCloud);
cloud_cluster->points.resize(indices_cluster.size());
Eigen::VectorXi cluster_cam_source(indices_cluster.size());
for (int i = 0; i < indices_cluster.size(); i++)
{
cloud_cluster->points[i] = cloud->points[indices_cluster[i]];
cluster_cam_source[i] = pts_cam_source[indices_cluster[i]];
}
cloud = cloud_cluster;
*cloud_plot = *cloud;
//std::cout << " PointCloud representing the non-planar component: " << cloud->points.size()
// << " data points." << std::endl;
}
// draw down-sampled and workspace reduced cloud
cloud_plot = cloud;
// set plotting within handle search on/off
bool plots_hands;
if (plotting_mode_ == PCL_PLOTTING)
plots_hands = true;
else
plots_hands = false;
// find hand configurations
HandSearch hand_search(finger_width_, hand_outer_diameter_, hand_depth_, hand_height_, init_bite_, num_threads_,
num_samples_, plots_hands);
hand_list = hand_search.findHands(cloud, pts_cam_source, indices, cloud_plot, calculates_antipodal, uses_clustering);
// remove hands at boundaries of workspace
if (filters_boundaries_)
{
//std::cout << "Filtering out hands close to workspace boundaries ...\n";
hand_list = filterHands(hand_list);
//std::cout << " # hands left: " << hand_list.size() << "\n";
}
double t2 = omp_get_wtime();
//std::cout << "Hand localization done in " << t2 - t0 << " sec\n";
if (plotting_mode_ == PCL_PLOTTING)
//{
plot_.plotHands(hand_list, cloud_plot, "");
//}
/*
else if (plotting_mode_ == RVIZ_PLOTTING)
{
plot_.plotGraspsRviz(hand_list, visuals_frame_);
}
*/
return hand_list;
}
void GraphicEnd::downsamplePointCloud(PointCloud::Ptr& pc_in,PointCloud::Ptr& pc_downsampled)
{
if(use_voxel)
{
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize(0.05,0.05,0.05);
grid.setFilterFieldName ("z");
grid.setFilterLimits (0.0,5.0);
grid.setInputCloud(pc_in);
grid.filter(*pc_downsampled);
}
else
{
int downsamplingStep=8;
static int j;j=0;
std::vector<double> xV;
std::vector<double> yV;
std::vector<double> zV;
std::vector<double> rV;
std::vector<double> gV;
std::vector<double> bV;
pc_downsampled.reset(new pcl::PointCloud<pcl::PointXYZRGB> );
pc_downsampled->points.resize(640*480/downsamplingStep*downsamplingStep);
for(int r=0;r<480;r=r+downsamplingStep)
{
for(int c=0;c<640;c=c+downsamplingStep)
{
int nPoints=0;
xV.resize(downsamplingStep*downsamplingStep);
yV.resize(downsamplingStep*downsamplingStep);
zV.resize(downsamplingStep*downsamplingStep);
rV.resize(downsamplingStep*downsamplingStep);
gV.resize(downsamplingStep*downsamplingStep);
bV.resize(downsamplingStep*downsamplingStep);
for(int r2=r;r2<r+downsamplingStep;r2++)
{
for(int c2=c;c2<c+downsamplingStep;c2++)
{
//Check if the point has valid data
if(pcl_isfinite (pc_in->points[r2*640+c2].x) &&
pcl_isfinite (pc_in->points[r2*640+c2].y) &&
pcl_isfinite (pc_in->points[r2*640+c2].z) &&
0.3<pc_in->points[r2*640+c2].z &&
pc_in->points[r2*640+c2].z<5)
{
//Create a vector with the x, y and z coordinates of the square region and RGB info
xV[nPoints]=pc_in->points[r2*640+c2].x;
yV[nPoints]=pc_in->points[r2*640+c2].y;
zV[nPoints]=pc_in->points[r2*640+c2].z;
rV[nPoints]=pc_in->points[r2*640+c2].r;
gV[nPoints]=pc_in->points[r2*640+c2].g;
bV[nPoints]=pc_in->points[r2*640+c2].b;
nPoints++;
}
}
}
if(nPoints>0)
{
xV.resize(nPoints);
yV.resize(nPoints);
zV.resize(nPoints);
rV.resize(nPoints);
gV.resize(nPoints);
bV.resize(nPoints);
//Compute the mean 3D point and mean RGB value
std::sort(xV.begin(),xV.end());
std::sort(yV.begin(),yV.end());
std::sort(zV.begin(),zV.end());
std::sort(rV.begin(),rV.end());
std::sort(gV.begin(),gV.end());
std::sort(bV.begin(),bV.end());
pcl::PointXYZRGB point;
point.x=xV[nPoints/2];
point.y=yV[nPoints/2];
point.z=zV[nPoints/2];
point.r=rV[nPoints/2];
point.g=gV[nPoints/2];
point.b=bV[nPoints/2];
//Set the mean point as the representative point of the region
pc_downsampled->points[j]=point;
j++;
}
}
}
pc_downsampled->points.resize(j);
pc_downsampled->width=pc_downsampled->size();
pc_downsampled->height=1;
}
}
Eigen::Matrix4d calculate_transformation(Eigen::Vector3d x_or_y_axe_vector, Eigen::Vector3d z_axe_vector, Eigen::Vector4d xyz_centroid)
{
transformata_finala=Eigen::MatrixXd::Identity(4,4);
Eigen::Vector3d axa3;
axa3(0)=x_or_y_axe_vector(1)*z_axe_vector(2)-z_axe_vector(1)*x_or_y_axe_vector(2);// a_nou = b1*c2 - b2*c1;
axa3(1)=z_axe_vector(0)*x_or_y_axe_vector(2)-x_or_y_axe_vector(0)*z_axe_vector(2);// b_nou = a2*c1 - a1*c2;
axa3(2)=x_or_y_axe_vector(0)*z_axe_vector(1)-x_or_y_axe_vector(1)*z_axe_vector(0);// c_nou = a1*b2 - b1*a2;
for(int i=0;i<3;i++)
{
transformata_finala(i,0) =z_axe_vector(i);
transformata_finala(i,1) =axa3(i);
transformata_finala(i,2) =x_or_y_axe_vector(i);
}
for(int i=0;i<3;i++)
transformata_finala(i,3)=xyz_centroid(i);
std::cout<<std::endl<<"Transformation Matrix:"<<std::endl<<transformata_finala<<std::endl;
float x0,y0,z0;
x0=xyz_centroid[0];
y0=xyz_centroid[1];
z0=xyz_centroid[2];
float l = 0.005;
pcl::PointXYZRGB p_x,p_y,p_z;
p_x.r=255;
p_x.g=0;
p_x.b=0;
p_y.r=0;
p_y.g=255;
p_y.b=0;
p_z.r=0;
p_z.g=0;
p_z.b=255;
PointCloud::Ptr axe (new PointCloud);
for(int i=0;i<100;i++)
{
//axa x red
p_x.x =x0 + transformata_finala(0,0)*l;
p_x.y =y0 + transformata_finala(1,0)*l;
p_x.z =z0 + transformata_finala(2,0)*l;
//axa y green
p_y.x =x0 + transformata_finala(0,1)*l;
p_y.y =y0 + transformata_finala(1,1)*l;
p_y.z =z0 + transformata_finala(1,1)*l;
//axa z blue
p_z.x =x0 + transformata_finala(0,2)*l;
p_z.y =y0 + transformata_finala(1,2)*l;
p_z.z =z0 + transformata_finala(2,2)*l;
axe->push_back(p_x);
axe->push_back(p_y);
axe->push_back(p_z);
l+=0.005;
}
pcl::visualization::PCLVisualizer viewer ("ICP demo");
viewer.addPointCloud (cloud, "plane",0);
viewer.addPointCloud (axe, "a",0);
while (!viewer.wasStopped ()) {
viewer.spinOnce ();
}
}
void App::VisOriginal(TUMDataSetVisualizer * viewer) {
viewer->EasyAdd(Input_, "input", [this] (int i) {
return Color(Input_->at(i));
});
}
void App::Load() {
Input_.reset(new PointCloud);
pcl::io::loadPCDFile(InputPath_, *Input_);
}
std::vector<PointCloud> TableClusterDetector::findTableClusters(const sensor_msgs::PointCloud2 &scene)
{
std::vector<PointCloud> clusters;
// Convert the dataset
PointCloud cloud; PointCloud::Ptr cloudPtr;
pcl::fromROSMsg (scene, cloud);
cloudPtr.reset(new PointCloud(cloud));
// Remove NaNs
PointCloud cloud_filtered;
pass_.setInputCloud (cloudPtr);
pass_.filter (cloud_filtered);
cloudPtr.reset(new PointCloud(cloud_filtered));
// Downsample
PointCloud cloud_downsampled;
grid_.setInputCloud (cloudPtr);
grid_.filter (cloud_downsampled);
cloudPtr.reset(new PointCloud(cloud_downsampled));
if ((int)cloud_filtered.points.size() < k_)
{
ROS_WARN("Filtering returned %zd points! Skipping.", cloud_filtered.points.size());
return clusters;
}
// Estimate the point normals
pcl::PointCloud<pcl::Normal> cloud_normals;pcl::PointCloud<pcl::Normal>::Ptr cloud_normalsPtr;
// add this if normal estimation is inaccurate
//n3d_.setSearchSurface (cloud_);
n3d_.setInputCloud (cloudPtr);
n3d_.compute (cloud_normals);
cloud_normalsPtr.reset(new pcl::PointCloud<pcl::Normal>(cloud_normals));
ROS_INFO ("[TableObjectDetector] %d normals estimated.", (int)cloud_normals.points.size ());
// ---[ Perform segmentation
pcl::PointIndices table_inliers; pcl::PointIndices::Ptr table_inliersPtr;
pcl::ModelCoefficients table_coefficients; pcl::ModelCoefficients::Ptr table_coefficientsPtr;
seg_.setInputCloud (cloudPtr);
seg_.setInputNormals (cloud_normalsPtr);
seg_.segment (table_inliers, table_coefficients);
table_inliersPtr = boost::make_shared<pcl::PointIndices>(table_inliers);
table_coefficientsPtr = boost::make_shared<pcl::ModelCoefficients>(table_coefficients);
if (table_coefficients.values.size () > 3)
ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers.indices.size (),
table_coefficients.values[0], table_coefficients.values[1], table_coefficients.values[2], table_coefficients.values[3]);
if (table_inliers.indices.size () == 0)
return clusters;
// ---[ Extract the table
PointCloud table_projected; PointCloud::Ptr table_projectedPtr;
proj_.setInputCloud (cloudPtr);
proj_.setIndices (table_inliersPtr);
proj_.setModelCoefficients (table_coefficientsPtr);
proj_.filter (table_projected);
table_projectedPtr.reset (new PointCloud(table_projected));
ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ());
sensor_msgs::PointCloud table_points;
tf::Transform table_plane_trans = getPlaneTransform (*table_coefficientsPtr, -1.0);
std::string base_frame_id = scene.header.frame_id;
ROS_INFO("sending table transform");
ros::Rate r(10);
for (int i=0; i < 10; i++) {
tf_pub_.sendTransform(tf::StampedTransform(table_plane_trans, ros::Time::now(),
base_frame_id, "table"));
r.sleep();
}
//takes the points projected on the table and transforms them into the PointCloud message
//while also transforming them into the table's coordinate system
getPlanePoints<Point> (table_projected, table_plane_trans, table_points);
ROS_INFO("Table computed");
table_ = computeTable<sensor_msgs::PointCloud>(cloud.header, table_plane_trans, table_points);
publishTable(table_);
// ---[ Estimate the convex hull
PointCloud table_hull; PointCloud::Ptr table_hullPtr;
hull_.setInputCloud (table_projectedPtr);
hull_.reconstruct (table_hull);
table_hullPtr.reset (new PointCloud(table_hull));
// ---[ Get the objects on top of the table
pcl::PointIndices cloud_object_indices; pcl::PointIndices::Ptr cloud_object_indicesPtr;
prism_.setInputCloud (cloudPtr);
prism_.setInputPlanarHull (table_hullPtr);
prism_.segment (cloud_object_indices);
cloud_object_indicesPtr = boost::make_shared<pcl::PointIndices>(cloud_object_indices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ());
PointCloud cloud_objects; PointCloud::Ptr cloud_objectsPtr;
pcl::ExtractIndices<Point> extract_object_indices;
extract_object_indices.setInputCloud (cloudPtr);
extract_object_indices.setIndices (cloud_object_indicesPtr);
extract_object_indices.filter (cloud_objects);
cloudPtr.reset(new PointCloud(cloud_objects));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates: %d.", (int)cloud_objects.points.size ());
if (cloud_objects.points.size () == 0)
return clusters;
// ---[ Downsample the points
PointCloud cloud_objects_downsampled;PointCloud::Ptr cloud_objects_downsampledPtr;
grid_objects_.setInputCloud (cloudPtr);
grid_objects_.filter (cloud_objects_downsampled);
cloudPtr.reset (new PointCloud(cloud_objects_downsampled));
ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates left after downsampling: %d.", (int)cloud_objects_downsampled.points.size ());
// ---[ Split the objects into Euclidean clusters
std::vector<pcl::PointIndices> clustersIndices;
cluster_.setInputCloud (cloudPtr);
cluster_.extract (clustersIndices);
ROS_INFO ("[TableObjectDetector::input_callback] Number of clusters found matching the given constraints: %d.", (int)clustersIndices.size ());
AffineTransform table_trans_eig;
cse481::tfToEigen(table_plane_trans, table_trans_eig);
table_trans_eig = table_trans_eig.inverse().eval();
PointCloud cloud_in_table_frame;
pcl::transformPointCloud(cloud_objects_downsampled, cloud_in_table_frame, table_trans_eig);
// Clouds are now in table frame
BOOST_FOREACH(pcl::PointIndices indices, clustersIndices) {
PointCloud clusterCloud;
pcl::copyPointCloud(cloud_in_table_frame, indices, clusterCloud);
clusters.push_back(clusterCloud);
}