depth_camera_frustum.hpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2018, Simbe Robotics, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Simbe Robotics, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: Steve Macenski (steven.macenski@simberobotics.com)
36  * Purpose: Structure for handling camera FOVs to construct frustums
37  * and associated methods
38  *********************************************************************/
39 
40 #ifndef DEPTH_FRUSTUM_H_
41 #define DEPTH_FRUSTUM_H_
42 
43 // STVL
45 
46 namespace geometry
47 {
48 
49 // visualize the frustum should someone other than me care
50 #define VISUALIZE_FRUSTUM 0
51 
52 // A class to model a depth sensor frustum in world space
53 class DepthCameraFrustum : public Frustum
54 {
55 public:
56  DepthCameraFrustum(const double& vFOV, const double& hFOV,
57  const double& min_dist, const double& max_dist);
58  virtual ~DepthCameraFrustum(void);
59 
60  // transform plane normals by depth camera pose
61  virtual void TransformModel(void);
62 
63  // determine if a point is inside of the transformed frustum
64  virtual bool IsInside(const openvdb::Vec3d& pt);
65 
66  // set pose of depth camera in global space
67  virtual void SetPosition(const geometry_msgs::Point& origin);
68  virtual void SetOrientation(const geometry_msgs::Quaternion& quat);
69 
70 private:
71  // utils to find useful frustum metadata
72  void ComputePlaneNormals(void);
73  double Dot(const VectorWithPt3D&, const openvdb::Vec3d&) const;
74  double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const;
75 
76  double _vFOV, _hFOV, _min_d, _max_d;
77  std::vector<VectorWithPt3D> _plane_normals;
78  Eigen::Vector3d _position;
79  Eigen::Quaterniond _orientation;
80  bool _valid_frustum;
81 
82  #if VISUALIZE_FRUSTUM
83  std::vector<Eigen::Vector3d> _frustum_pts;
84  ros::Publisher _frustumPub;
85  #endif
86 };
87 
88 } // end namespace
89 
90 #endif
ros::Publisher
geometry::VectorWithPt3D
Definition: frustum.hpp:99
geometry
Definition: depth_camera_frustum.hpp:46
geometry::DepthCameraFrustum
Definition: depth_camera_frustum.hpp:90
geometry::DepthCameraFrustum::_valid_frustum
bool _valid_frustum
Definition: depth_camera_frustum.hpp:117
geometry::DepthCameraFrustum::_vFOV
double _vFOV
Definition: depth_camera_frustum.hpp:113
geometry::DepthCameraFrustum::ComputePlaneNormals
void ComputePlaneNormals(void)
Definition: depth_camera_frustum.cpp:102
geometry::DepthCameraFrustum::_min_d
double _min_d
Definition: depth_camera_frustum.hpp:113
geometry::DepthCameraFrustum::DepthCameraFrustum
DepthCameraFrustum(const double &vFOV, const double &hFOV, const double &min_dist, const double &max_dist)
Definition: depth_camera_frustum.cpp:79
geometry::DepthCameraFrustum::SetOrientation
virtual void SetOrientation(const geometry_msgs::Quaternion &quat)
Definition: depth_camera_frustum.cpp:359
frustum.hpp
geometry::DepthCameraFrustum::_hFOV
double _hFOV
Definition: depth_camera_frustum.hpp:113
geometry::DepthCameraFrustum::Dot
double Dot(const VectorWithPt3D &, const openvdb::Vec3d &) const
Definition: depth_camera_frustum.cpp:366
geometry::DepthCameraFrustum::_max_d
double _max_d
Definition: depth_camera_frustum.hpp:113
geometry::DepthCameraFrustum::_orientation
Eigen::Quaterniond _orientation
Definition: depth_camera_frustum.hpp:116
geometry::Frustum
Definition: frustum.hpp:131
geometry::DepthCameraFrustum::~DepthCameraFrustum
virtual ~DepthCameraFrustum(void)
Definition: depth_camera_frustum.cpp:96
geometry::DepthCameraFrustum::IsInside
virtual bool IsInside(const openvdb::Vec3d &pt)
Definition: depth_camera_frustum.cpp:327
geometry::DepthCameraFrustum::SetPosition
virtual void SetPosition(const geometry_msgs::Point &origin)
Definition: depth_camera_frustum.cpp:352
geometry::DepthCameraFrustum::_position
Eigen::Vector3d _position
Definition: depth_camera_frustum.hpp:115
geometry::DepthCameraFrustum::TransformModel
virtual void TransformModel(void)
Definition: depth_camera_frustum.cpp:196
geometry::DepthCameraFrustum::_plane_normals
std::vector< VectorWithPt3D > _plane_normals
Definition: depth_camera_frustum.hpp:114


spatio_temporal_voxel_layer
Author(s):
autogenerated on Wed Aug 16 2023 02:10:02