Ejemplo n.º 1
0
bool FloorFilter::IntersectWithSightline(
    const ros::Time &time, const Eigen::ParametrizedLine<float, 3> &line, const pcl::PointXYZ &point,
    pcl::PointXYZ *intersection_point) {
  pcl::PointXYZ viewpoint_pcl;
  if (!GetViewpointPoint(time, &viewpoint_pcl)) {
    return false;
  }
  Eigen::ParametrizedLine<float, 3>::VectorType viewpoint = viewpoint_pcl.getVector3fMap();
  Eigen::ParametrizedLine<float, 3> viewpoint_line(viewpoint, point.getVector3fMap() - viewpoint);
  Eigen::ParametrizedLine<float, 3>::VectorType intersection;
  if (geometry::IntersectLines(viewpoint_line, line, &intersection)) {
    *intersection_point = pcl::PointXYZ(intersection[0], intersection[1], intersection[2]);
    // If the point is actually closer to the viewpoint than the
    // intersection point we have no real intersection because the
    // point is above the floor.
    if (pcl::euclideanDistance(viewpoint_pcl, *intersection_point) >
        pcl::euclideanDistance(viewpoint_pcl, point)) {
      return false;
    }
    // Check if the intersection point and point are on the same side
    // of viewpoint.
    if ((intersection - viewpoint).dot(point.getVector3fMap() - viewpoint) < 0) {
      return false;
    }
    return true;      
  }
  return false;
}
  TPPLPoint MsgToPoint2D(const pcl::PointXYZ &point, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message) {
    TPPLPoint pt;

    if(new_message->params.size()==4) {
      Eigen::Vector3f u,v,normal,origin;
      Eigen::Affine3f transformation;
      normal(0)=new_message->params[0];
      normal(1)=new_message->params[1];
      normal(2)=new_message->params[2];
      origin(0)=new_message->centroid.x;
      origin(1)=new_message->centroid.y;
      origin(2)=new_message->centroid.z;
      //std::cout << "normal: " << normal << std::endl;
      //std::cout << "centroid: " << origin << std::endl;
      v = normal.unitOrthogonal ();
      u = normal.cross (v);
      pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal,  origin, transformation);

      Eigen::Vector3f p3 = transformation*point.getVector3fMap();
      pt.x = p3(0);
      pt.y = p3(1);
    }
    else if(new_message->params.size()==5) {
      pt.x=point.x;
      pt.y=point.y;
    }

    return pt;
  }
Ejemplo n.º 3
0
 bool FootstepGraph::isCollidingBox(const Eigen::Affine3f& c, pcl::PointIndices::Ptr candidates) const
 {
   const pcl::PointCloud<pcl::PointXYZ>::ConstPtr input_cloud = obstacle_tree_model_->getInputCloud();
   Eigen::Affine3f inv_c = c.inverse();
   for (size_t i = 0; i < candidates->indices.size(); i++) {
     int index = candidates->indices[i];
     const pcl::PointXYZ candidate_point = input_cloud->points[index];
     // convert candidate_point into `c' local representation.
     const Eigen::Vector3f local_p = inv_c * candidate_point.getVector3fMap();
     if (std::abs(local_p[0]) < collision_bbox_size_[0] / 2 &&
         std::abs(local_p[1]) < collision_bbox_size_[1] / 2 &&
         std::abs(local_p[2]) < collision_bbox_size_[2] / 2) {
       return true;
     }
   }
   return false;
 }
TPPLPoint
ShapeMarker::msgToPoint2D (const pcl::PointXYZ &point)
{
  //ROS_INFO(" transform 3D point to 2D ");
  TPPLPoint pt;
  Eigen::Vector3f p = transformation_ * point.getVector3fMap ();
  pt.x = p (0);
  pt.y = p (1);
  //std::cout << "\n transformed point : \n" << p << std::endl;
  return pt;
}
Ejemplo n.º 5
0
	bool determine_normal_of_point_cloud_at_clicked_point() {
		visualization_msgs::Marker marker;

		pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
		kdtree.setInputCloud(pcl_input_point_cloud);

		//transform selected point from robot frame (BASE_LINK) to Kinect frame (/kinect_rgb_optical_frame)
		tf::Stamped<tf::Vector3> searchPointInRobotFrame;

		tf::pointStampedMsgToTF(desired_pickup_point, searchPointInRobotFrame);

		tf::StampedTransform transformRobotToPointCloud;

		try {
			tf_listener->lookupTransform(pcl_input_point_cloud->header.frame_id,
					BASE_LINK, ros::Time(0), transformRobotToPointCloud);
		} catch (tf::TransformException& ex) {
			ROS_ERROR("%s", ex.what());
		}

		tf::Vector3 searchPointPointCloudFrame = transformRobotToPointCloud
				* searchPointInRobotFrame;

		pcl::PointXYZ searchPoint;

		searchPoint.x = searchPointPointCloudFrame.getX();
		searchPoint.y = searchPointPointCloudFrame.getY();
		searchPoint.z = searchPointPointCloudFrame.getZ();

		float radius = 0.02;

		ROS_INFO(
				"Search searchPointWorldFrame set to: x: %f, y: %f, z: %f ", searchPoint.x, searchPoint.y, searchPoint.z);

		// Neighbors within radius search

		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;

		ROS_DEBUG_STREAM(
				"Input cloud frame id: " << pcl_input_point_cloud->header.frame_id);

		if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch,
				pointRadiusSquaredDistance) > 5) {
			for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
				//ROS_DEBUG_STREAM(
				//		"   " << cloud->points[pointIdxRadiusSearch[i]].x << " " << cloud->points[pointIdxRadiusSearch[i]].y << " " << cloud->points[pointIdxRadiusSearch[i]].z << " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl);

				marker.pose.position.x =
						pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].x;
				marker.pose.position.y =
						pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].y;
				marker.pose.position.z =
						pcl_input_point_cloud->points[pointIdxRadiusSearch[i]].z;

				//show markers in kinect frame
				marker.header.frame_id = pcl_input_point_cloud->header.frame_id;
				marker.id = i;
				marker.ns = "selection";
				marker.header.stamp = ros::Time::now();
				marker.action = visualization_msgs::Marker::ADD;
				marker.lifetime = ros::Duration();
				marker.type = visualization_msgs::Marker::SPHERE;
				marker.scale.x = 0.003;
				marker.scale.y = 0.003;
				marker.scale.z = 0.003;
				marker.color.r = 1;
				marker.color.g = 0;
				marker.color.b = 0;
				marker.color.a = 1.0;
				vis_marker_publisher.publish(marker);

			}

			ROS_INFO_STREAM(
					"Number of points in radius: " << pointIdxRadiusSearch.size());

			//Determine Normal from points in radius

			Eigen::Vector4f plane_parameters;

			float curvature;

			pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;

			normalEstimator.computePointNormal(*pcl_input_point_cloud,
					pointIdxRadiusSearch, plane_parameters, curvature);

			normal.getVector4fMap() = plane_parameters;

			ROS_INFO(
					"Normal: x: %f, y: %f, z: %f ", normal.x, normal.y, normal.z);
			ROS_DEBUG_STREAM("Normal: " << normal);

			/*
			 tf::Vector3 normalTF(normal.x, normal.y, normal.z);

			 normalTF = transformWorldToPointCloud.inverse() * normalTF;
			 */
			geometry_msgs::PoseStamped normalPosePointCloudFrame;

			normalPosePointCloudFrame.header.frame_id =
					pcl_input_point_cloud->header.frame_id;

			normalPosePointCloudFrame.pose.position.x = searchPoint.x;
			normalPosePointCloudFrame.pose.position.y = searchPoint.y;
			normalPosePointCloudFrame.pose.position.z = searchPoint.z;

			//determine orientation of normal for marker

			btVector3 axis(normal.x, normal.y, normal.z);
			//tf::Vector3 axis(normal.x, normal.y, normal.z);

			btVector3 marker_axis(1, 0, 0);
			//tf::Vector3 marker_axis(1,0,0);

			btQuaternion qt(marker_axis.cross(axis.normalize()),
					marker_axis.angle(axis.normalize()));

			qt.normalize();

			//tf::Quaternion qt2(marker_axis.cross(axis.normalize()),
			//	marker_axis.angle(axis.normalize()));

			/*
			 geometry_msgs::Quaternion quat_msg;
			 tf::quaternionTFToMsg(qt, quat_msg);
			 normalPosePointCloudFrame.pose.orientation = quat_msg;
			 */

			normalPosePointCloudFrame.pose.orientation.x = qt.getX();
			normalPosePointCloudFrame.pose.orientation.y = qt.getY();
			normalPosePointCloudFrame.pose.orientation.z = qt.getZ();
			normalPosePointCloudFrame.pose.orientation.w = qt.getW();

			ROS_DEBUG_STREAM(
					"Pose in Point Cloud Frame: " << normalPosePointCloudFrame.pose);

			//transform normal pose to Katana base

			try {
				tf_listener->transformPose(BASE_LINK, normalPosePointCloudFrame,
						normalPoseRobotFrame);
			} catch (const tf::TransformException &ex) {

				ROS_ERROR("%s", ex.what());

			} catch (const std::exception &ex) {

				ROS_ERROR("%s", ex.what());

			}

			ROS_DEBUG_STREAM(
					"base link frame frame id: " << normalPoseRobotFrame.header.frame_id);

			marker.pose = normalPoseRobotFrame.pose;

			//marker.pose = normalPose.pose;
			marker.header.frame_id = BASE_LINK;
			marker.id = 12345;
			marker.ns = "normal";
			marker.header.stamp = ros::Time::now();
			marker.action = visualization_msgs::Marker::ADD;
			marker.lifetime = ros::Duration();
			marker.type = visualization_msgs::Marker::ARROW;
			marker.scale.x = 0.05;
			marker.scale.y = 0.005;
			marker.scale.z = 0.005;
			marker.color.r = 1;
			marker.color.g = 0;
			marker.color.b = 0;
			marker.color.a = 1.0;
			vis_marker_publisher.publish(marker);

			ROS_DEBUG_STREAM(
					"Nomal pose in base link frame: " << normalPoseRobotFrame.pose);

			return true;

		} else {

			ROS_ERROR(
					"Normal:No Points found inside search radius! Search radios too small?");
			return false;

		}

	}