00001 00060 //################## 00061 //#### includes #### 00062 #include <cob_3d_fov_segmentation/field_of_view.h> 00063 00064 using namespace cob_3d_mapping; 00065 00066 void 00067 FieldOfView::computeFieldOfView () 00068 { 00069 double fovHorFrac = sensor_fov_hor_ / 2; 00070 double fovVerFrac = sensor_fov_ver_ / 2; 00071 00072 p_1_cam_ (0) = p_0_ (0) + tan (fovHorFrac) * sensor_max_range_; 00073 p_1_cam_ (1) = p_0_ (1) - tan (fovVerFrac) * sensor_max_range_; 00074 p_1_cam_ (2) = p_0_ (2) + sensor_max_range_; 00075 00076 p_2_cam_ (0) = -p_1_cam_ (0); 00077 p_2_cam_ (1) = p_1_cam_ (1); 00078 p_2_cam_ (2) = sensor_max_range_; 00079 00080 p_3_cam_ (0) = -p_1_cam_ (0); 00081 p_3_cam_ (1) = -p_1_cam_ (1); 00082 p_3_cam_ (2) = sensor_max_range_; 00083 00084 p_4_cam_ (0) = p_1_cam_ (0); 00085 p_4_cam_ (1) = -p_1_cam_ (1); 00086 p_4_cam_ (2) = sensor_max_range_; 00087 00088 p_0_ = p_0_cam_; 00089 p_1_ = p_1_cam_; 00090 p_2_ = p_2_cam_; 00091 p_3_ = p_3_cam_; 00092 p_4_ = p_4_cam_; 00093 }