Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

embrace trigonometry (fix also issue #164) #165

Open
wants to merge 2 commits into
base: melodic-devel
Choose a base branch
from
Open

embrace trigonometry (fix also issue #164) #165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
30d92fc
embrace trigonometry (fix also issue #164)
facontidavide Apr 7, 2020
c61613b
fix compilation
facontidavide Apr 7, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
8 changes: 5 additions & 3 deletions include/spatio_temporal_voxel_layer/frustum_models/depth_camera_frustum.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@

// STVL
#include <spatio_temporal_voxel_layer/frustum_models/frustum.hpp>
#include <array>

namespace geometry
{
Expand All @@ -55,7 +56,8 @@ class DepthCameraFrustum : public Frustum
public:
DepthCameraFrustum(const double& vFOV, const double& hFOV,
const double& min_dist, const double& max_dist);
virtual ~DepthCameraFrustum(void);

virtual ~DepthCameraFrustum(void) = default;

// transform plane normals by depth camera pose
virtual void TransformModel(void);
Expand All @@ -74,13 +76,13 @@ class DepthCameraFrustum : public Frustum
double Dot(const VectorWithPt3D&, const Eigen::Vector3d&) const;

double _vFOV, _hFOV, _min_d, _max_d;
std::vector<VectorWithPt3D> _plane_normals;
std::array<VectorWithPt3D,6> _plane_normals;
Eigen::Vector3d _position;
Eigen::Quaterniond _orientation;
bool _valid_frustum;

#if VISUALIZE_FRUSTUM
std::vector<Eigen::Vector3d> _frustum_pts;
std::array<Eigen::Vector3d,8> _frustum_pts;
ros::Publisher _frustumPub;
#endif
};
Expand Down
132 changes: 41 additions & 91 deletions src/frustum_models/depth_camera_frustum.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,102 +57,53 @@ DepthCameraFrustum::DepthCameraFrustum(const double& vFOV, const double& hFOV,
this->ComputePlaneNormals();
}

/*****************************************************************************/
DepthCameraFrustum::~DepthCameraFrustum(void)
/*****************************************************************************/
{
}

/*****************************************************************************/
void DepthCameraFrustum::ComputePlaneNormals(void)
/*****************************************************************************/
{
// give ability to construct with bogus values
if (_vFOV == 0 && _hFOV == 0)
if (_vFOV <= 0 || _hFOV <= 0)
{
_valid_frustum = false;
return;
}

// Z vector and deflected vector capture
std::vector<Eigen::Vector3d> deflected_vecs;
deflected_vecs.reserve(4);
Eigen::Vector3d Z = Eigen::Vector3d::UnitZ();

// rotate going CCW
Eigen::Affine3d rx =
Eigen::Affine3d(Eigen::AngleAxisd(_vFOV/2.,Eigen::Vector3d::UnitX()));
Eigen::Affine3d ry =
Eigen::Affine3d(Eigen::AngleAxisd(_hFOV/2.,Eigen::Vector3d::UnitY()));
deflected_vecs.push_back(rx * ry * Z);

rx = Eigen::Affine3d(Eigen::AngleAxisd(-_vFOV/2.,Eigen::Vector3d::UnitX()));
deflected_vecs.push_back(rx * ry * Z);
const double cosH = std::cos(_hFOV/2);
const double cosV = std::cos(_vFOV/2);

const double sinH = std::sin(_hFOV/2);
const double sinV = std::sin(_vFOV/2);

// min distance plane
_plane_normals[0] = VectorWithPt3D(0, 0, 1.0, Eigen::Vector3d(0, 0, _min_d) );
// max distance plane
_plane_normals[1] = VectorWithPt3D(0, 0, -1.0, Eigen::Vector3d(0, 0, _max_d) );
// left plane
_plane_normals[2] = VectorWithPt3D( cosH, 0.0, sinH, Eigen::Vector3d::Zero() );
// right plane
_plane_normals[3] = VectorWithPt3D( -cosH, 0.0, sinH, Eigen::Vector3d::Zero() );
// bottom plane
_plane_normals[4] = VectorWithPt3D( 0.0, cosV, sinV, Eigen::Vector3d::Zero() );
// top plane
_plane_normals[5] = VectorWithPt3D( 0.0, -cosV, sinV, Eigen::Vector3d::Zero() );

ry = Eigen::Affine3d(Eigen::AngleAxisd(-_hFOV/2.,Eigen::Vector3d::UnitY()));
deflected_vecs.push_back(rx * ry * Z);

rx = Eigen::Affine3d(Eigen::AngleAxisd( _vFOV/2.,Eigen::Vector3d::UnitX()));
deflected_vecs.push_back(rx * ry * Z);

// get and store CCW 4 corners for each 2 planes at ends
std::vector<Eigen::Vector3d> pt_;
pt_.reserve(2*deflected_vecs.size());
std::vector<Eigen::Vector3d>::iterator it;
for (it = deflected_vecs.begin(); it != deflected_vecs.end(); ++it)
#if VISUALIZE_FRUSTUM
const double tanH = std::tan(_hFOV/2);
const double tanV = std::tan(_vFOV/2);

Eigen::Vector3d pt_direction[4] = {
Eigen::Vector3d( tanH, tanV, 1.0 ),
Eigen::Vector3d( tanH, -tanV, 1.0 ),
Eigen::Vector3d( -tanH, -tanV, 1.0 ),
Eigen::Vector3d( -tanH, tanV, 1.0 )
};
for (int i=0; i<4; i++)
{
pt_.push_back(*(it) * _min_d);
pt_.push_back(*(it) * _max_d);
_frustum_pts[i] = pt_direction[i] * _min_d;
_frustum_pts[i+4] = pt_direction[i] * _max_d;
}

assert(pt_.size() == 8);

// cross each plane and get normals
const Eigen::Vector3d v_01(pt_[1][0]-pt_[0][0], \
pt_[1][1]-pt_[0][1], pt_[1][2]-pt_[0][2]);
const Eigen::Vector3d v_13(pt_[3][0]-pt_[1][0], \
pt_[3][1]-pt_[1][1], pt_[3][2]-pt_[1][2]);
Eigen::Vector3d T_n(v_13.cross(v_01));
T_n.normalize();
_plane_normals.push_back(VectorWithPt3D(T_n[0],T_n[1],T_n[2],pt_[0]));

const Eigen::Vector3d v_23(pt_[3][0]-pt_[2][0], \
pt_[3][1]-pt_[2][1], pt_[3][2]-pt_[2][2]);
const Eigen::Vector3d v_35(pt_[5][0]-pt_[3][0], \
pt_[5][1]-pt_[3][1], pt_[5][2]-pt_[3][2]);
Eigen::Vector3d T_l(v_35.cross(v_23));
T_l.normalize();
_plane_normals.push_back(VectorWithPt3D(T_l[0],T_l[1],T_l[2],pt_[2]));

const Eigen::Vector3d v_45(pt_[5][0]-pt_[4][0], \
pt_[5][1]-pt_[4][1], pt_[5][2]-pt_[4][2]);
const Eigen::Vector3d v_57(pt_[7][0]-pt_[5][0], \
pt_[7][1]-pt_[5][1], pt_[7][2]-pt_[5][2]);
Eigen::Vector3d T_b(v_57.cross(v_45));
T_b.normalize();
_plane_normals.push_back(VectorWithPt3D(T_b[0],T_b[1],T_b[2],pt_[4]));

const Eigen::Vector3d v_67(pt_[7][0]-pt_[6][0], \
pt_[7][1]-pt_[6][1], pt_[7][2]-pt_[6][2]);
const Eigen::Vector3d v_71(pt_[1][0]-pt_[7][0], \
pt_[1][1]-pt_[7][1], pt_[1][2]-pt_[7][2]);
Eigen::Vector3d T_r(v_71.cross(v_67));
T_r.normalize();
_plane_normals.push_back(VectorWithPt3D(T_r[0],T_r[1],T_r[2],pt_[6]));

// far plane
Eigen::Vector3d T_1(v_57.cross(v_71));
T_1.normalize();
_plane_normals.push_back(VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[7]));

// near plane
_plane_normals.push_back(VectorWithPt3D(T_1[0],T_1[1],T_1[2],pt_[2]) * -1);

#if VISUALIZE_FRUSTUM
_frustum_pts = pt_;
#endif

assert(_plane_normals.size() == 6);
_valid_frustum = true;
return;
}
Expand All @@ -170,10 +121,9 @@ void DepthCameraFrustum::TransformModel(void)
T.pretranslate(_orientation.inverse()*_position);
T.prerotate(_orientation);

std::vector<VectorWithPt3D>::iterator it;
for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
for (auto& plane: _plane_normals)
{
it->TransformFrames(T);
plane.TransformFrames(T);
}

#if VISUALIZE_FRUSTUM
Expand Down Expand Up @@ -297,15 +247,15 @@ bool DepthCameraFrustum::IsInside(const openvdb::Vec3d& pt)
return false;
}

std::vector<VectorWithPt3D>::iterator it;
for (it = _plane_normals.begin(); it != _plane_normals.end(); ++it)
for (auto& plane: _plane_normals)
{
Eigen::Vector3d p_delta(pt[0] - it->initial_point[0], \
pt[1] - it->initial_point[1], \
pt[2] - it->initial_point[2]);
p_delta.normalize();
Eigen::Vector3d p_delta(pt[0] - plane.initial_point[0],
pt[1] - plane.initial_point[1],
pt[2] - plane.initial_point[2]);

//p_delta.normalize(); // not needed. prove me wrong

if (Dot(*it, p_delta) > 0.)
if (Dot(plane, p_delta) < 0.)
{
return false;
}
Expand Down