void ObjectRecognition::loadModels(boost::shared_ptr<std::vector<Object>> objects) {
boost::filesystem::path models_path(ros::package::getPath("object_recognition") + "/trained_objects");
boost::regex modelfile_pattern("^model_([a-z]+)[0-9]*[.]pcd$");
for (boost::filesystem::recursive_directory_iterator iter(models_path), end; iter!=end; iter++) {
boost::match_results<std::string::const_iterator> results;
std::string file_path = iter->path().string();
std::string file_name = iter->path().leaf().string();
if (regex_match(file_name, results, modelfile_pattern)) {
std::string object_name = std::string(results[1].first, results[1].second);
Object trained_object;
trained_object.label = object_name;
if (-1 == pcl::io::loadPCDFile(file_path, *(trained_object.descriptors))) {
ROS_ERROR_STREAM("Unable to load object model: \"" << file_path << "\"");
continue;
}
objects->push_back(trained_object);
ROS_INFO_STREAM("Loaded object model \"" << object_name << "\"");
}
}
}
bool Tracker::cb_track_object(pacman_vision_comm::track_object::Request& req, pacman_vision_comm::track_object::Response& res)
{
if (isDisabled()){
//Module was temporary disabled, notify the sad user, then exit
ROS_ERROR("[Tracker::%s]\tNode is globally disabled, this service is suspended!",__func__);
return false;
}
if (req.name.empty()){
ROS_ERROR("[Tracker::%s] You need to provide the name of the object you want to track!", __func__);
return false;
}
std::string models_path (ros::package::getPath("asus_scanner_models"));
if (!storage->searchObjName(req.name, index)){
ROS_ERROR("[Tracker::%s] Cannot find %s from the pool of already estimated objects, check spelling or run a Pose Estimation first!", __func__, req.name.c_str());
return false;
}
obj_name.first = (req.name);
std::vector<std::string> vst;
boost::split(vst, req.name, boost::is_any_of("_"), boost::token_compress_on);
obj_name.second = vst.at(0);
if (!storage->readObjTransformByIndex(index, transform)){
ROS_ERROR("[Tracker::%s] Cannot find %s transform from the pool of already estimated transforms, check spelling or run an estimation first!", __func__, req.name.c_str());
return false;
}
boost::filesystem::path model_path (models_path + "/" + obj_name.second + "/" + obj_name.second + ".pcd");
if (boost::filesystem::exists(model_path) && boost::filesystem::is_regular_file(model_path))
{
if (pcl::io::loadPCDFile(model_path.c_str(), *orig_model))
{
ROS_ERROR("[Tracker::%s] Error loading model %s",__func__, model_path.c_str());
return false;
}
}
else
{
ROS_ERROR("[Tracker::%s] Requested model (%s) does not exists in asus_scanner_models package",__func__, model_path.stem().c_str());
return false;
}
basic_config->get("downsampling_leaf_size", leaf);
computeModel();
//Get the minimum and maximum values on each of the 3 (x-y-z) dimensions of model
//also get model centroid
pcl::CentroidPoint<PX> mc;
std::vector<float> xvec,yvec,zvec;
for (size_t i=0; i<model->points.size(); ++i)
{
xvec.push_back(model->points[i].x);
yvec.push_back(model->points[i].y);
zvec.push_back(model->points[i].z);
mc.add(model->points[i]);
}
bounding_box->x1 = *std::min_element(xvec.begin(), xvec.end());
bounding_box->y1 = *std::min_element(yvec.begin(), yvec.end());
bounding_box->z1 = *std::min_element(zvec.begin(), zvec.end());
bounding_box->x2 = *std::max_element(xvec.begin(), xvec.end());
bounding_box->y2 = *std::max_element(yvec.begin(), yvec.end());
bounding_box->z2 = *std::max_element(zvec.begin(), zvec.end());
mc.get(model_centroid);
//init icps
icp.setUseReciprocalCorrespondences(false);
icp.setMaximumIterations(50);
icp.setTransformationEpsilon(1e-9);
icp.setEuclideanFitnessEpsilon(1e-9);
//Correspondence Rejector(s)
crd->setMaximumDistance(rej_distance);
icp.addCorrespondenceRejector(crd);
icp.setInputSource(model);
//Transformation Estimation
icp.setTransformationEstimation(teDQ);
storage->writeTrackedIndex(index);
//we are ready to start
started = true;
create_markers();
return true;
}
