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