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;
}
