/** * mergeFrames function * * This function merges multiple point clouds to one point cloud * @param std::vector<Frame3D> frames * @param std::string filename the filename to save to * @return point cloud */ pcl::PointCloud<pcl::PointNormal>::Ptr Functions3D::mergeFrames(const std::vector<Frame3D> &frames, std::string filename) { /** * Initialize a new point cloud */ pcl::PointCloud<pcl::PointNormal>::Ptr total_cloud(new pcl::PointCloud<pcl::PointNormal>); /** * Check if the file exists, otherwise process all frames */ if (pcl::io::loadPLYFile<pcl::PointNormal>("../data/" + filename, *total_cloud) == -1) { /** * Loop over all frames in the vector */ for (size_t i = 0; i < frames.size(); i++) { std::cout << "Processing frame " << i << std::endl; // Save reference instead of copy const Frame3D ¤t_frame = frames[i]; // get data from the frame cv::Mat depth = current_frame.depth_image_; double focal = current_frame.focal_length_; const Eigen::Matrix4f camera_pose = current_frame.getEigenTransform(); // compute cloud nessecary to append the point cloud pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud = depthToPointCloud(depth, focal, 1); pcl::PointCloud<pcl::PointNormal>::Ptr normal_cloud = computeNormals(pcloud); pcl::PointCloud<pcl::PointNormal>::Ptr transformed_normal_cloud = transformPointCloud(normal_cloud, camera_pose); // add the cloud to the existing point cloud addPointCloud(total_cloud, transformed_normal_cloud); } // @todo the combination of this with the load function does not work yet pcl::io::savePLYFile("../data/" + filename, *total_cloud); } /** * Convert to XYZ to show the cloud * uncomment other code to show the point cloud */ // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // pcl::copyPointCloud(*total_cloud, *cloud); // pcl::visualization::CloudViewer viewer("Simple Cloud Viewer"); // viewer.showCloud(cloud); // while (!viewer.wasStopped()) {} /** * Return the concatenated clouds */ return total_cloud; }
/*! This is the main routine for parsing input on the clientSocket. There should be one command for each line of input. This reads one line, and looks at the first word (up to the first space character) to determine the command. Then if there are body or robot indices to read, it calls a support routine to read those and return a vector of bodies or robots. These are then passed to the appropriate routine to carry out the action and write out any necessary results. */ void ClientSocket::readClient() { int i,numData,numBodies,numRobots; double time; std::vector<Body *> bodyVec; std::vector<Robot *> robVec; bool ok; while ( canReadLine() ) { line = readLine(); line.truncate(line.length()-1); //strip newline character lineStrList = QStringList::split(' ',line); strPtr = lineStrList.begin(); #ifdef GRASPITDBG std::cout <<"Command parser line: "<<line << std::endl; #endif if (*strPtr == "getContacts") { strPtr++; if (strPtr == lineStrList.end()) continue; numData = (*strPtr).toInt(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << "Num data: "<<numData<<std::endl; #endif if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendContacts(bodyVec[i],numData); } else if (*strPtr == "getAverageContacts") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendAverageContacts(bodyVec[i]); } else if (*strPtr == "getBodyName") { strPtr++; if (readBodyIndList(bodyVec)) continue; numBodies = bodyVec.size(); for (i=0;i<numBodies;i++) sendBodyName(bodyVec[i]); } else if(*strPtr == "setBodyName") { strPtr++; int body_index; if(strPtr != lineStrList.end()){ body_index = strPtr->toInt(&ok); strPtr++; if(strPtr == lineStrList.end()) return; if (body_index == -1 || body_index >= graspItGUI->getIVmgr()->getWorld()->getNumBodies()) { body_index = graspItGUI->getIVmgr()->getWorld()->getNumBodies() - 1; } graspItGUI->getIVmgr()->getWorld()->getBody(body_index)->setName(*strPtr); } } else if (*strPtr == "getRobotName") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendRobotName(robVec[i]); } else if (*strPtr == "getDOFVals") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) sendDOFVals(robVec[i]); } else if (*strPtr == "moveDOFs") { strPtr++; readDOFVals(); } else if (*strPtr == "render") graspItGUI->getIVmgr()->getViewer()->render(); else if (*strPtr == "setDOFForces") { strPtr++; if (readRobotIndList(robVec)) continue; numRobots = robVec.size(); for (i=0;i<numRobots;i++) if (readDOFForces(robVec[i])==FAILURE) continue; } else if ((*strPtr) == "moveDynamicBodies") { strPtr++; if (strPtr == lineStrList.end()) ok = FALSE; else { time = (*strPtr).toDouble(&ok); strPtr++; } if (!ok) moveDynamicBodies(-1); else moveDynamicBodies(time); } else if (*strPtr == "computeNewVelocities") { #ifdef GRASPITDBG std::cout << "cnv" << std::endl; #endif strPtr++; if (strPtr == lineStrList.end()) continue; time = (*strPtr).toDouble(&ok); strPtr++; if (!ok) continue; #ifdef GRASPITDBG std::cout << time <<std::endl; #endif computeNewVelocities(time); } else if ((*strPtr) == "outputPlannerResults"){ strPtr++; outputPlannerResults(0); } else if ((*strPtr) == "outputCurrentGrasp"){ strPtr++; outputCurrentGrasp(); } else if ((*strPtr) == "sendBodyTransf"){ strPtr++; verifyInput(1); sendBodyTransf(); } else if ((*strPtr) == "setBodyTransf"){ strPtr++; verifyInput(7); setBodyTransf(); } else if ((*strPtr) == "addObstacle"){ strPtr++; verifyInput(1); addObstacle(*(strPtr+1)); strPtr+=2; } else if ((*strPtr) == "addObject"){ verifyInput(2); addGraspableBody(*(strPtr+1), *(strPtr+2)); strPtr+=3; verifyInput(7); transf object_pose; readTransf(&object_pose); World * w = graspItGUI->getIVmgr()->getWorld(); w->getGB(w->getNumGB() - 1)->setTran(object_pose); } else if ((*strPtr) == "getCurrentHandTran"){ strPtr++; getCurrentHandTran(); } else if ((*strPtr) == "signalGraspUnreachable"){ strPtr+=4; std::cout << line.toStdString() << std::endl; graspItGUI->getIVmgr()->blinkBackground(); } else if ((*strPtr) == "getPlannerTarget"){ strPtr+=1; QTextStream os (this) ; os << graspItGUI->getIVmgr()->getWorld()->getCurrentHand()->getGrasp()->getObject()->getName() << "\n"; } else if ((*strPtr) == "setPlannerTarget"){ QTextStream os(this); os << setPlannerTarget(*(strPtr+1)) << "\n"; strPtr+=2; } else if ((*strPtr) == "rotateHandLat"){ strPtr+=1; rotateHandLat(); } else if ((*strPtr) == "rotateHandLong"){ strPtr+=1; rotateHandLong(); } else if ((*strPtr) == "exec"){ strPtr+=1; exec(); } else if ((*strPtr) == "next"){ strPtr+=1; next(); } else if ((*strPtr) == "addPointCloud") { strPtr += 1; addPointCloud(); //QTextStream os(this); //os << addPointCloud() <<" \n"; } else if ((*strPtr) == "setCameraOrigin") { strPtr += 1; setCameraOrigin(); } else if ((*strPtr) == "removeBodies"){ strPtr += 1; removeBodies(); } else if ((*strPtr) == "clearGraspableBodies"){ strPtr += 1; removeBodies(true); } } }