void view_file( const std::string& fileName ) { pcl::PointCloud<pcl::PointXYZ>::Ptr pScene( new pcl::PointCloud<pcl::PointXYZ>() ); if (pcl::io::loadPCDFile( fileName, *pScene ) < 0) { std::cout << "Error loading scene cloud." << std::endl; return; } visualize( pScene ); }
int main (int argc, char **argv) { #ifdef COMPILE_ME std::map<std::string, v4r::multiviewGraph> mv_environment_; boost::shared_ptr<ros::NodeHandle> nh; std::string models_dir; bool visualize_output; bool play_sequence_randomly = false; bool do_eval = true; // v4r::worldRepresentationROS myWorld; boost::shared_ptr<v4r::multiviewGraph> pMultiview_recognizer (new v4r::multiviewGraph()); // boost::shared_ptr<v4r::Recognizer> pSingleview_recognizer (new v4r::Recognizer()); ros::init ( argc, argv, "multiview_object_recognizer_node" ); nh.reset( new ros::NodeHandle ( "~" ) ); if(!(nh->getParam ( "do_eval", do_eval))) do_eval = false; // if(nh->getParam ( "visualize_output", visualize_output)) // myWorld.set_visualize_output(visualize_output); // myWorld.setModels_dir(models_dir); // myWorld.setSift(sift); if(!do_eval) { ROS_INFO("Multiview object recognizer is ready to get service callsssss."); ros::spin(); } else // do some offline evaluation (with files saved locally) { int id = 0; // for (pMultiview_recognizer->hv_params_.regularizer_ = 1; pMultiview_recognizer->hv_params_.regularizer_ <= 7; pMultiview_recognizer->hv_params_.regularizer_+=2) { // for (pMultiview_recognizer->hv_params_.color_sigma_l_ = 0.2; pMultiview_recognizer->hv_params_.color_sigma_l_ <= 0.4; pMultiview_recognizer->hv_params_.color_sigma_l_ +=0.2) { // for (pMultiview_recognizer->hv_params_.clutter_regularizer_ = 1; // pMultiview_recognizer->hv_params_.clutter_regularizer_ <= 5; // pMultiview_recognizer->hv_params_.clutter_regularizer_ += 2) { // if ((pMultiview_recognizer->hv_params_.regularizer_ == 5 && // pMultiview_recognizer->hv_params_.color_sigma_l_ == 0.2 && // pMultiview_recognizer->hv_params_.clutter_regularizer_ <= 1.5)) //// || (pMultiview_recognizer->cg_params_.cg_size_ ==0.01 && pMultiview_recognizer->cg_params_.cg_size_threshold_ == 4 //// && pMultiview_recognizer->hv_params_.clutter_regularizer_ < 1.5)) // continue; std::string dataset_path, sequence_name_provided; const std::string transform_prefix_ = "transformation_"; if(!nh->getParam("dataset_path", dataset_path)) ROS_ERROR("No dataset path given (arg \"dataset_path\"). "); if(!nh->getParam("sequence_name", sequence_name_provided)) ROS_ERROR("No sequence name given (arg \"sequence_name\"). "); std::stringstream eval_path_ss; boost::filesystem::path eval_folderpath; do { eval_path_ss.str(""); eval_path_ss << "/home/thomas/Projects/thomas.faeulhammer/" << "eval_" << id << "/"; eval_folderpath = eval_path_ss.str(); id++; }while(boost::filesystem::exists(eval_folderpath) ); boost::filesystem::create_directory(eval_folderpath); std::string eval_path = eval_path_ss.str(); std::vector < std::string > scene_folder; std::string start = ""; v4r::utils::getFoldersInDirectory(dataset_path, start, scene_folder); std::cout << "There are " << scene_folder.size() << " folders in directory " << dataset_path << "." << std::endl; for(size_t seq_id=0; seq_id < scene_folder.size(); seq_id++) { std::stringstream seq_path_ss; //seq_name_ss << "set_" << setw(5) << setfill('0') << seq_id; //seq_name_ss << "T_" << setw(2) << setfill('0') << seq_id << "_willow_dataset"; seq_path_ss << dataset_path << "/" << scene_folder[seq_id]; if(sequence_name_provided.length() && sequence_name_provided.compare(scene_folder[seq_id])!=0) continue; // std::stringstream scenes_dir_ss; // scenes_dir_ss << dataset_path << "/" << seq_name_ss.str(); // std::string scenes_dir = scenes_dir_ss.str(); std::cout << "Starting eval for " << seq_path_ss.str() << std::endl; std::vector < std::string > files_intern; v4r::utils::getFilesInDirectory (seq_path_ss.str(), files_intern, "", ".*.pcd", true); if(play_sequence_randomly) std::random_shuffle(files_intern.begin(), files_intern.end()); else std::sort(files_intern.begin(), files_intern.end()); if (files_intern.size()) { size_t sub_id_mv = 0; std::stringstream or_folderpath_mv_ss; boost::filesystem::path or_folderpath_mv; do { or_folderpath_mv_ss.str(std::string()); or_folderpath_mv_ss << eval_path << scene_folder[seq_id] << "_" << sub_id_mv << "_mv/"; or_folderpath_mv = or_folderpath_mv_ss.str(); sub_id_mv++; }while(boost::filesystem::exists(or_folderpath_mv) ); boost::filesystem::create_directory(or_folderpath_mv); size_t sub_id_sv = 0; std::stringstream or_folderpath_sv_ss; boost::filesystem::path or_folderpath_sv; do { or_folderpath_sv_ss.str(std::string()); or_folderpath_sv_ss << eval_path << scene_folder[seq_id] << "_" << sub_id_sv << "_sv/"; or_folderpath_sv = or_folderpath_sv_ss.str(); sub_id_sv++; }while(boost::filesystem::exists(or_folderpath_sv) ); boost::filesystem::create_directory(or_folderpath_sv); std::stringstream or_filepath_parameter_ss_sv; or_filepath_parameter_ss_sv << or_folderpath_sv_ss.str() << "parameter.nfo"; ofstream or_file; std::stringstream or_filepath_parameter_ss_mv; or_filepath_parameter_ss_mv << or_folderpath_mv_ss.str() << "parameter.nfo"; or_file.open (or_filepath_parameter_ss_mv.str().c_str()); or_filepath_parameter_ss_mv.str(std::string()); or_filepath_parameter_ss_mv << or_folderpath_mv_ss.str() << "view_temporal_order.nfo"; or_file.open (or_filepath_parameter_ss_mv.str().c_str()); for (size_t file_id=0; file_id < files_intern.size(); file_id++) { or_file << files_intern[file_id] << std::endl; } or_file.close(); for (size_t file_id=0; file_id < files_intern.size(); file_id++) { std::stringstream full_file_name; full_file_name << dataset_path << "/" << scene_folder[seq_id] << "/" << files_intern[file_id]; pcl::PointCloud<pcl::PointXYZRGB>::Ptr pScene (new pcl::PointCloud<pcl::PointXYZRGB>); pcl::io::loadPCDFile(full_file_name.str(), *pScene); std::vector<double> transform; transform.clear(); std::stringstream transform_ss; #ifndef USE_WILLOW_DATASET_FOR_EVAL transform_ss << dataset_path << "/" << scene_folder[seq_id] << "/" << transform_prefix_ << files_intern[file_id].substr(0, files_intern[file_id].length() - 3) << "txt"; #else transform_ss << dataset_path << "/" << scene_folder[seq_id] << "/" << "pose_" << files_intern[file_id].substr(6, files_intern[file_id].length() - 3 - 6 ) << "txt"; #endif std::cout << "Checking if path " << transform_ss.str() << " for transform exists. " << std::endl; if ( boost::filesystem::exists( transform_ss.str() ) ) { std::cout << "File exists." << std::endl; std::ifstream is(transform_ss.str().c_str()); #ifdef USE_WILLOW_DATASET_FOR_EVAL std::string s; std::vector<std::string> file_parts; std::getline( is, s ); std::istringstream ss( s ); std::vector<double> numbers; while (ss) { std::string s; if (!std::getline( ss, s, ' ' )) break; file_parts.push_back( s ); if(file_parts.size()>1) numbers.push_back(atof(s.c_str())); } #else std::istream_iterator<double> start(is), end; std::vector<double> numbers(start, end); std::cout << "Read " << numbers.size() << " numbers" << std::endl; #endif // print the numbers to stdout std::cout << "Transform to world coordinate system: " << std::endl; for(size_t i=0; i<numbers.size(); i++) { std::cout << numbers[i] << " "; transform.push_back(numbers[i]); } std::cout << std::endl; } else { std::cout << "File does not exist. Using it without world transform." << std::endl; } std::vector<ModelTPtr> models_mv; std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_mv; myWorld.recognize(pScene, scene_folder[seq_id], files_intern[file_id].substr(0, files_intern[file_id].length() - 3 - 1), ros::Time::now().nsec, transform, models_mv, transforms_mv, or_folderpath_mv_ss.str(), or_folderpath_sv_ss.str()); if ( models_mv.size() == 0 ) { std::cout << "I didn't detect any object in the current scene." << std::endl; } else { for ( size_t i=0; i < models_mv.size(); i++ ) { std::cout << "I detected object " << models_mv.at(i)->id_ << " in the scene." << std::endl; } } } } myWorld.clear(); // to reduce memory load } } } } } #endif return 0; }
bool CGeppetto::BuildFromInputScript(const tstring& sScript) { FILE* fp = tfopen_asset(GetPath(sScript), "r"); if (!fp) { TError("Could not read input script '" + sScript + "'\n"); return false; } std::shared_ptr<CData> pData(new CData()); CDataSerializer::Read(fp, pData.get()); CData* pOutput = pData->FindChild("Output"); if (!pOutput) { TError("Could not find Output section in input script '" + sScript + "'\n"); return false; } CData* pGame = pData->FindChild("Game"); if (!pGame) { TError("Could not find Game section in input script '" + sScript + "'\n"); return false; } t.SetGameDirectory(FindAbsolutePath(GetPath(pGame->GetValueString()))); tstring sOutputDir = ToForwardSlashes(pOutput->GetValueString()); t.SetOutputDirectory(GetDirectory(sOutputDir)); t.SetOutputFile(GetFilename(sOutputDir)); t.SetScriptDirectory(GetDirectory((GetPath(sScript)))); m_sOutput = FindAbsolutePath(t.GetGameDirectory() + T_DIR_SEP + pOutput->GetValueString()); CData* pSceneAreas = pData->FindChild("SceneAreas"); CData* pMesh = pData->FindChild("Mesh"); CData* pPhysics = pData->FindChild("Physics"); CData* pPhysicsShapes = pData->FindChild("PhysicsShapes"); // Find all file modification times. time_t iScriptModificationTime = GetFileModificationTime(sScript.c_str()); time_t iOutputModificationTime = GetFileModificationTime(m_sOutput.c_str()); tmap<tstring, time_t> aiSceneModificationTimes; if (pSceneAreas) { for (size_t i = 0; i < pSceneAreas->GetNumChildren(); i++) { CData* pArea = pSceneAreas->GetChild(i); if (pArea->GetKey() != "Area") continue; tstring sFile = pArea->FindChildValueString("File"); TAssert(sFile.length()); if (!sFile.length()) continue; auto it = aiSceneModificationTimes.find(sFile); if (it == aiSceneModificationTimes.end()) aiSceneModificationTimes[sFile] = GetFileModificationTime(sFile.c_str()); } } time_t iInputModificationTime = 0; if (pMesh) iInputModificationTime = GetFileModificationTime(pMesh->GetValueString().c_str()); time_t iPhysicsModificationTime = 0; if (pPhysics) iPhysicsModificationTime = GetFileModificationTime(pPhysics->GetValueString().c_str()); bool bRecompile = false; if (iScriptModificationTime > iOutputModificationTime) bRecompile = true; else if (iInputModificationTime > iOutputModificationTime) bRecompile = true; else if (iPhysicsModificationTime > iOutputModificationTime) bRecompile = true; else if (m_iBinaryModificationTime > iOutputModificationTime) bRecompile = true; else { for (auto it = aiSceneModificationTimes.begin(); it != aiSceneModificationTimes.end(); it++) { if (it->second > iOutputModificationTime) { bRecompile = true; break; } } } if (!bRecompile) { if (m_bForceCompile) { TMsg("Forcing rebuild even though no changes detected.\n"); } else { TMsg("No changes detected. Skipping '" + m_sOutput + "'.\n\n"); return true; } } CData* pGlobalTransforms = pData->FindChild("UseGlobalTransforms"); if (pGlobalTransforms) t.UseGlobalTransformations(); else t.UseLocalTransformations(); t.UseUV(); t.UseNormals(); if (pMesh) { tstring sExtension = pMesh->GetValueString().substr(pMesh->GetValueString().length()-4); if (sExtension == ".png") { TUnimplemented(); // Not updated since the switch to materials. int x, y, n; unsigned char* pData = stbi_load((GetPath(pMesh->GetValueString())).c_str(), &x, &y, &n, 0); if (!pData) { TError("Couldn't load '" + pMesh->GetValueString() + "', reason: " + stbi_failure_reason() + "\n"); return false; } stbi_image_free(pData); // Don't need it, just need the dimensions. Vector vecUp = Vector(0, 0, 0.5f) * ((float)y/100); Vector vecLeft = Vector(0, 0.5f, 0) * ((float)x/100); t.UseNormals(false); if (IsAbsolutePath(pMesh->GetValueString())) t.AddMaterial(GetPath(pMesh->GetValueString())); else t.AddMaterial(t.GetOutputDirectory() + "/" + pMesh->GetValueString(), GetPath(pMesh->GetValueString())); t.AddVertex(0, -vecLeft + vecUp, Vector2D(0.0f, 1.0f)); t.AddVertex(0, -vecLeft - vecUp, Vector2D(0.0f, 0.0f)); t.AddVertex(0, vecLeft - vecUp, Vector2D(1.0f, 0.0f)); t.AddVertex(0, -vecLeft + vecUp, Vector2D(0.0f, 1.0f)); t.AddVertex(0, vecLeft - vecUp, Vector2D(1.0f, 0.0f)); t.AddVertex(0, vecLeft + vecUp, Vector2D(1.0f, 1.0f)); } else if (sExtension == ".mat") { CMaterialHandle hMaterial(pMesh->GetValueString()); if (!hMaterial.IsValid()) { TError("Input material '" + pMesh->GetValueString() + "' does not exist or is invalid.\n"); return false; } if (!hMaterial->m_ahTextures.size()) { TError("Input material '" + pMesh->GetValueString() + "' has no textures.\n"); return false; } float w = (float)hMaterial->m_ahTextures[0]->m_iWidth; float h = (float)hMaterial->m_ahTextures[0]->m_iHeight; Vector vecUp = Vector(0, 0.5f, 0) * (h/hMaterial->m_iTexelsPerMeter); Vector vecRight = Vector(0, 0, 0.5f) * (w/hMaterial->m_iTexelsPerMeter); t.UseNormals(false); t.AddMaterial(pMesh->GetValueString()); t.AddVertex(0, -vecRight + vecUp, Vector2D(0.0f, 1.0f)); t.AddVertex(0, -vecRight - vecUp, Vector2D(0.0f, 0.0f)); t.AddVertex(0, vecRight - vecUp, Vector2D(1.0f, 0.0f)); t.AddVertex(0, -vecRight + vecUp, Vector2D(0.0f, 1.0f)); t.AddVertex(0, vecRight - vecUp, Vector2D(1.0f, 0.0f)); t.AddVertex(0, vecRight + vecUp, Vector2D(1.0f, 1.0f)); } else { TMsg("Reading model '" + GetPath(pMesh->GetValueString()) + "' ..."); std::shared_ptr<CConversionScene> pScene(new CConversionScene()); CModelConverter c(pScene.get()); if (!c.ReadModel(GetPath(pMesh->GetValueString()))) { TError("Couldn't read '" + GetPath(pMesh->GetValueString()) + "'.\n"); return false; } TMsg(" Done.\n"); TMsg("Building toy mesh ..."); LoadSceneIntoToy(pScene.get(), &t); TMsg(" Done.\n"); } } if (pPhysics) { TMsg("Reading physics model '" + GetPath(pPhysics->GetValueString()) + "' ..."); std::shared_ptr<CConversionScene> pScene(new CConversionScene()); CModelConverter c(pScene.get()); if (!c.ReadModel(GetPath(pPhysics->GetValueString()))) { TError("Couldn't read '" + GetPath(pPhysics->GetValueString()) + "'.\n"); return false; } TMsg(" Done.\n"); TMsg("Building toy physics model ..."); LoadSceneIntoToyPhysics(pScene.get(), &t); TMsg(" Done.\n"); } if (pPhysicsShapes) { for (size_t i = 0; i < pPhysicsShapes->GetNumChildren(); i++) { CData* pShape = pPhysicsShapes->GetChild(i); TAssert(pShape->GetKey() == "Box"); if (pShape->GetKey() != "Box") continue; TRS trs = pShape->GetValueTRS(); t.AddPhysBox(trs); } } if (pSceneAreas) LoadSceneAreas(pSceneAreas); return Compile(); }
bool CGeppetto::LoadSceneAreas(CData* pData) { tmap<tstring, std::shared_ptr<CConversionScene> > asScenes; tmap<tstring, size_t> aiSceneIDs; size_t iSceneArea = 0; for (size_t i = 0; i < pData->GetNumChildren(); i++) { CData* pArea = pData->GetChild(i); if (pArea->GetKey() == "NeighborDistance") { t.SetNeighborDistance(pArea->GetValueFloat()); continue; } if (pArea->GetKey() == "UseGlobalTransforms") { t.UseGlobalTransformations(); continue; } if (pArea->GetKey() == "UseLocalTransforms") { t.UseLocalTransformations(); continue; } TAssert(pArea->GetKey() == "Area"); if (pArea->GetKey() != "Area") continue; tstring sFile = pArea->FindChildValueString("File"); TAssert(sFile.length()); if (!sFile.length()) continue; tstring sMesh = pArea->FindChildValueString("Mesh"); TAssert(sMesh.length()); if (!sMesh.length()) continue; tstring sPhysics = pArea->FindChildValueString("Physics"); auto it = asScenes.find(sFile); if (it == asScenes.end()) { TMsg("Reading model '" + GetPath(sFile) + "' ..."); std::shared_ptr<CConversionScene> pScene(new CConversionScene()); CModelConverter c(pScene.get()); if (!c.ReadModel(GetPath(sFile))) { TError("Couldn't read '" + GetPath(sFile) + "'.\n"); return false; } TMsg(" Done.\n"); asScenes[sFile] = pScene; } iSceneArea++; CToyUtil ts; if (t.IsUsingUV()) ts.UseUV(); if (t.IsUsingNormals()) ts.UseNormals(); ts.SetGameDirectory(t.GetGameDirectory()); ts.SetOutputDirectory(t.GetOutputDirectory()); ts.SetOutputFile(tsprintf(t.GetOutputFile() + "_sa%d_" + pArea->GetValueString().tolower(), iSceneArea)); ts.UseLocalTransformations(t.IsUsingLocalTransformations()); CConversionSceneNode* pMeshNode = asScenes[sFile]->FindSceneNode(sMesh); CConversionSceneNode* pPhysicsNode = nullptr; if (sPhysics.length()) { pPhysicsNode = asScenes[sFile]->FindSceneNode(sPhysics); if (!pPhysicsNode) TError("Couldn't find a scene node in '" + sFile + "' named '" + sMesh + "'\n"); } TAssert(pMeshNode); TMsg("Building scene area toy ..."); Matrix4x4 mUpLeftSwap(Vector(1, 0, 0), Vector(0, 0, 1), Vector(0, -1, 0)); if (pMeshNode) LoadSceneNodeIntoToy(asScenes[sFile].get(), pMeshNode, mUpLeftSwap, &ts); else TError("Couldn't find a scene node in '" + sFile + "' named '" + sMesh + "'\n"); if (pPhysicsNode) LoadSceneNodeIntoToyPhysics(asScenes[sFile].get(), pPhysicsNode, mUpLeftSwap, &ts); TMsg(" Done.\n"); tstring sGameOutput = pArea->FindChildValueString("Output"); if (!sGameOutput.length()) sGameOutput = t.GetOutputDirectory() + "/" + ts.GetOutputFile() + ".toy"; tstring sFileOutput = FindAbsolutePath(t.GetGameDirectory() + T_DIR_SEP + sGameOutput); TMsg(tsprintf(" Mesh materials: %d\n", ts.GetNumMaterials())); TMsg(tsprintf(" Mesh tris: %d\n", ts.GetNumVerts() / 3)); TMsg(tsprintf(" Physics tris: %d\n", ts.GetNumPhysIndices() / 3)); if (t.IsUsingUV()) TMsg(" Using UV's\n"); if (t.IsUsingNormals()) TMsg(" Using normals\n"); TMsg("Writing scene area toy '" + sFileOutput + "' ..."); if (ts.Write(sFileOutput)) TMsg(" Done.\n"); else TMsg(" FAILED!\n"); aiSceneIDs[pArea->GetValueString()] = t.AddSceneArea(sGameOutput); } for (size_t i = 0; i < pData->GetNumChildren(); i++) { CData* pArea = pData->GetChild(i); if (pArea->GetKey() != "Area") continue; size_t iArea = aiSceneIDs[pArea->GetValueString()]; for (size_t i = 0; i < pArea->GetNumChildren(); i++) { CData* pNeighbor = pArea->GetChild(i); if (pNeighbor->GetKey() == "Neighbor") { TAssert(aiSceneIDs.find(pNeighbor->GetValueString()) != aiSceneIDs.end()); if (aiSceneIDs.find(pNeighbor->GetValueString()) == aiSceneIDs.end()) { TError("Couldn't find area \"" + pNeighbor->GetValueString() + "\"\n"); continue; } t.AddSceneAreaNeighbor(iArea, aiSceneIDs[pNeighbor->GetValueString()]); continue; } if (pNeighbor->GetKey() == "Visible") { TAssert(aiSceneIDs.find(pNeighbor->GetValueString()) != aiSceneIDs.end()); if (aiSceneIDs.find(pNeighbor->GetValueString()) == aiSceneIDs.end()) { TError("Couldn't find area \"" + pNeighbor->GetValueString() + "\"\n"); continue; } t.AddSceneAreaVisible(iArea, aiSceneIDs[pNeighbor->GetValueString()]); continue; } } } return true; }