double
dist_nn_3d_model(pcl::PointCloud<pcl::PointXYZ>::Ptr model_cloud, pcl::PointCloud<pcl::PointXYZ> obj_cloud,
carmen_point_t particle_pose, carmen_vector_3D_t car_global_position)
{
double theta = -particle_pose.theta;
pcl::PointXYZ point;
double dist = 0.0;
int k = 1; //k-nearest neighbor
// kd-tree object
pcl::search::KdTree<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(model_cloud);
// This vector will store the output neighbors.
std::vector<int> pointIndices(k);
// This vector will store their squared distances to the search point.
std::vector<float> squaredDistances(k);
for (pcl::PointCloud<pcl::PointXYZ>::iterator it = obj_cloud.begin(); it != obj_cloud.end(); ++it)
{
// Necessary transformations (translation and rotation):
float x = car_global_position.x + it->x - particle_pose.x;
float y = car_global_position.y + it->y - particle_pose.y;
point.z = it->z;
point.x = x*cos(theta) - y*sin(theta);
point.y = x*sin(theta) + y*cos(theta);
if (kdtree.nearestKSearch(point, k, pointIndices, squaredDistances) > 0)
{
// dist += sqrt(double(squaredDistances[0]));
dist += double(squaredDistances[0]);
}
}
return dist;
}
void InnerModelManager::setPointCloudData(const std::string id, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud)
{
QString m = QString("setPointCloudData");
/// Aqui Marco va a mejorar el código :-) felicidad (comprobar que la nube existe)
IMVPointCloud *pcNode = imv->pointCloudsHash[QString::fromStdString(id)];
int points = cloud->size();
pcNode->points->resize(points);
pcNode->colors->resize(points);
pcNode->setPointSize(3);
int i = 0;
for(pcl::PointCloud<pcl::PointXYZRGBA>::iterator it = cloud->begin(); it != cloud->end(); it++ )
{
if (!pcl_isnan(it->x)&&!pcl_isnan(it->y)&&!pcl_isnan(it->z))
{
std::cout<<"Adding: "<<it->x<<" "<<it->y<<" "<<it->z<<endl;
pcNode->points->operator[](i) = QVecToOSGVec(QVec::vec3(it->x, it->y, it->z));
pcNode->colors->operator[](i) = osg::Vec4f(float(it->r)/255, float(it->g)/255, float(it->b)/255, 1.f);
}
i++;
}
pcNode->update();
imv->update();
}
template <typename GeneratorT> int
pcl::common::CloudGenerator<pcl::PointXY, GeneratorT>::fill (int width, int height, pcl::PointCloud<pcl::PointXY>& cloud)
{
if (width < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud width must be >= 1\n!");
return (-1);
}
if (height < 1)
{
PCL_ERROR ("[pcl::common::CloudGenerator] Cloud height must be >= 1\n!");
return (-1);
}
if (!cloud.empty ())
PCL_WARN ("[pcl::common::CloudGenerator] Cloud data will be erased with new data\n!");
cloud.width = width;
cloud.height = height;
cloud.resize (cloud.width * cloud.height);
cloud.is_dense = true;
for (pcl::PointCloud<pcl::PointXY>::iterator points_it = cloud.begin ();
points_it != cloud.end ();
++points_it)
{
points_it->x = x_generator_.run ();
points_it->y = y_generator_.run ();
}
return (0);
}
void occlude_pcd(pcl::PointCloud<pcl::PointXYZ>::Ptr & cld_ptr,
int dim, double threshA, double threshB)
{
for(pcl::PointCloud<pcl::PointXYZ>::iterator it = cld_ptr->begin();
it != cld_ptr->end();)
{
if((it->data[dim] < threshA) || (it->data[dim] > threshB))
it = cld_ptr->erase(it);
else
++it;
}
}
void PointCloudDetection::setPointcloud(pcl::PointCloud<PointRGBT>::Ptr points){
pointcloud_.clear();
pcl::PointCloud<PointRGBT>::iterator iter;
for (iter=points->begin();iter!=points->end();iter++){
PointRGBT color_pt(iter->r,iter->g,iter->b);
color_pt.x=iter->x;
color_pt.y=iter->y;
color_pt.z=iter->z;
pointcloud_.push_back(color_pt);
}
this->computeNormals();
}
bool DynModelExporter::getCenterAndScale(Plane<float> &plane, pcl::PointCloud<PointXYZRGB>::Ptr scene_cloud, PointXYZ ¢er, PointXYZ &scale)
{
center.x = 0.0;
center.y = 0.0;
center.z = 0.0;
float size = 0.0;
PointXYZ min(9999999.0, 9999999.0, 9999999.0);
PointXYZ max(-9999999.0, -9999999.0, -9999999.0);
for (pcl::PointCloud<PointXYZRGB>::iterator it = scene_cloud->begin(); it != scene_cloud->end(); ++it)
{
// 1cm TODO - make as param
if (plane.distance(cv::Vec3f(it->x, it->y, it->z)) < 0.05)
{
center.x += it->x;
center.y += it->y;
center.z += it->z;
size += 1.0;
if (it->x < min.x) min.x = it->x;
if (it->y < min.y) min.y = it->y;
if (it->z < min.z) min.z = it->z;
if (it->x > max.x) max.x = it->x;
if (it->y > max.y) max.y = it->y;
if (it->z > max.z) max.z = it->z;
}
}
//std::cout << std::endl << std::endl << min << " " << max << std::endl << std::endl;
if (size > 10)
{
center.x /= size;
center.y /= size;
center.z /= size;
//center.z = -(center.x*plane.a + center.y*plane.b + plane.d)/plane.c;
}
else return false;
scale.x = max.x - min.x;
if (scale.x > 3)
scale.x = 3;
scale.y = max.y - min.y;
if (scale.y > 3)
scale.y = 3;
scale.z = max.z - min.z;
if (scale.z > 3)
scale.z = 3;
return true;
}
/**
* @brief sets the internal point cloud
* @param points the pointcloud
*/
void PointCloudDetection::setPointcloud(pcl::PointCloud<PointT>::Ptr points){
pointcloud_.clear();
pcl::PointCloud<PointT>::iterator iter;
PointRGBT color_pt(255,255,255); //default color white if no color is given
for (iter=points->begin();iter!=points->end();iter++){
color_pt.x=iter->x;
color_pt.y=iter->y;
color_pt.z=iter->z;
pointcloud_.push_back(color_pt);
}
this->computeNormals();
}
bool OverRelation::solve(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &source,
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &target,
pcl::PointCloud<pcl::PointXYZRGBA>::VectorType &source_voxel_vector,
pcl::PointCloud<pcl::PointXYZRGBA>::VectorType &target_voxel_vector){
Vector4f source_center;
pcl::compute3DCentroid(*source, source_center);
Vector4f target_center;
pcl::compute3DCentroid(*target, target_center);
Vector3f target_to_source((source_center[0]-target_center[0]), (source_center[1]-target_center[1]), (source_center[2]-target_center[2]));
target_to_source = target_to_source.normalized();
//check is it about above?
// float dot_result = target_to_source.dot(Vector3f(1,0,0));
// ROS_INFO("DOT_RESULT x: %f", dot_result);
// dot_result = target_to_source.dot(Vector3f(0,1,0));
// ROS_INFO("DOT_RESULT y: %f", dot_result);
float dot_result = target_to_source.dot(Vector3f(0,-1,0));
// dot_result = target_to_source.dot(Vector3f(0,0,1));
if( dot_result < 1 - threshold_)
return false;
ROS_INFO("DOT_RESULT y: %f", dot_result);
for(pcl::PointCloud<pcl::PointXYZRGBA>::VectorType::iterator target_it = target_voxel_vector.begin();
target_it < target_voxel_vector.end();
target_it++){
for(pcl::PointCloud<pcl::PointXYZRGBA>::VectorType::iterator source_it = source_voxel_vector.begin();
source_it < source_voxel_vector.end();
source_it++){
if((*target_it).y < (*source_it).y)
return false;
}
}
return true;
}
template <typename PointT> void
pcl::LCCPSegmentation<PointT>::relabelCloud (pcl::PointCloud<pcl::PointXYZL> &labeled_cloud_arg)
{
if (grouping_data_valid_)
{
// Relabel all Points in cloud with new labels
typename pcl::PointCloud<pcl::PointXYZL>::iterator voxel_itr = labeled_cloud_arg.begin ();
for (; voxel_itr != labeled_cloud_arg.end (); ++voxel_itr)
{
voxel_itr->label = sv_label_to_seg_label_map_[voxel_itr->label];
}
}
else
{
PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
}
}
pcl::PointCloud<PointT>::Ptr removePlane(const pcl::PointCloud<PointT>::Ptr scene)
{
pcl::ModelCoefficients::Ptr plane_coef(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.02);
seg.setInputCloud(scene);
seg.segment(*inliers, *plane_coef);
pcl::ProjectInliers<PointT> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (scene);
proj.setModelCoefficients (plane_coef);
pcl::PointCloud<PointT>::Ptr scene_projected(new pcl::PointCloud<PointT>());
proj.filter (*scene_projected);
pcl::PointCloud<PointT>::iterator it_ori = scene->begin();
pcl::PointCloud<PointT>::iterator it_proj = scene_projected->begin();
pcl::PointCloud<PointT>::Ptr scene_f(new pcl::PointCloud<PointT>());
for( int base = 0 ; it_ori < scene->end(), it_proj < scene_projected->end() ; it_ori++, it_proj++, base++ )
{
float diffx = it_ori->x-it_proj->x;
float diffy = it_ori->y-it_proj->y;
float diffz = it_ori->z-it_proj->z;
if( diffx * it_ori->x + diffy * it_ori->y + diffz * it_ori->z >= 0 )
continue;
//distance from the point to the plane
float dist = sqrt(diffx*diffx + diffy*diffy + diffz*diffz);
if ( dist >= 0.03 )//fabs((*it_ori).x) <= 0.1 && fabs((*it_ori).y) <= 0.1 )
scene_f->push_back(*it_ori);
}
return scene_f;
}
void addSupervoxelConnectionsToViewer(pcl::PointXYZRGBA &supervoxel_center,
pcl::PointCloud< pcl::PointXYZRGBA> &adjacent_supervoxel_centers,
std::string supervoxel_name,
pcl::visualization::PCLVisualizer &viewer)
{
vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();
vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New();
// Setup colors
unsigned char red[3] = {255, 0, 0};
unsigned char green[3] = {0, 255, 0};
unsigned char blue[3] = {0, 0, 255};
vtkSmartPointer<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
colors->SetNumberOfComponents(3);
colors->SetName("Colors");
colors->InsertNextTupleValue(red);
colors->InsertNextTupleValue(green);
colors->InsertNextTupleValue(blue);
//Iterate through all adjacent points, and add a center point to adjacent point pair
pcl::PointCloud< pcl::PointXYZRGBA>::iterator adjacent_itr = adjacent_supervoxel_centers.begin();
for(; adjacent_itr != adjacent_supervoxel_centers.end(); ++adjacent_itr)
{
points->InsertNextPoint(supervoxel_center.data);
points->InsertNextPoint(adjacent_itr->data);
}
// Create a polydata to store everything in
vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
// Add the points to the dataset
polyData->SetPoints(points);
polyLine->GetPointIds()->SetNumberOfIds(points->GetNumberOfPoints());
for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
{
polyLine->GetPointIds()->SetId(i, i);
}
cells->InsertNextCell(polyLine);
// Add the lines to the dataset
polyData->SetLines(cells);
// polyData->GetPointData()->SetScalars(colors);
viewer.addModelFromPolyData(polyData, supervoxel_name);
}
void Node::crop_cloud(pcl::PointCloud<pcl::PointXYZ> &pcl_cloud, int laser, ros::Time time)
{
//CROP CLOUD
pcl::PointCloud<pcl::PointXYZ>::iterator i;
for (i = pcl_cloud.begin(); i != pcl_cloud.end();)
{
bool remove_point = 0;
if (i->z < 0 || i->z > max_z)
{
remove_point = 1;
}
if (sqrt(pow(i->x,2) + pow(i->y,2)) > max_radius)
{
remove_point = 1;
}
tf::StampedTransform transform;
listener_.lookupTransform ("/world", "/laser1", time, transform);
if (sqrt(pow(transform.getOrigin().x() - i->x,2) + pow(transform.getOrigin().y() - i->y,2)) > 0.25 && laser == 1)
{
remove_point = 1;
}
if (remove_point == 1)
{
i = pcl_cloud.erase(i);
}
else
{
i++;
}
}
//END CROP CLOUD
}
pcl::PointCloud<pcl::PointXYZ>::Ptr PlanarCutTransformator::transformPc(pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
pcl::PointCloud<pcl::PointXYZ> retPc;
pcl::PointCloud<pcl::PointXYZ>::iterator pointIt = pc->begin();
pcl::PointCloud<pcl::PointXYZ>::iterator lastIt = pc->end();
while(pointIt != lastIt) {
pcl::PointXYZ currentPoint = *pointIt;
vec r = stdToArmadilloVec(createJointsVector(3, currentPoint.x, currentPoint.y, currentPoint.z));
vec comp = normalVec.t() * (r - plainOriginVec);
double coordinate = comp(0);
if(coordinate >= 0) {
retPc.push_back(*pointIt);
} else {
// point gets kicked out
}
++pointIt;
}
return retPc.makeShared();
}
cos_lib::bounding_box::bounding_box(pcl::PointCloud<point_clstr>::Ptr cloud)
{
pcl::PointCloud<point_clstr>::iterator cloud_it;
float xmin=99999,xmax=-99999,ymin=99999,ymax=-99999,zmin=99999,zmax=-99999;
cloud_it++;
for(cloud_it=cloud->begin(); cloud_it!=cloud->end(); cloud_it++)
{
if((*cloud_it).x < xmin) xmin = (*cloud_it).x;
if((*cloud_it).x >= xmax) xmax = (*cloud_it).x;
if((*cloud_it).y < ymin) ymin = (*cloud_it).y;
if((*cloud_it).y >= ymax) ymax = (*cloud_it).y;
if((*cloud_it).z < zmin) zmin = (*cloud_it).z;
if((*cloud_it).z >= zmax) zmax = (*cloud_it).z;
this->addPointIntoBox(&(*cloud_it));
}
this->A = new point_clstr(xmin,ymax,zmin, 0, 255, 0);
this->B = new point_clstr(xmin,ymax,zmax, 0, 255, 0);
this->C = new point_clstr(xmax,ymax,zmax, 0, 255, 0);
this->D = new point_clstr(xmax,ymax,zmin, 0, 255, 0);
this->E = new point_clstr(xmin,ymin,zmax, 0, 255, 0);
this->F = new point_clstr(xmax,ymin,zmax, 0, 255, 0);
this->G = new point_clstr(xmax,ymin,zmin, 0, 255, 0);
this->H = new point_clstr(xmin,ymin,zmin, 0, 255, 0);
}
void LocalPlanner::filterPointCloud(pcl::PointCloud<pcl::PointXYZ>& complete_cloud)
{
pcl::PointCloud<pcl::PointXYZ>::iterator pcl_it;
pcl::PointCloud<pcl::PointXYZ> front_cloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
final_cloud.points.clear();
for (pcl_it = complete_cloud.begin(); pcl_it != complete_cloud.end(); ++pcl_it)
{
// Check if the point is invalid
if (!std::isnan (pcl_it->x) && !std::isnan (pcl_it->y) && !std::isnan (pcl_it->z))
{
if((pcl_it->x)<max_cache.x&&(pcl_it->x)>min_cache.x&&(pcl_it->y)<max_cache.y&&(pcl_it->y)>min_cache.y&&(pcl_it->z)<max_cache.z&&(pcl_it->z)>min_cache.z)
{
octomapCloud.push_back(pcl_it->x, pcl_it->y, pcl_it->z);
}
if((pcl_it->x)<front.x&&(pcl_it->x)>back.x&&(pcl_it->y)<front.y&&(pcl_it->y)>back.y&&(pcl_it->z)<front.z&&(pcl_it->z)>back.z)
{
front_cloud.points.push_back(pcl::PointXYZ(pcl_it->x,pcl_it->y,pcl_it->z));
}
// if((pcl_it->x)<max.x&&(pcl_it->x)>min.x&&(pcl_it->y)<max.y&&(pcl_it->y)>min.y&&(pcl_it->z)<max.z&&(pcl_it->z)>min.z)
// {
// cloud->points.push_back(pcl::PointXYZ(pcl_it->x,pcl_it->y,pcl_it->z));
// }
}
}
if(front_cloud.points.size()>1)
{
// octomapCloud.crop(octomap::point3d(min_cache.x,min_cache.y,min_cache.z),octomap::point3d(half_cache.x,half_cache.y,half_cache.z));
obstacle = true;
octomap::Pointcloud::iterator oc_it;
for (oc_it = octomapCloud.begin(); oc_it != octomapCloud.end(); ++oc_it)
{
if((oc_it->x())<max.x&&(oc_it->x())>min.x&&(oc_it->y())<max.y&&(oc_it->y())>min.y&&(oc_it->z())<max.z&&(oc_it->z())>min.z)// if((data.x)<max_x&&(data.x)>-min_x&&(data.y)<max_y&&(data.y)>-min_y&&(data.z)<max_z&&(data.z)>-min_z)
{
cloud->points.push_back(pcl::PointXYZ(oc_it->x(),oc_it->y(),oc_it->z()));
}
}
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud);
sor.setMeanK (5);
sor.setStddevMulThresh (0.5);
sor.filter(final_cloud);
}
else
obstacle = false;
ROS_INFO(" Cloud transformed");
final_cloud.header.stamp = complete_cloud.header.stamp;
final_cloud.header.frame_id = complete_cloud.header.frame_id;
final_cloud.width = final_cloud.points.size();
final_cloud.height = 1;
}
static int vscanDetection(int closest_waypoint)
{
if (_vscan.empty() == true)
return -1;
for (int i = closest_waypoint + 1; i < closest_waypoint + _search_distance; i++) {
if(i > _path_dk.getPathSize() - 1 )
return -1;
tf::Vector3 tf_waypoint = _path_dk.transformWaypoint(i);
//tf::Vector3 tf_waypoint = TransformWaypoint(_transform,_current_path.waypoints[i].pose.pose);
tf_waypoint.setZ(0);
//std::cout << "waypoint : "<< tf_waypoint.getX() << " "<< tf_waypoint.getY() << std::endl;
int point_count = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
if ((item->x == 0 && item->y == 0) || item->z > _detection_height_top || item->z < _detection_height_bottom)
continue;
tf::Vector3 point((double) item->x, (double) item->y, 0);
double dt = tf::tfDistance(point, tf_waypoint);
if (dt < _detection_range) {
point_count++;
//std::cout << "distance :" << dt << std::endl;
//std::cout << "point : "<< (double) item->x << " " << (double)item->y << " " <<(double) item->z << std::endl;
//std::cout << "count : "<< point_count << std::endl;
}
if (point_count > _threshold_points)
return i;
}
}
return -1;
}
static EControl vscanDetection()
{
if (_vscan.empty() == true || _closest_waypoint < 0)
return KEEP;
int decelerate_or_stop = -10000;
int decelerate2stop_waypoints = 15;
for (int i = _closest_waypoint; i < _closest_waypoint + _search_distance; i++) {
g_obstacle.clearStopPoints();
if (!g_obstacle.isDecided())
g_obstacle.clearDeceleratePoints();
decelerate_or_stop++;
if (decelerate_or_stop > decelerate2stop_waypoints ||
(decelerate_or_stop >= 0 && i >= _path_dk.getSize()-1) ||
(decelerate_or_stop >= 0 && i == _closest_waypoint+_search_distance-1))
return DECELERATE;
if (i > _path_dk.getSize() - 1 )
return KEEP;
// Detection for cross walk
if (i == vmap.getDetectionWaypoint()) {
if (CrossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP) {
_obstacle_waypoint = i;
return STOP;
}
}
// waypoint seen by vehicle
geometry_msgs::Point waypoint = calcRelativeCoordinate(_path_dk.getWaypointPosition(i),
_current_pose.pose);
tf::Vector3 tf_waypoint = point2vector(waypoint);
tf_waypoint.setZ(0);
int stop_point_count = 0;
int decelerate_point_count = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
tf::Vector3 vscan_vector((double) item->x, (double) item->y, 0);
// for simulation
if (g_sim_mode) {
tf::Transform transform;
tf::poseMsgToTF(_sim_ndt_pose.pose, transform);
geometry_msgs::Point world2vscan = vector2point(transform * vscan_vector);
vscan_vector = point2vector(calcRelativeCoordinate(world2vscan, _current_pose.pose));
vscan_vector.setZ(0);
}
// 2D distance between waypoint and vscan points(obstacle)
// ---STOP OBSTACLE DETECTION---
double dt = tf::tfDistance(vscan_vector, tf_waypoint);
if (dt < _detection_range) {
stop_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
if (stop_point_count > _threshold_points) {
_obstacle_waypoint = i;
return STOP;
}
// without deceleration range
if (_deceleration_range < 0.01)
continue;
// deceleration search runs "decelerate_search_distance" waypoints from closest
if (i > _closest_waypoint+_deceleration_search_distance || decelerate_or_stop >= 0)
continue;
// ---DECELERATE OBSTACLE DETECTION---
if (dt > _detection_range && dt < _detection_range + _deceleration_range) {
bool count_flag = true;
// search overlaps between DETECTION range and DECELERATION range
for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++) {
if (i+waypoint_search < 0 || i+waypoint_search >= _path_dk.getSize() || !waypoint_search)
continue;
geometry_msgs::Point temp_waypoint = calcRelativeCoordinate(
_path_dk.getWaypointPosition(i+waypoint_search),
_current_pose.pose);
tf::Vector3 waypoint_vector = point2vector(temp_waypoint);
waypoint_vector.setZ(0);
// if there is a overlap, give priority to DETECTION range
if (tf::tfDistance(vscan_vector, waypoint_vector) < _detection_range) {
count_flag = false;
break;
}
}
if (count_flag) {
decelerate_point_count++;
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
if (g_sim_mode)
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _sim_ndt_pose.pose));
else
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, _current_pose.pose));
}
}
// found obstacle to DECELERATE
if (decelerate_point_count > _threshold_points) {
_obstacle_waypoint = i;
decelerate_or_stop = 0; // for searching near STOP obstacle
g_obstacle.setDecided(true);
}
}
}
return KEEP; //no obstacles
}
static void Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
pcl::fromROSMsg(*msg, _vscan);
// int i = 0;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = _vscan.begin(); item != _vscan.end(); item++) {
if ((item->x == 0 && item->y == 0))
continue;
// if (i < _loop_limit) {
// std::cout << "vscan_points : ( " << item->x << " , " << item->y << " , " << item->z << " )" << std::endl;
geometry_msgs::Point point;
point.x = item->x;
point.y = item->y;
point.z = item->z;
_linelist.points.push_back(point);
//}else{
// break;
// }
//i++;
}
// std::cout << "i = " << i << std::endl;
_pub.publish(_linelist);
_linelist.points.clear();
}
// Separate into separate clouds and publish polygons
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > // use jsk_recognition_msgs::PointsArray
separate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot, std_msgs::Header header) {
double x_pitch = 0.25, x_min = 1.0, x_max = 3.0; // 1.5~1.75 1.75~2.00 1.5~1.675
double y_min = -0.75, y_max = 0.75;
double z_min = -0.250, z_1 = 0.000, z_2 = 1.000, z_max = 1.750; // -0.3125, 2.0
pcl::PointXYZ pt_1, pt_2, pt_3, pt_4, pt_5, pt_6; // deprecate with polygon
// Divide large cloud
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > cloud_vector;
// pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// pcl::PointXYZ tmp_p;
jsk_recognition_msgs::PolygonArray polygon_array;
polygon_array.header = header;
for (int i = 0; i < (int)( (x_max - x_min) / x_pitch ); i++) {
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
geometry_msgs::PolygonStamped polygon;
visualization_msgs::Marker texts, line_strip; // TEXT_VIEW_FACING
texts.header = header;
texts.ns = "text"; // namespace + ID
texts.action = visualization_msgs::Marker::ADD;
texts.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
texts.pose.orientation.x = 0.0;
texts.pose.orientation.y = 0.0;
texts.pose.orientation.z = 0.0;
texts.pose.orientation.w = 1.0;
texts.scale.x = 0.125;
texts.scale.y = 0.125;
texts.scale.z = 0.125;
texts.color.r = 1.0f;
texts.color.g = 0.0f;
texts.color.b = 0.0f;
texts.color.a = 1.0;
geometry_msgs::Point32 tmp_p_up_0, tmp_p_up_1, tmp_p_up_2, tmp_p_down_0, tmp_p_down_1, tmp_p_down_2;
pcl::PointXYZ tmp_p;
double width_tmp, width_min_up = 2.000, width_min_down = 4.000;
double width_min_bottom = 0.500, width_min_top = 0.200;
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator itr = cloud_xyz_rot->begin();
itr != cloud_xyz_rot->end(); itr++) {
if ( (x_min + i*x_pitch) < itr->x && itr->x < (x_min + (i+1)*x_pitch) ) {
if (y_min < itr->y && itr->y < y_max) {
if (z_min < itr->z && itr->z < z_max) {
// compare tmp_p and itr, and calculate width and points
if (itr != cloud_xyz_rot->begin()) { // skip at 1st time
if ( (tmp_p.y < 0 && 0 <= itr->y) || (itr->y < 0 && 0 <= tmp_p.y) ) {
if (itr->z < z_1) {
width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
+ pow(fabs(tmp_p.y - itr->y), 2)
+ pow(fabs(tmp_p.z - itr->z), 2));
if (width_min_bottom < width_tmp && width_tmp <= width_min_down) {
width_min_down = width_tmp; // create width_min array
tmp_p_down_0.x = tmp_p.x; tmp_p_down_0.y = tmp_p.y;
tmp_p_down_0.z = (tmp_p.z + itr->z) / 2;
tmp_p_down_1.x = itr->x; tmp_p_down_1.y = itr->y;
tmp_p_down_1.z = (tmp_p.z + itr->z) / 2;
tmp_p_down_2.x = tmp_p.x; // ignore adding sqrt
tmp_p_down_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
tmp_p_down_2.z = (tmp_p.z + itr->z) / 2;
}
}
if (z_2 < itr->z) {
width_tmp = sqrt(pow(fabs(tmp_p.x - itr->x), 2)
+ pow(fabs(tmp_p.y - itr->y), 2)
+ pow(fabs(tmp_p.z - itr->z), 2));
if (width_tmp <= width_min_down) {
width_min_up = width_tmp;
tmp_p_up_0.x = tmp_p.x; tmp_p_up_0.y = tmp_p.y;
tmp_p_up_0.z = (tmp_p.z + itr->z) / 2;
tmp_p_up_1.x = itr->x; tmp_p_up_1.y = itr->y;
tmp_p_up_1.z = (tmp_p.z + itr->z) / 2;
tmp_p_up_2.x = tmp_p.x; // ignore adding sqrt
tmp_p_up_2.y = tmp_p.y + sqrt(pow(fabs(tmp_p.y - itr->y), 2)) / 2;
tmp_p_up_2.z = (tmp_p.z + itr->z) / 2;
}
}
}
tmp_p.x = itr->x; tmp_p.y = itr->y; tmp_p.z = itr->z;
tmp_cloud->points.push_back(tmp_p);
}
}
}
}
// From tmp_cloud, get 4 points to publish marker
// Create polygon
}
cloud_vector.push_back(tmp_cloud);
tmp_p_up_0.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_up_1.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_down_0.x = x_min + i*x_pitch - x_pitch/2;
tmp_p_down_1.x = x_min + i*x_pitch - x_pitch/2;
if (tmp_p_up_0.y < tmp_p_up_1.y) {
polygon.polygon.points.push_back(tmp_p_up_0);
polygon.polygon.points.push_back(tmp_p_up_1);
}
if (tmp_p_up_0.y >= tmp_p_up_1.y) {
polygon.polygon.points.push_back(tmp_p_up_1);
polygon.polygon.points.push_back(tmp_p_up_0);
}
if (tmp_p_down_0.y < tmp_p_down_1.y) {
polygon.polygon.points.push_back(tmp_p_down_1);
polygon.polygon.points.push_back(tmp_p_down_0);
}
if (tmp_p_down_0.y >= tmp_p_down_1.y) {
polygon.polygon.points.push_back(tmp_p_down_0);
polygon.polygon.points.push_back(tmp_p_down_1);
}
polygon.header = header;
polygon_array.polygons.push_back(polygon);
std::cerr << "count:" << i << ", " << "size:" << cloud_vector.at(i)->size() << std::endl;
std::cerr << "width_min_up:" << width_min_up << std::endl;
std::cerr << "width_min_down:" << width_min_down << std::endl;
texts.id = 2*i;
texts.pose.position.x = tmp_p_up_0.x;
texts.pose.position.y = tmp_p_up_2.y;
texts.pose.position.z = tmp_p_up_2.z;
std::ostringstream strs;
strs << width_min_up;
std::string str = strs.str();
texts.text = str;
pub_marker.publish(texts);
texts.id = 2*i + 1;
texts.pose.position.x = tmp_p_down_0.x;
texts.pose.position.y = tmp_p_down_2.y;
texts.pose.position.z = tmp_p_down_2.z;
strs.str("");
strs.clear(std::stringstream::goodbit);
strs << width_min_down;
str = strs.str();
texts.text = str;
pub_marker.publish(texts);
}
pub_polygon_array.publish(polygon_array); // error
return cloud_vector;
}
bool ObjRecInterface::recognize_objects(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud_full,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
boost::shared_ptr<pcl::VoxelGrid<pcl::PointXYZRGB> > &voxel_grid,
pcl::ModelCoefficients::Ptr &coefficients,
pcl::PointIndices::Ptr &inliers,
pcl::PointIndices::Ptr &outliers,
vtkSmartPointer<vtkPoints> &foreground_points,
std::list<boost::shared_ptr<PointSetShape> > &detected_models,
bool downsample,
bool segment_plane)
{
// Downsample cloud
{
ROS_DEBUG_STREAM("ObjRec: Downsampling full cloud from "<<cloud_full->points.size()<<" points...");
voxel_grid->setLeafSize(
downsample_voxel_size_,
downsample_voxel_size_,
downsample_voxel_size_);
voxel_grid->setInputCloud(cloud_full);
voxel_grid->filter(*cloud);
ROS_DEBUG_STREAM("ObjRec: Downsampled cloud has "<<cloud->points.size()<<" points.");
}
// Remove plane points
if(use_only_points_above_plane_)
{
ROS_DEBUG("ObjRec: Removing points not above plane with PCL...");
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_ERROR_STREAM("Could not estimate a planar model for the given dataset.");
return false;
}
ROS_DEBUG_STREAM("Objrec: found plane with "<<inliers->indices.size()<<" points");
// Flip plane if it's pointing away
if(coefficients->values[2] > 0.0) {
coefficients->values[0] *= -1.0;
coefficients->values[1] *= -1.0;
coefficients->values[2] *= -1.0;
coefficients->values[3] *= -1.0;
}
// Remove the plane points and extract the rest
// TODO: Is this double work??
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*cloud);
ROS_DEBUG_STREAM("Objrec: extracted "<<cloud->points.size()<<" foreground points");
// Fill the foreground cloud
foreground_points->SetNumberOfPoints(cloud->points.size());
foreground_points->Reset();
// Require the points are inside of the clopping box
for (pcl::PointCloud<pcl::PointXYZRGB>::const_iterator it = cloud->begin();
it != cloud->end();
++it)
{
const double dist =
it->x * coefficients->values[0] +
it->y * coefficients->values[1] +
it->z * coefficients->values[2] +
coefficients->values[3];
if(dist > plane_thickness_/2.0) {
// Add point if it's above the plane
foreground_points->InsertNextPoint(
it->x,
it->y,
it->z);
}
}
} else {
// Fill the foreground cloud
foreground_points->SetNumberOfPoints(cloud->points.size());
foreground_points->Reset();
// Require the points are inside of the clopping box
for (pcl::PointCloud<pcl::PointXYZRGB>::const_iterator it = cloud->begin();
it != cloud->end();
++it)
{
foreground_points->InsertNextPoint(
it->x,
it->y,
it->z);
}
}
// Detect models
{
ROS_DEBUG_STREAM("ObjRec: Attempting recognition on "<<foreground_points->GetNumberOfPoints()<<" foregeound points...");
detected_models.clear();
int success = objrec_->doRecognition(foreground_points, success_probability_, detected_models);
if(success != 0) {
ROS_ERROR_STREAM("Failed to recognize anything!");
}
ROS_DEBUG("ObjRec: Seconds elapsed = %.2lf", objrec_->getLastOverallRecognitionTimeSec());
ROS_DEBUG("ObjRec: Seconds per hypothesis = %.6lf", objrec_->getLastOverallRecognitionTimeSec()
/ (double) objrec_->getLastNumberOfCheckedHypotheses());
}
return true;
}
