void GridAnalysis::rotateProbeGroup_(int axis, int degree)
{
TMatrix4x4<float> M;
TAngle<float> angle(degree, false);
Vector3 axis_vector(axis == 0, axis == 1, axis == 2);
M.setRotation(angle, axis_vector);
for (AtomIterator it = probe_group_.beginAtom(); it != probe_group_.endAtom(); it++)
{
it->setPosition(M*(it->getPosition()-center_)+center_);
}
}
void scan_callback(const sensor_msgs::PointCloud2ConstPtr &cloud_msg){
//local variables
double likelihood=0.0;
double height=0.0;
int id=1;
Fuzzy::FuzzyInstance instance;
std::vector<pcl::PointCloud<PointT> >::iterator cluster_iter;
visualization_msgs::MarkerArray marker_array;
impera_shape_msgs::CylinderShapeBag cylinder_shape_bag;
pcl::ModelCoefficients coefficients;
pcl::PointCloud<PointT> input_cloud;// the raw input point
std::vector<pcl::PointCloud<PointT> > clusters;
Fuzzy::FuzzyDLProducer producer;
///////////////////////////////////////////////
//take the same frame id as the original cloud;
if (cloud_msg->header.frame_id.empty())
frame_id_="/base_link";
else
frame_id_=cloud_msg->header.frame_id;
pcl::fromROSMsg(*cloud_msg, input_cloud);
ROS_INFO("Cylinder segmentation received %d points",(int)input_cloud.size());
//clusterize point cloud before applying Ransac
this->segment_clusters(boost::make_shared<pcl::PointCloud<PointT> >(input_cloud), clusters);
data_file_.open("cluster_features.txt");
for (cluster_iter=clusters.begin();cluster_iter!=clusters.end();cluster_iter++){
impera_shape_msgs::CylinderShape cylinder_shape;
PointT maxPointOnAxis;
PointT minPointOnAxis;
likelihood=get_cylinder_likelihood(boost::make_shared<pcl::PointCloud<PointT> >(*cluster_iter),coefficients);
if (coefficients.values.size()!=7){ //check if model is found otherwise continue;
continue;
}
//Debug coefficients
//std::cout << "Coefficients: "<<coefficients<<std::endl;
PointT pointOnAxis(coefficients.values[0],coefficients.values[1],coefficients.values[2]);
tf::Vector3 axis_vector(coefficients.values[3],coefficients.values[4],coefficients.values[5]);
height=get_cylinder_height(boost::make_shared<pcl::PointCloud<PointT> >(*cluster_iter),&maxPointOnAxis, &minPointOnAxis, coefficients);
//calculate the Orientation of the Cylinder in Quaternions
tf::Vector3 up_vector(0.0, 0.0, 1.0);
tf::Vector3 right_vector = axis_vector.cross(up_vector);
right_vector.normalize();
tf::Quaternion q(right_vector, -1.0*acos(axis_vector.dot(up_vector)));
q.normalize();
geometry_msgs::Quaternion cylinder_orientation;
geometry_msgs::Point cylinder_position;
geometry_msgs::Pose label_pose;
label_pose.orientation.w = 1.0;
tf::quaternionTFToMsg(q, cylinder_orientation);
cylinder_position.x=(maxPointOnAxis.x+minPointOnAxis.x)/2.0;
cylinder_position.y=(maxPointOnAxis.y+minPointOnAxis.y)/2.0;
cylinder_position.z=(maxPointOnAxis.z+minPointOnAxis.z)/2.0;
label_pose.position.x=maxPointOnAxis.x;
label_pose.position.y=maxPointOnAxis.y;
label_pose.position.z=maxPointOnAxis.z+0.30;
if (likelihood>=min_likelihood_){
visualization_msgs::Marker marker;
visualization_msgs::Marker label;
marker.ns = std::string("cylinders");
marker.header.frame_id = frame_id_ ;
marker.id = id;
marker.type = visualization_msgs::Marker::CYLINDER;
marker.action = visualization_msgs::Marker::ADD;
marker.scale.x = coefficients.values[6]*2.0;
marker.scale.y = coefficients.values[6]*2.0;
marker.scale.z = height;
marker.color.g = 0.5;
marker.color.r = 0.5;
marker.color.b = 0.0;
marker.color.a = 0.8;
marker.pose.orientation=cylinder_orientation;
marker.pose.position=cylinder_position;
marker.lifetime = ros::Duration(10);
marker.header.stamp = ros::Time::now();
label.ns = std::string("labels");
label.header.frame_id = frame_id_ ;
label.id = id;
label.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
label.action = visualization_msgs::Marker::ADD;
label.text=std::string("Object "+boost::lexical_cast<std::string>(id));
label.scale.x = 0.2;
label.scale.y = 0.2;
label.scale.z = 0.2;
label.color.g = 1.0;
label.color.r = 1.0;
label.color.b = 0.0;
label.color.a = 1.0;
label.pose=label_pose;
label.lifetime = ros::Duration(10);
label.header.stamp = ros::Time::now();
marker_array.markers.push_back(marker);
marker_array.markers.push_back(label);
//TODO send correct features as a shape
cylinder_shape.height=height;
//Take the base of the object as the position for the reasoner. It is important
//where the object is standing
cylinder_shape.position.x=cylinder_position.x;
cylinder_shape.position.y=cylinder_position.y;
cylinder_shape.position.z=cylinder_position.z;
cylinder_shape.orientation.x=1;
cylinder_shape.orientation.y=0;
cylinder_shape.orientation.z=0;
cylinder_shape.likelihood=likelihood;
cylinder_shape.radius= coefficients.values[6];
cylinder_shape_bag.cylinder_shape_bag.push_back(cylinder_shape);
instance.name=std::string("cylinder"+boost::lexical_cast<std::string>(id));
instance.hasCylinderType=likelihood;
instance.hasCylinderHeight=cylinder_shape.height;
instance.hasCylinderRadius=cylinder_shape.radius;
instance.hasX=cylinder_shape.position.x;
instance.hasY=cylinder_shape.position.y;
instance.hasZ=cylinder_shape.position.z;
instance.isOthogonal=(1.0-std::max(fabs(tf::Vector3(1.0,0.0,0).dot(axis_vector)),fabs(tf::Vector3(0.0,1.0,0).dot(axis_vector))));
instance.isParallel=std::max(fabs(tf::Vector3(1.0,0.0,0).dot(axis_vector)),fabs(tf::Vector3(0.0,1.0,0).dot(axis_vector)));
//instance.isOthogonal=1.0-tf::Vector3(1.0,1.0,0).dot(axis_vector);
//instance.isParallel=tf::Vector3(1.0,1.0,0).dot(axis_vector);
producer.AddInstance(instance);
//write extracted features
data_file_<<"Object Nr: "<<id<<std::endl;
data_file_<<"Height: "<<cylinder_shape.height<<std::endl;
data_file_<<"Radius: "<<cylinder_shape.radius<<std::endl;
data_file_<<"Position: "<<cylinder_shape.position.x<<","<<cylinder_shape.position.y<<","<<cylinder_shape.position.z<<std::endl;
data_file_<<"Orientation: "<<axis_vector.getX()<<","<<axis_vector.getY()<<","<<axis_vector.getZ()<<std::endl;
data_file_<<"##############################"<<std::endl;
id++;
pub_cylinder_marker_.publish(marker_array);
}
pub_cylinder_marker_.publish(marker_array);
//_pub_cylinder_bag.publish(cylinder_shape_bag);
}
data_file_.close();
//Save the instances to a file for later reasoning
//file has to be saved under the same name in any case
if (std::getenv("ROS_HOME")==NULL){
producer.GenerateKB(std::string(std::getenv("HOME"))+std::string("/.ros/")+std::string(Fuzzy::CYLINDER_KB_FILENAME));
}
else
producer.GenerateKB(std::string(std::getenv("ROS_HOME"))+std::string("/")+std::string(Fuzzy::CYLINDER_KB_FILENAME));
/*
//Message call to get the likelihood of the cylinders
reasoner_msgs::GetCylinders getCylinders;
reasoner_client_.call(getCylinders);
std::vector<reasoner_msgs::Reasoner>::iterator iter=getCylinders.response.result.begin();
while (iter!=getCylinders.response.result.end()){
std::cout << (*iter).name<<": "<<(*iter).likelihood <<std::endl;
iter++;
}
*/
}
