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