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

Fix crushing when lidar poses are out of map #351

Merged
merged 2 commits into from
Oct 24, 2020
Merged

Fix crushing when lidar poses are out of map #351

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
014152e
Fix crushing when lidar poses are out of map
nhatao Oct 23, 2020
a9beedf
Fix lint error
nhatao Oct 23, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 18 additions & 0 deletions include/mcl_3dl/raycasts/raycast_using_dda.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,12 @@ class RaycastUsingDDA : public Raycast<POINT_TYPE>
{
kdtree_ = kdtree;
updatePointCloud();
if (!isPointWithinMap(ray_begin))
{
max_movement_ = 0;
pos_ = 0;
return;
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think raycaster still should cast the ray by handling out-of-the-map as no-hit.
Otherwise, it causes a problem at the open end of the map.

(For very temporary fix, it is acceptable though.)

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Merging for now.
#352

}
ray_begin_ = ray_begin;
ray_direction_vector_ = (ray_end_org - ray_begin).normalized();
const Vec3 ray_end = ray_direction_vector_ * additional_range_ + ray_end_org;
Expand Down Expand Up @@ -261,6 +267,18 @@ class RaycastUsingDDA : public Raycast<POINT_TYPE>
return nullptr;
}

bool isPointWithinMap(const Vec3& point) const
{
for (int i = 0; i < 3; ++i)
{
if ((point[i] < min_p_[i]) || (max_p_[i] < point[i]))
{
return false;
}
}
return true;
}

typename ChunkedKdtree<POINT_TYPE>::Ptr kdtree_;
const double min_dist_thr_sq_;
const double dda_grid_size_;
Expand Down