45 const double& min_dist,
const double& max_dist) :
46 _vFOV(vFOV), _hFOV(hFOV), _min_d(min_dist), _max_d(max_dist)
49 _valid_frustum =
false;
52 _frustumPub = nh.
advertise<visualization_msgs::MarkerArray>(
"/frustum", 1);
57 this->ComputePlaneNormals();
61 DepthCameraFrustum::~DepthCameraFrustum(
void)
67 void DepthCameraFrustum::ComputePlaneNormals(
void)
71 if (_vFOV == 0 && _hFOV == 0)
73 _valid_frustum =
false;
78 std::vector<Eigen::Vector3d> deflected_vecs;
79 deflected_vecs.reserve(4);
80 Eigen::Vector3d Z = Eigen::Vector3d::UnitZ();
84 Eigen::Affine3d(Eigen::AngleAxisd(_vFOV/2.,Eigen::Vector3d::UnitX()));
86 Eigen::Affine3d(Eigen::AngleAxisd(_hFOV/2.,Eigen::Vector3d::UnitY()));
87 deflected_vecs.push_back(rx * ry * Z);
89 rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vFOV/2.,Eigen::Vector3d::UnitX()));
90 deflected_vecs.push_back(rx * ry * Z);
92 ry = Eigen::Affine3d(Eigen::AngleAxisd(-_hFOV/2.,Eigen::Vector3d::UnitY()));
93 deflected_vecs.push_back(rx * ry * Z);
95 rx = Eigen::Affine3d(Eigen::AngleAxisd( _vFOV/2.,Eigen::Vector3d::UnitX()));
96 deflected_vecs.push_back(rx * ry * Z);
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)
104 pt_.push_back(*(it) * _min_d);
105 pt_.push_back(*(it) * _max_d);
108 assert(pt_.size() == 8);
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));
117 _plane_normals.push_back(
VectorWithPt3D(T_n[0],T_n[1],T_n[2],pt_[0]));
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));
125 _plane_normals.push_back(
VectorWithPt3D(T_l[0],T_l[1],T_l[2],pt_[2]));
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));
133 _plane_normals.push_back(
VectorWithPt3D(T_b[0],T_b[1],T_b[2],pt_[4]));
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));
141 _plane_normals.push_back(
VectorWithPt3D(T_r[0],T_r[1],T_r[2],pt_[6]));
144 Eigen::Vector3d T_1(v_57.cross(v_71));
146 _plane_normals.push_back(
VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[7]));
149 _plane_normals.push_back(
VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[2]) * -1);
151 #if VISUALIZE_FRUSTUM
155 assert(_plane_normals.size() == 6);
156 _valid_frustum =
true;
161 void DepthCameraFrustum::TransformModel(
void)
169 Eigen::Affine3d T = Eigen::Affine3d::Identity();
170 T.pretranslate(_orientation.inverse()*_position);
171 T.prerotate(_orientation);
173 std::vector<VectorWithPt3D>::iterator it;
174 for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
176 it->TransformFrames(T);
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++)
185 msg.header.frame_id = std::string(
"map");
186 msg.type = visualization_msgs::Marker::SPHERE;
187 msg.action = visualization_msgs::Marker::ADD;
191 msg.pose.orientation.w = 1.0;
193 msg.ns =
"pt_" + std::to_string(i);
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;
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);
214 msg.header.frame_id = std::string(
"map");
215 msg.type = visualization_msgs::Marker::LINE_STRIP;
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;
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};
243 for (uint i=0; i != v_t.size(); i++)
246 msg.ns =
"line_" + std::to_string(i);
249 for (uint j=0; j!= v_t[i].size(); j++)
251 Eigen::Vector3d T_pt = T*_frustum_pts.at(v_t[i][j]);
252 geometry_msgs::Point point;
256 msg.points.push_back(point);
261 for (uint i = 0; i != _plane_normals.size(); i++)
264 msg.pose.position.z -= 0.15;
265 msg.type = visualization_msgs::Marker::ARROW;
266 msg.ns =
"normal_" + std::to_string(i);
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];
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();
287 _frustumPub.publish(msg_list);
292 bool DepthCameraFrustum::IsInside(
const openvdb::Vec3d& pt)
300 std::vector<VectorWithPt3D>::iterator it;
301 for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
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]);
308 if (Dot(*it, p_delta) > 0.)
317 void DepthCameraFrustum::SetPosition(
const geometry_msgs::Point& origin)
320 _position = Eigen::Vector3d(origin.x, origin.y, origin.z);
324 void DepthCameraFrustum::SetOrientation(
const geometry_msgs::Quaternion& quat)
327 _orientation = Eigen::Quaterniond(quat.w, quat.x, quat.y, quat.z);
332 const openvdb::Vec3d& query_pt)
const
335 return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];
339 double DepthCameraFrustum::Dot(
const VectorWithPt3D& plane_pt, \
340 const Eigen::Vector3d& query_pt)
const
343 return plane_pt.x*query_pt[0]+plane_pt.y*query_pt[1]+plane_pt.z*query_pt[2];