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; }
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; }
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; } }