45 const double& vFOVPadding,
47 const double& min_dist,
48 const double& max_dist)
50 _vFOVPadding(vFOVPadding),
87 Eigen::Vector3d point_in_global_frame(pt[0], pt[1], pt[2]);
88 Eigen::Vector3d transformed_pt =
91 const double radial_distance_squared = \
92 (transformed_pt[0] * transformed_pt[0]) + \
93 (transformed_pt[1] * transformed_pt[1]);
103 const double v_padded = fabs(transformed_pt[2]) +
_vFOVPadding;
112 if (transformed_pt[0] > 0)
114 if (fabs(atan(transformed_pt[1] / transformed_pt[0])) >
_hFOVhalf)
120 fabs(atan(transformed_pt[0] / transformed_pt[1])) + M_PI/2 >
_hFOVhalf)
131 geometry_msgs::Point& origin)
134 _position = Eigen::Vector3d(origin.x, origin.y, origin.z);
139 geometry_msgs::Quaternion& quat)
142 _orientation = Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z);
147 const openvdb::Vec3d &query_pt)
const 150 return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];
155 const Eigen::Vector3d &query_pt)
const 158 return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];
ThreeDimensionalLidarFrustum(const double &vFOV, const double &vFOVPadding, const double &hFOV, const double &min_dist, const double &max_dist)
double Dot(const VectorWithPt3D &, const openvdb::Vec3d &) const
virtual void SetPosition(const geometry_msgs::Point &origin)
Eigen::Vector3d _position
Eigen::Quaterniond _orientation
double _tan_vFOVhalf_squared
virtual bool IsInside(const openvdb::Vec3d &pt)
virtual void TransformModel(void)
virtual void SetOrientation(const geometry_msgs::Quaternion &quat)
virtual ~ThreeDimensionalLidarFrustum(void)
Eigen::Quaterniond _orientation_conjugate