PCL: VFH Descriptorを使ったソースコードでsegmentation fault

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.1
  • mkdir build
  • cd build
  • cmake -DCMAKE_BUILD_TYPE=Release ..
  • make -j12
  • sudo make install
  • エラーが出ていたソースコードをビルドし直す。以下は例。
    • cd ~/catkin_ws
    • catkin_make

終わり

 

コメントを残す

メールアドレスが公開されることはありません。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください