void PCPolyhedron::detectPrimitives() {
pcPolygonsMutex.lock();
saveCloud("detectPrimitives.pcd", *cloud);
vector<PCPolygonPtr> nuevos = detectPolygons(cloud,Constants::OBJECT_PLANE_TOLERANCE(),Constants::CLOUD_VOXEL_SIZE * 2);
nuevos = discardPolygonsOutOfBox(nuevos);
namePolygons(nuevos);
bool estimationOk;
vector<PCPolygonPtr> estimated = estimateHiddenPolygons(nuevos,estimationOk);
if(estimationOk)
{
pcpolygons = nuevos;
pcpolygons.insert(pcpolygons.end(),estimated.begin(),estimated.end());
unifyVertexs();
polygonsCache.clear();
for (vector<PCPolygonPtr>::iterator p = pcpolygons.begin(); p != pcpolygons.end(); ++p)
{
polygonsCache.push_back((*p)->getPolygonModelObject());
}
for(int i = 0; i < pcpolygons.size(); i ++)
{
saveCloud("pol" + ofToString(pcpolygons.at(i)->getPolygonModelObject()->getName()) + ".pcd", *pcpolygons.at(i)->getCloud());
}
}
else
cout << "estimation failed in detect primitives" << endl;
pcPolygonsMutex.unlock();
}
int
batchProcess (const std::vector<std::string> &pcd_files, std::string &output_dir,
float radius, bool inside, bool keep_organized)
{
std::vector<std::string> st;
for (size_t i = 0; i < pcd_files.size (); ++i)
{
// Load the first file
Cloud::Ptr cloud (new Cloud);
if (!loadCloud (pcd_files[i], cloud))
return (-1);
// Perform the feature estimation
Cloud::Ptr output (new Cloud);
compute (cloud, output, radius, inside, keep_organized);
// Prepare output file name
std::string filename = pcd_files[i];
boost::trim (filename);
boost::split (st, filename, boost::is_any_of ("/\\"), boost::token_compress_on);
// Save into the second file
std::stringstream ss;
ss << output_dir << "/" << st.at (st.size () - 1);
saveCloud (ss.str (), output);
}
return (0);
}
float computeVolume(const PCPtr& cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudHull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setInputCloud (cloud);
chull.setDimension(3);
chull.setComputeAreaVolume(true);
chull.reconstruct (*cloudHull);
//SOLO PARA FOTOS
saveCloud("cloudParaHull.pcd",*cloud);
saveCloud("Hull.pcd",*cloudHull);
////
return chull.getTotalVolume();
}
void saveCloud(const string& filename, const vector<ofVec3f>& v)
{
if (mapinect::IsFeatureSaveCloudActive())
{
PCPtr cloud(ofVecVectorToPointCloud(v));
saveCloud(filename, *cloud);
}
}
void PCPolyhedron::unifyVertexs() {
vertexs.clear();
struct VertexInPCPolygon
{
VertexInPCPolygon(PCPolygon* pcp, int vertex) : pcp(pcp), vertex(vertex) { }
PCPolygon* pcp;
int vertex;
};
vector<VertexInPCPolygon> updateVertexs;
for (vector<PCPolygonPtr>::iterator nextIter = pcpolygons.begin(); nextIter != pcpolygons.end();) {
vector<PCPolygonPtr>::iterator iter = nextIter++;
Polygon* polygon = (*iter)->getPolygonModelObject();
if (polygon == NULL) {
// No model object is available yet, quit!
return;
}
for (int j = 0; j < polygon->getMathModel().getVertexs().size(); j++) {
updateVertexs.clear();
VertexInPCPolygon vpp(iter->get(), j);
updateVertexs.push_back(vpp);
ofVec3f v(polygon->getMathModel().getVertexs()[j]);
for (vector<PCPolygonPtr>::iterator iter2 = iter; iter2 != pcpolygons.end(); iter2++) {
Polygon* polygon2 = (*iter2)->getPolygonModelObject();
for (int k = 0; k < polygon2->getMathModel().getVertexs().size(); k++) {
ofVec3f v2(polygon2->getMathModel().getVertexs()[k]);
if (!(v == v2)
&& polygon->getMathModel().getVertexs()[j].distance(polygon2->getMathModel().getVertexs()[k]) <= Constants::OBJECT_VERTEX_UNIFYING_DISTANCE()) {
VertexInPCPolygon vpp2(iter2->get(), k);
updateVertexs.push_back(vpp2);
}
}
}
if (updateVertexs.size() > 1) {
ofVec3f avg(0, 0, 0);
for (int i = 0; i < updateVertexs.size(); i++) {
avg += updateVertexs.at(i).pcp->getPolygonModelObject()->getMathModel().getVertexs()[updateVertexs.at(i).vertex];
}
avg /= updateVertexs.size();
for (int i = 0; i < updateVertexs.size(); i++) {
updateVertexs.at(i).pcp->getPolygonModelObject()->setVertex(updateVertexs.at(i).vertex, avg);
//vx.Polygons.push_back(PCPolygonPtr(updateVertexs.at(i).pcp));
}
this->vertexs.push_back(avg);
//vx.setPoint(avg);
//vertexs.push_back(vx);
}
}
}
saveCloud("UnifiedVertex.pcd", vertexs);
}
void saveCloud(const string& filename, const ofVec3f& p)
{
if (mapinect::IsFeatureSaveCloudActive())
{
PCPtr cloud (new PC());
cloud->push_back(OFVEC3F_PCXYZ(p));
saveCloud(filename, *cloud);
}
}
void ObjectsThread::updateDetectedObjects()
{
vector<TrackedCloudPtr> potentialOcclusions;
map<int,TrackedCloudPtr> occlusions = computeOcclusions(trackedClouds);
if(occlusions.size() > 0)
{
for (map<int,TrackedCloudPtr>::iterator iter = occlusions.begin(); iter != occlusions.end(); iter++) {
if(!(*iter).second->hasMatching())
(*iter).second->addCounter(1);
}
}
for (list<TrackedCloudPtr>::iterator iter = trackedClouds.begin(); iter != trackedClouds.end(); iter++) {
if ((*iter)->hasMatching() && (*iter)->isInViewField())
{
//if(!(*iter)->hasObject() ||
// occlusions.find((*iter)->getTrackedObject()->getId()) == occlusions.end())
//{
saveCloud("objPreMatched.pcd",*(*iter)->getTrackedCloud());
(*iter)->updateMatching();
saveCloud("objPostMatched.pcd",*(*iter)->getTrackedCloud());
//}
if (!((*iter)->hasObject())) {
(*iter)->addCounter(2);
}
else
{
(*iter)->addCounter(1);
}
(*iter)->removeMatching();
}
else
{
saveCloud("noMatched.pcd",*(*iter)->getTrackedCloud());
}
}
objectsCount(trackedClouds.size());
}
//--------------------------------------------------------------
void ArmCalibration::saveRawClouds()
{
int i = 1;
for (vector<MotorRotationsCloud>::const_iterator mrc = storedClouds.begin(); mrc != storedClouds.end(); ++mrc)
{
ofxXmlSettings XML;
XML.setValue(STOREDCLOUD_CONFIG "R1", mrc->rotations[0]);
XML.setValue(STOREDCLOUD_CONFIG "R2", mrc->rotations[1]);
XML.setValue(STOREDCLOUD_CONFIG "R4", mrc->rotations[2]);
XML.setValue(STOREDCLOUD_CONFIG "R8", mrc->rotations[3]);
XML.saveFile(getRawFilename(i, kFileTypeXML));
saveCloud(getRawFilename(i, kFileTypePCD), *mrc->cloud);
i++;
}
}
//--------------------------------------------------------------
void ArmCalibration::saveTransformedClouds()
{
int i = 1;
for (vector<MotorRotationsCloud>::const_iterator mrc = storedClouds.begin(); mrc != storedClouds.end(); ++mrc)
{
Eigen::Affine3f transformationMatrix =
arduino.calculateWorldTransformation(mrc->rotations[0], mrc->rotations[1], mrc->rotations[2], mrc->rotations[3]);
PC transformedCloud;
pcl::transformPointCloud(*mrc->cloud, transformedCloud, transformationMatrix);
saveCloud(getTransformedFilename(i), transformedCloud);
i++;
}
string cmd = "pcd_viewer_release.exe";
for (int j = 1; j < i; j++)
cmd += " " + getTransformedFilename(j);
system(cmd.c_str());
}
/* ---[ */
int
main (int argc, char** argv)
{
pcl::console::print_info ("Filter a point cloud using the pcl::TfQuadraticXYZComparison. For more information, use: %s -h\n", argv[0]);
if (argc < 3)
{
printHelp (argc, argv);
return (-1);
}
bool batch_mode = false;
// Command line parsing
float radius = default_radius;
bool inside = default_inside;
bool keep_organized = default_keep_organized;
pcl::console::parse_argument (argc, argv, "-radius", radius);
pcl::console::parse_argument (argc, argv, "-inside", inside);
pcl::console::parse_argument (argc, argv, "-keep", keep_organized);
std::string input_dir, output_dir;
if (pcl::console::parse_argument (argc, argv, "-input_dir", input_dir) != -1)
{
PCL_INFO ("Input directory given as %s. Batch process mode on.\n", input_dir.c_str ());
if (pcl::console::parse_argument (argc, argv, "-output_dir", output_dir) == -1)
{
PCL_ERROR ("Need an output directory! Please use -output_dir to continue.\n");
return (-1);
}
// Both input dir and output dir given, switch into batch processing mode
batch_mode = true;
}
if (!batch_mode)
{
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices;
p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (p_file_indices.size () != 2)
{
pcl::console::print_error ("Need one input PCD file and one output PCD file to continue.\n");
return (-1);
}
// Load the first file
Cloud::Ptr cloud (new Cloud);
if (!loadCloud (argv[p_file_indices[0]], cloud))
return (-1);
// Perform the feature estimation
Cloud::Ptr output (new Cloud);
compute (cloud, output, radius, inside, keep_organized);
// Save into the second file
saveCloud (argv[p_file_indices[1]], output);
}
else
{
if (input_dir != "" && boost::filesystem::exists (input_dir))
{
std::vector<std::string> pcd_files;
boost::filesystem::directory_iterator end_itr;
for (boost::filesystem::directory_iterator itr (input_dir); itr != end_itr; ++itr)
{
// Only add PCD files
if (!is_directory (itr->status ()) && boost::algorithm::to_upper_copy (boost::filesystem::extension (itr->path ())) == ".PCD" )
{
pcd_files.push_back (itr->path ().string ());
PCL_INFO ("[Batch processing mode] Added %s for processing.\n", itr->path ().string ().c_str ());
}
}
batchProcess (pcd_files, output_dir, radius, inside, keep_organized);
}
else
{
PCL_ERROR ("Batch processing mode enabled, but invalid input directory (%s) given!\n", input_dir.c_str ());
return (-1);
}
}
}
void PCPolyhedron::addToModel(const PCPtr& nuCloud)
{
PCModelObject::addToModel(nuCloud);
/// 1 - Detectar caras de la nueva nube
/// 2 - Descartar caras que no pertenecen a la caja (caras de la mano)
/// 3 - Matchear caras de la nube anterior con las nuevas caras
/// 4 - Estimar caras ocultas (?)
PCPtr trimmedCloud = getHalo(this->getvMin(),this->getvMax(),0.05,nuCloud);
saveCloud("nucloud.pcd",*nuCloud);
saveCloud("trimmed.pcd",*trimmedCloud);
pcPolygonsMutex.lock();
vector<PCPolygonPtr> prevPcPolygons = pcpolygons;
//Detecto nuevas caras
vector<PCPolygonPtr> nuevos = detectPolygons(trimmedCloud,Constants::OBJECT_PLANE_TOLERANCE(),Constants::CLOUD_VOXEL_SIZE * 2,false);
//Matching de las caras detectadas con las anteriores
nuevos = mergePolygons(nuevos);
//Estimación de caras no visibles
bool estimationOK;
vector<PCPolygonPtr> estimated = estimateHiddenPolygons(nuevos,estimationOK);
bool valid = true;
if(estimationOK)
{
for(int i = 0; i < nuevos.size(); i++)
nuevos.at(i)->setEstimated(false);
//Actualizo los nuevos polygons si no hubo errores
nuevos.insert(nuevos.end(),estimated.begin(),estimated.end());
pcpolygons = nuevos;
//Unifico vertices
unifyVertexs();
isvalid = validate();
}
//Chequeo volumenes
if(IsFeatureActive(FEATURE_INVALIDATE_OBJECT_BY_VOLUME))
{
float inVol = computeVolume(trimmedCloud);
if(getVolume() < inVol * Constants::OBJECT_VOLUME_TOLERANCE)
{
cout << "[ INVALID VOLUME ]" << endl;
cout << "need: " << inVol * 0.6 << " --- has: " << getVolume() << endl;
isvalid = false;
}
}
if(estimationOK && isvalid)
{
polygonsCache.clear();
for (vector<PCPolygonPtr>::iterator p = pcpolygons.begin(); p != pcpolygons.end(); ++p)
{
polygonsCache.push_back((*p)->getPolygonModelObject());
}
}
else
{
pcpolygons = prevPcPolygons;
//Si hay errores en la estimación, rollback de los pcpolygons mergeados
for(int i = 0; i < pcpolygons.size(); i++)
pcpolygons.at(i)->rollBackMatching();
if(!isvalid)
{
//Necesito unificar los vertices despues del rollback
unifyVertexs();
}
isvalid = false;
}
//Seteo nueva nube
PCPtr finalCloud (new PC());
for(int i = 0; i < pcpolygons.size(); i ++)
{
*finalCloud += *pcpolygons.at(i)->getCloud();
saveCloud("pol" + ofToString(pcpolygons.at(i)->getPolygonModelObject()->getName()) + ".pcd", *pcpolygons.at(i)->getCloud());
}
saveCloud("finalCloud" + ofToString(getId()) + ".pcd", *finalCloud);
setCloud(finalCloud);
pcPolygonsMutex.unlock();
}
vector<PCPolygonPtr> PCPolyhedron::detectPolygons(const PCPtr& cloud, float planeTolerance, float pointsTolerance, bool limitFaces)
{
float DOT_EPSILON = 0.15;
saveCloud("1_toDetect.pcd", *cloud);
PCPtr cloudTemp(cloud);
float maxFaces = limitFaces ? MAX_FACES : 100;
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2());
sensor_msgs::PointCloud2::Ptr cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
vector<PCPolygonPtr> nuevos;
PCPtr cloudP (new PC());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ExtractIndices<pcl::PointXYZ> extract;
std::vector<ofVec3f> vCloudHull;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (50);
seg.setDistanceThreshold (planeTolerance); //original: 0.01
// Create the filtering object
int i = 0, nrPoints = cloudTemp->points.size ();
// mientras 7% de la nube no se haya procesad+o
int numFaces = 0;
while (cloudTemp->points.size () > 0.07 * nrPoints && numFaces < maxFaces)
{
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloudTemp);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0) {
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
//FIX
PCPtr cloudFilteredTempInliers (new PC());
PCPtr cloudFilteredTempOutliers (new PC());
if(inliers->indices.size() != cloudTemp->size())
{
// Extract the inliers
extract.setInputCloud (cloudTemp);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloudFilteredTempInliers);
cloudP = cloudFilteredTempInliers;
}
else
cloudP = cloudTemp;
// Create the filtering object
extract.setInputCloud (cloudTemp);
extract.setIndices (inliers);
extract.setNegative (true);
if(cloudP->size() != cloudTemp->size())
extract.filter (*cloudFilteredTempOutliers);
cloudTemp = cloudFilteredTempOutliers;
saveCloud("2_DetectedPol" + ofToString(i) + ".pcd", *cloudP);
//Remove outliers by clustering
vector<pcl::PointIndices> clusterIndices(findClusters(cloudP, pointsTolerance, 10, 10000));
int debuccount = 0;
PCPtr cloudPFiltered (new PC());
if(clusterIndices.size() > 0)
{
cloudPFiltered = getCloudFromIndices(cloudP, clusterIndices.at(0));
}
saveCloud("3_Postfilter_pol" + ofToString(i) + ".pcd",*cloudPFiltered);
if (cloudPFiltered->size() < 4)
break;
//Controlo que las normales sean perpendiculares
ofVec3f norm (coefficients->values[0],coefficients->values[1],coefficients->values[2]);
norm.normalize();
bool normalCheck = true;
for(int i = 0; i < nuevos.size() && normalCheck; i++)
{
float dot = abs(nuevos[i]->getNormal().dot(norm));
if( dot > DOT_EPSILON)
{
normalCheck = false;
}
}
if(normalCheck)
{
//proyecto los puntos sobre el plano
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
PCPtr projectedCloud (new PC());
proj.setInputCloud(cloudPFiltered);
proj.setModelCoefficients(coefficients);
proj.filter(*projectedCloud);
saveCloud("4_Proy_Pol" + ofToString(i) + ".pcd",*projectedCloud);
PCPolygonPtr pcp(new PCQuadrilateral(*coefficients, projectedCloud));
pcp->detectPolygon();
nuevos.push_back(pcp);
numFaces++;
}
i++;
}
return nuevos;
}
//--------------------------------------------------------------
void ObjectsThread::processCloud()
{
PCPtr cloud;
{
inCloudMutex.lock();
cloud = PCPtr(new PC(*inCloud));
inCloud.reset();
inCloudMutex.unlock();
}
// Updating temporal detections
for (list<TrackedCloudPtr>::iterator iter = trackedClouds.begin(); iter != trackedClouds.end(); iter++) {
if((*iter)->hasObject())
{
PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get());
if (polyhedron != NULL)
{
// Está dentro del frustum si todos sus vértices lo están
(*iter)->setInViewField(Frustum::IsInFrustum(polyhedron->getVertexs()));
}
}
if(!(*iter)->hasObject() || (*iter)->isInViewField())
{
(*iter)->addCounter(-1);
}
}
int size = trackedClouds.size();
trackedClouds.remove_if(countIsLessThanZero);
int dif = size - trackedClouds.size();
if(cloud->empty())
{
updateDetectedObjects();
return;
}
list<TrackedCloudPtr> newClouds;
int debugCounter = 0;
{
setObjectsThreadStatus("Detecting clusters...");
saveCloud("rawClusters.pcd", *cloud);
std::vector<pcl::PointIndices> clusterIndices =
findClusters(cloud, Constants::OBJECT_CLUSTER_TOLERANCE(), Constants::OBJECT_CLUSTER_MIN_SIZE());
for (std::vector<pcl::PointIndices>::const_iterator it = clusterIndices.begin (); it != clusterIndices.end (); ++it)
{
PCPtr cloudCluster = getCloudFromIndices(cloud, *it);
gModel->tableMutex.lock();
TablePtr table = gModel->getTable();
gModel->tableMutex.unlock();
saveCloud("objectsCluster" + ofToString(debugCounter) + ".pcd", *cloudCluster);
if(table->isOnTable(cloudCluster))
newClouds.push_back(TrackedCloudPtr (new TrackedCloud(cloudCluster,false)));
else
newClouds.push_back(TrackedCloudPtr (new TrackedCloud(cloudCluster,true)));
debugCounter++;
}
}
// Look into the new clouds for the best fit
list<TrackedCloudPtr> cloudsToMatch;
list<TrackedCloudPtr> cloudsToAdd;
debugCounter = 0;
int maxIter = 10;
setObjectsThreadStatus("Matching clusters with existing ones...");
do
{
for (list<TrackedCloudPtr>::iterator iter = newClouds.begin(); iter != newClouds.end(); iter++)
{
//saveCloudAsFile("clusterInTable" + ofToString(debugCounter) + ".pcd", *(*iter)->getTrackedCloud());
TrackedCloudPtr removedCloud;
bool removed = false;
bool fitted = findBestFit(*iter, removedCloud, removed);
if (removed)
cloudsToMatch.push_back(removedCloud); // Push back the old cloud to try again
if (!fitted)
cloudsToAdd.push_back(*iter); // No matching cloud, this will be a new object
debugCounter++;
}
newClouds = cloudsToMatch;
cloudsToMatch.clear();
maxIter--;
}
while (newClouds.size() > 0 && maxIter > 0);
// Effectuate the update of the tracked cloud with the new ones
setObjectsThreadStatus("Update existing and new data...");
updateDetectedObjects();
trackedClouds.insert(trackedClouds.end(), cloudsToAdd.begin(), cloudsToAdd.end());
}
//--------------------------------------------------------------
map<int,TrackedCloudPtr> ObjectsThread::computeOcclusions(const list<TrackedCloudPtr>& potentialOcclusions)
{
map<int,TrackedCloudPtr> occlusions;
ofVec3f origin = PCXYZ_OFVEC3F(eyePos());
inCloudMutex.lock();
PCPtr cloud = PCPtr(new PC(*inRawCloud));
inRawCloud.reset();
inCloudMutex.unlock();
saveCloud("rawInternal.pcd",*cloud);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree(Constants::CLOUD_VOXEL_SIZE*2);
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::AlignedPointTVector voxelList;
if(cloud->size() > 0)
{
octree.setInputCloud(cloud);
octree.defineBoundingBox();
octree.addPointsFromInputCloud();
gModel->objectsMutex.lock();
for (list<TrackedCloudPtr>::const_iterator iter = potentialOcclusions.begin(); iter != potentialOcclusions.end(); iter++)
{
if((*iter)->hasObject())
{
bool occludedPol = true;
PCPolyhedron* polyhedron = dynamic_cast<PCPolyhedron*>((*iter)->getTrackedObject().get());
polyhedron->resetOccludedFaces();
vector<IPolygonPtr> pols = polyhedron->getVisiblePolygons();
int occludedFaces = 0;
for(int i = 0; i < pols.size(); i++)
{
vector<ofVec3f> vexs = pols.at(i)->getMathModel().getVertexs();
int occludedVertexs = 0;
for(int o = 0; o < vexs.size(); o++)
{
bool occludedVertex = false;
ofVec3f end = vexs.at(o);
Eigen::Vector3f endPoint(end.x,end.y,end.z);
Eigen::Vector3f originPoint = PCXYZ_EIGEN3F(eyePos());
voxelList.clear();
int voxs = octree.getApproxIntersectedVoxelCentersBySegment(originPoint,endPoint,voxelList,Constants::CLOUD_VOXEL_SIZE*2);
for(int i = 0; i < voxelList.size(); i ++)
{
if(octree.isVoxelOccupiedAtPoint(voxelList.at(i)))
{
ofVec3f intersect (voxelList.at(i).x,voxelList.at(i).y,voxelList.at(i).z);
if(((intersect - end).length() > Constants::CLOUD_VOXEL_SIZE*5) &&
(intersect - origin).length() < (end - origin).length())
occludedVertexs++;
}
}
}
if(occludedVertexs >= 2)
{
polyhedron->setOccludedFace(pols.at(i)->getName());
occludedFaces++;
}
}
if(occludedFaces > 1)
{
occlusions[(*iter)->getTrackedObject()->getId()] = (*iter);
//cout << " occluded pol " << endl;
}
}
}
gModel->objectsMutex.unlock();
}
return occlusions;
}
