VHF (Viewpoint Feature Histogram) descriptorsを使ったvfh_recognition_node.cppでsegmentation faultのエラーを解決するために数日悩んだのでメモを残す。
次のソースコード42行目のvfh.compute (*vfhs)でexit-11、セグメンテーションフォールトでプロセスが落ちる。バックトレースするとeigen3ライブラリが怪しい。結局、いろいろ試したり、調べた結果、PCL Users mailing listの過去記事に答えがあった。pclをC++11のフラグをつけてソースからビルドし直すと解決した。
bool recognize_cb(tabletop_object_detector::TabletopObjectRecognition::Request &srv_request,
tabletop_object_detector::TabletopObjectRecognition::Response &srv_response)
{
//build kdtree index
flann::Index<flann::ChiSquareDistance<float> > index (*data, flann::LinearIndexParams ());
index.buildIndex ();
//clear any models in the response:
srv_response.models.resize(0);
// Create the VFH estimation class, and pass the input dataset+normals to it
pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
//Euclidean segmentation:
SegmentCloud(fromKinect, clouds);
//For storing results:
pcl::PointCloud<pcl::PointXYZ>::Ptr aligned_template (new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 recognized_msg;
Eigen::Matrix4f objectToView;
int numFound = 0;
std::cout << "Found " << clouds.size() << " clusters.\n";
//For each segment passed in:
for(unsigned int segment_it = 0; segment_it < clouds.size(); segment_it++){
vfh.setInputCloud (clouds.at(segment_it));
//Estimate normals:
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (clouds.at(segment_it));
pcl::search::KdTree<pcl::PointXYZ>::Ptr treeNorm (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (treeNorm);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
//VFH estimation
vfh.setInputNormals (cloud_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
vfh.setSearchMethod (tree);
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());
vfh.compute (*vfhs);
//Load histogram
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = vfhs->points[0].histogram[i];
}
//Algorithm parameters
float thresh = 100; //similarity threshold
int k = 1; //number of neighbors
//KNN classification
flann::Index<flann::ChiSquareDistance<float> > index (*data, flann::LinearIndexParams ());
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
//If model match is close enough, do finer pose estimation by RANSAC fitting.
if(k_distances[0][0] < thresh){
numFound++;
//Load nearest match
std::string cloud_name = models.at(k_indices[0][0]).first;
//Extract object label and view number from file name:
cloud_name.erase(cloud_name.end()-8, cloud_name.end()-4);
std::string recognitionLabel, viewNumber;
recognitionLabel.assign(cloud_name.begin()+5, cloud_name.end()-6);
viewNumber.assign(cloud_name.end()-5, cloud_name.end()-4);
//ROS_INFO the recognitionLabel and view number. This info will later be used to look the object up in a manipulation database.
ROS_INFO("%s", recognitionLabel.c_str());
ROS_INFO("%i", atoi(viewNumber.c_str()));
//Here, objectToView is the transformation of the detected object to its nearest viewpoint in the database.
//To get the object pose in the world frame: T(camera_to_world)*T(training_view_to_camera)*objectToView.
//The transformation to camera coordinates would then happen here by finding T(view_cam) in a lookup table and premultiplying
//by objectToView
alignTemplate(clouds.at(segment_it), cloud_name, aligned_template, objectToView);
//Convert rotational component of objectToView to Quaternion for messaging:
Eigen::Matrix3f rotation = objectToView.block<3,3>(0, 0);
Eigen::Quaternionf rotQ(rotation);
//Set num models in request:
srv_request.num_models = numFound;
//Build service response:
srv_response.models.resize(numFound);
srv_response.models[numFound-1].model_list.resize(1);
//model_id
srv_response.models[numFound-1].model_list[0].model_id = atoi(viewNumber.c_str()); //This ID will eventually correspond to the object label.
//PoseStamped
//Header
srv_response.models[numFound-1].model_list[0].pose.header.seq = 1; //Don't know what this is, but it's set.
srv_response.models[numFound-1].model_list[0].pose.header.stamp = fromKinect.header.stamp;
srv_response.models[numFound-1].model_list[0].pose.header.frame_id = "/camera_depth_optical_frame";
//Pose
//Position:
srv_response.models[numFound-1].model_list[0].pose.pose.position.x = objectToView(0,3);
srv_response.models[numFound-1].model_list[0].pose.pose.position.y = objectToView(1,3);
srv_response.models[numFound-1].model_list[0].pose.pose.position.z = objectToView(2,3);
//Orientation:
srv_response.models[numFound-1].model_list[0].pose.pose.orientation.x = rotQ.x();
srv_response.models[numFound-1].model_list[0].pose.pose.orientation.y = rotQ.y();
srv_response.models[numFound-1].model_list[0].pose.pose.orientation.z = rotQ.z();
srv_response.models[numFound-1].model_list[0].pose.pose.orientation.w = rotQ.w();
//confidence and cluster_model_indcices are not currently used.
}//end threshold if statement
}//end segment iterator
return(1);
}
環 境
- Ubuntu-16.04, PCL-1.8.1, gcc-5.4.0
実施した作業
- ここでは、pclのソースコードが~/src/pcl/pcl-1.8.1にあるする。
cd ~/src/pcl/pcl-1.8.1
- PCLのCMakeLists.txtの一番最後に以下を追加。
set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
cd ~/src/pcl/pcl-1.8.1mkdir buildcd buildcmake -DCMAKE_BUILD_TYPE=Release ..make -j12sudo make install- エラーが出ていたソースコードをビルドし直す。以下は例。
cd ~/catkin_wscatkin_make
終わり
コメント