depth_camera_frustum.cpp
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  *********************************************************************/
37 
38 #include <spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp>
39 
40 namespace geometry
41 {
42 
43 /*****************************************************************************/
44 DepthCameraFrustum::DepthCameraFrustum(const double& vFOV, const double& hFOV,
45  const double& min_dist, const double& max_dist) :
46  _vFOV(vFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist)
47 /*****************************************************************************/
48 {
49  _valid_frustum = false;
50  ros::NodeHandle nh;
51  #if VISUALIZE_FRUSTUM
52  _frustumPub = nh.advertise<visualization_msgs::MarkerArray>("/frustum", 1);
53  // give enough time for publisher to register, don't use in production.
54  ros::Duration(0.5).sleep();
55  #endif
56 
57  this->ComputePlaneNormals();
58 }
59 
60 /*****************************************************************************/
61 DepthCameraFrustum::~DepthCameraFrustum(void)
62 /*****************************************************************************/
63 {
64 }
65 
66 /*****************************************************************************/
67 void DepthCameraFrustum::ComputePlaneNormals(void)
68 /*****************************************************************************/
69 {
70  // give ability to construct with bogus values
71  if (_vFOV == 0 && _hFOV == 0)
72  {
73  _valid_frustum = false;
74  return;
75  }
76 
77  // Z vector and deflected vector capture
78  std::vector<Eigen::Vector3d> deflected_vecs;
79  deflected_vecs.reserve(4);
80  Eigen::Vector3d Z = Eigen::Vector3d::UnitZ();
81 
82  // rotate going CCW
83  Eigen::Affine3d rx =
84  Eigen::Affine3d(Eigen::AngleAxisd(_vFOV/2.,Eigen::Vector3d::UnitX()));
85  Eigen::Affine3d ry =
86  Eigen::Affine3d(Eigen::AngleAxisd(_hFOV/2.,Eigen::Vector3d::UnitY()));
87  deflected_vecs.push_back(rx * ry * Z);
88 
89  rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vFOV/2.,Eigen::Vector3d::UnitX()));
90  deflected_vecs.push_back(rx * ry * Z);
91 
92  ry = Eigen::Affine3d(Eigen::AngleAxisd(-_hFOV/2.,Eigen::Vector3d::UnitY()));
93  deflected_vecs.push_back(rx * ry * Z);
94 
95  rx = Eigen::Affine3d(Eigen::AngleAxisd( _vFOV/2.,Eigen::Vector3d::UnitX()));
96  deflected_vecs.push_back(rx * ry * Z);
97 
98  // get and store CCW 4 corners for each 2 planes at ends
99  std::vector<Eigen::Vector3d> pt_;
100  pt_.reserve(2*deflected_vecs.size());
101  std::vector<Eigen::Vector3d>::iterator it;
102  for (it = deflected_vecs.begin(); it != deflected_vecs.end(); ++it)
103  {
104  pt_.push_back(*(it) * _min_d);
105  pt_.push_back(*(it) * _max_d);
106  }
107 
108  assert(pt_.size() == 8);
109 
110  // cross each plane and get normals
111  const Eigen::Vector3d v_01(pt_[1][0]-pt_[0][0], \
112  pt_[1][1]-pt_[0][1], pt_[1][2]-pt_[0][2]);
113  const Eigen::Vector3d v_13(pt_[3][0]-pt_[1][0], \
114  pt_[3][1]-pt_[1][1], pt_[3][2]-pt_[1][2]);
115  Eigen::Vector3d T_n(v_13.cross(v_01));
116  T_n.normalize();
117  _plane_normals.push_back(VectorWithPt3D(T_n[0],T_n[1],T_n[2],pt_[0]));
118 
119  const Eigen::Vector3d v_23(pt_[3][0]-pt_[2][0], \
120  pt_[3][1]-pt_[2][1], pt_[3][2]-pt_[2][2]);
121  const Eigen::Vector3d v_35(pt_[5][0]-pt_[3][0], \
122  pt_[5][1]-pt_[3][1], pt_[5][2]-pt_[3][2]);
123  Eigen::Vector3d T_l(v_35.cross(v_23));
124  T_l.normalize();
125  _plane_normals.push_back(VectorWithPt3D(T_l[0],T_l[1],T_l[2],pt_[2]));
126 
127  const Eigen::Vector3d v_45(pt_[5][0]-pt_[4][0], \
128  pt_[5][1]-pt_[4][1], pt_[5][2]-pt_[4][2]);
129  const Eigen::Vector3d v_57(pt_[7][0]-pt_[5][0], \
130  pt_[7][1]-pt_[5][1], pt_[7][2]-pt_[5][2]);
131  Eigen::Vector3d T_b(v_57.cross(v_45));
132  T_b.normalize();
133  _plane_normals.push_back(VectorWithPt3D(T_b[0],T_b[1],T_b[2],pt_[4]));
134 
135  const Eigen::Vector3d v_67(pt_[7][0]-pt_[6][0], \
136  pt_[7][1]-pt_[6][1], pt_[7][2]-pt_[6][2]);
137  const Eigen::Vector3d v_71(pt_[1][0]-pt_[7][0], \
138  pt_[1][1]-pt_[7][1], pt_[1][2]-pt_[7][2]);
139  Eigen::Vector3d T_r(v_71.cross(v_67));
140  T_r.normalize();
141  _plane_normals.push_back(VectorWithPt3D(T_r[0],T_r[1],T_r[2],pt_[6]));
142 
143  // far plane
144  Eigen::Vector3d T_1(v_57.cross(v_71));
145  T_1.normalize();
146  _plane_normals.push_back(VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[7]));
147 
148  // near plane
149  _plane_normals.push_back(VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[2]) * -1);
150 
151  #if VISUALIZE_FRUSTUM
152  _frustum_pts = pt_;
153  #endif
154 
155  assert(_plane_normals.size() == 6);
156  _valid_frustum = true;
157  return;
158 }
159 
160 /*****************************************************************************/
161 void DepthCameraFrustum::TransformModel(void)
162 /*****************************************************************************/
163 {
164  if (!_valid_frustum)
165  {
166  return;
167  }
168 
169  Eigen::Affine3d T = Eigen::Affine3d::Identity();
170  T.pretranslate(_orientation.inverse()*_position);
171  T.prerotate(_orientation);
172 
173  std::vector<VectorWithPt3D>::iterator it;
174  for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
175  {
176  it->TransformFrames(T);
177  }
178 
179  #if VISUALIZE_FRUSTUM
180  visualization_msgs::MarkerArray msg_list;
181  visualization_msgs::Marker msg;
182  for (uint i = 0; i != _frustum_pts.size(); i++)
183  {
184  // frustum pts
185  msg.header.frame_id = std::string("map");
186  msg.type = visualization_msgs::Marker::SPHERE;
187  msg.action = visualization_msgs::Marker::ADD;
188  msg.scale.x = 0.15;
189  msg.scale.y = 0.15;
190  msg.scale.z = 0.15;
191  msg.pose.orientation.w = 1.0;
192  msg.header.stamp = ros::Time::now();
193  msg.ns = "pt_" + std::to_string(i);
194  msg.color.g = 1.0f;
195  msg.color.a = 1.0;
196  Eigen::Vector3d T_pt = T*_frustum_pts.at(i);
197  geometry_msgs::Pose pnt;
198  pnt.position.x = T_pt[0];
199  pnt.position.y = T_pt[1];
200  pnt.position.z = T_pt[2];
201  pnt.orientation.w = 1;
202  msg.pose = pnt;
203  msg_list.markers.push_back(msg);
204 
205  // point numbers
206  msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
207  msg.ns = std::to_string(i);
208  msg.pose.position.z+=0.15;
209  msg.text = std::to_string(i);
210  msg_list.markers.push_back(msg);
211  }
212 
213  // frustum lines
214  msg.header.frame_id = std::string("map");
215  msg.type = visualization_msgs::Marker::LINE_STRIP;
216  msg.scale.x = 0.15;
217  msg.scale.y = 0.15;
218  msg.scale.z = 0.15;
219  msg.pose.orientation.w = 1.0;
220  msg.pose.position.x = 0;
221  msg.pose.position.y = 0;
222  msg.pose.position.z = 0;
223  msg.header.stamp = ros::Time::now();
224  msg.color.g = 1.0f;
225  msg.color.a = 1.0;
226 
227  // annoying but only evaluates once
228  const static std::vector<int> v1 = {0, 2};
229  const static std::vector<int> v2 = {2, 4};
230  const static std::vector<int> v3 = {4, 6};
231  const static std::vector<int> v4 = {6, 0};
232  const static std::vector<int> v5 = {1, 3};
233  const static std::vector<int> v6 = {3, 5};
234  const static std::vector<int> v7 = {5, 7};
235  const static std::vector<int> v8 = {7, 1};
236  const static std::vector<int> v9 = {0, 1};
237  const static std::vector<int> v10 = {2, 3};
238  const static std::vector<int> v11 = {4, 5};
239  const static std::vector<int> v12 = {6, 7};
240  const static std::vector<std::vector<int> > v_t = \
241  {v1, v2, v3, v4, v5, v6, v7, v8, v9, v10, v11, v12};
242 
243  for (uint i=0; i != v_t.size(); i++)
244  {
245  // frustum lines
246  msg.ns = "line_" + std::to_string(i);
247  msg.points.clear();
248 
249  for (uint j=0; j!= v_t[i].size(); j++)
250  {
251  Eigen::Vector3d T_pt = T*_frustum_pts.at(v_t[i][j]);
252  geometry_msgs::Point point;
253  point.x = T_pt[0];
254  point.y = T_pt[1];
255  point.z = T_pt[2];
256  msg.points.push_back(point);
257  }
258  msg_list.markers.push_back(msg);
259  }
260 
261  for (uint i = 0; i != _plane_normals.size(); i++)
262  {
263  // normal vectors
264  msg.pose.position.z -= 0.15;
265  msg.type = visualization_msgs::Marker::ARROW;
266  msg.ns = "normal_" + std::to_string(i);
267  msg.scale.y = 0.07;
268  msg.scale.z = 0.07;
269  msg.scale.x = 1;
270  msg.color.g = 1.0f;
271  const VectorWithPt3D nml = _plane_normals.at(i);
272  msg.pose.position.x = nml.initial_point[0];
273  msg.pose.position.y = nml.initial_point[1];
274  msg.pose.position.z = nml.initial_point[2];
275 
276  // turn unit vector into a quaternion
277  const Eigen::Quaterniond quat =
278  Eigen::Quaterniond::FromTwoVectors( Eigen::Vector3d::UnitX(), \
279  Eigen::Vector3d(nml.x, nml.y, nml.z) );
280  msg.pose.orientation.x = quat.x();
281  msg.pose.orientation.y = quat.y();
282  msg.pose.orientation.z = quat.z();
283  msg.pose.orientation.w = quat.w();
284 
285  msg_list.markers.push_back(msg);
286  }
287  _frustumPub.publish(msg_list);
288  #endif
289 }
290 
291 /*****************************************************************************/
292 bool DepthCameraFrustum::IsInside(const openvdb::Vec3d& pt)
293 /*****************************************************************************/
294 {
295  if (!_valid_frustum)
296  {
297  return false;
298  }
299 
300  std::vector<VectorWithPt3D>::iterator it;
301  for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
302  {
303  Eigen::Vector3d p_delta(pt[0] - it->initial_point[0], \
304  pt[1] - it->initial_point[1], \
305  pt[2] - it->initial_point[2]);
306  p_delta.normalize();
307 
308  if (Dot(*it, p_delta) > 0.)
309  {
310  return false;
311  }
312  }
313  return true;
314 }
315 
316 /*****************************************************************************/
317 void DepthCameraFrustum::SetPosition(const geometry_msgs::Point& origin)
318 /*****************************************************************************/
319 {
320  _position = Eigen::Vector3d(origin.x, origin.y, origin.z);
321 }
322 
323 /*****************************************************************************/
324 void DepthCameraFrustum::SetOrientation(const geometry_msgs::Quaternion& quat)
325 /*****************************************************************************/
326 {
327  _orientation = Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z);
328 }
329 
330 /*****************************************************************************/
331 double DepthCameraFrustum::Dot(const VectorWithPt3D& plane_pt, \
332  const openvdb::Vec3d& query_pt) const
333 /*****************************************************************************/
334 {
335  return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];
336 }
337 
338 /*****************************************************************************/
339 double DepthCameraFrustum::Dot(const VectorWithPt3D& plane_pt, \
340  const Eigen::Vector3d& query_pt) const
341 /*****************************************************************************/
342 {
343  return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];
344 }
345 
346 } // end namespace
347 
geometry::VectorWithPt3D
Definition: frustum.hpp:99
geometry
Definition: depth_camera_frustum.hpp:46
msg_list
def msg_list(pkg, search_path, ext)
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
geometry::DepthCameraFrustum::DepthCameraFrustum
DepthCameraFrustum(const double &vFOV, const double &hFOV, const double &min_dist, const double &max_dist)
Definition: depth_camera_frustum.cpp:79
depth_camera_frustum.hpp
ros::Duration::sleep
bool sleep() const
ros::Duration
ros::NodeHandle
ros::Time::now
static Time now()

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