Make lidar measurement model class #195
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
at-wat
commented
Sep 9, 2018
| return true; | ||
| return false; | ||
| }; | ||
| pc->erase( |
This comment has been minimized.
This comment has been minimized.
at-wat
commented
Sep 9, 2018
| num_points_ = num_points_default_; | ||
| return; | ||
| } | ||
| size_t num = num_points_; |
This comment has been minimized.
This comment has been minimized.
Codecov Report
@@ Coverage Diff @@
## master #195 +/- ##
==========================================
- Coverage 89.25% 89.25% -0.01%
==========================================
Files 9 13 +4
Lines 1368 1312 -56
==========================================
- Hits 1221 1171 -50
+ Misses 147 141 -6
Continue to review full report at Codecov.
|
This comment has been minimized.
This comment has been minimized.
at-wat
commented
Sep 18, 2018
| virtual typename pcl::PointCloud<POINT_TYPE>::Ptr filter( | ||
| const typename pcl::PointCloud<POINT_TYPE>::Ptr &) const = 0; | ||
|
|
||
| virtual std::pair<float, float> measure( |
There was a problem hiding this comment.
define measurement result type
There was a problem hiding this comment.
oh, I've forgotten this. thanks.
at-wat
commented
Sep 18, 2018
at-wat
commented
Sep 18, 2018
src/mcl_3dl.cpp
Outdated
| if (match_ratio_min > qualities["likelihood"]) | ||
| match_ratio_min = qualities["likelihood"]; | ||
| if (match_ratio_max < qualities["likelihood"]) | ||
| match_ratio_max = qualities["likelihood"]; | ||
|
|
||
| // approximative beam model |
There was a problem hiding this comment.
implement beam model as a class
at-wat
commented
Sep 18, 2018
src/mcl_3dl.cpp
Outdated
| @@ -1560,7 +1290,7 @@ class MCL3dlNode | |||
| params_.global_localization_div_yaw = lroundf(2 * M_PI / grid_ang); | |||
|
|
|||
| pnh_.param("num_particles", params_.num_particles, 64); | |||
| pf_.reset(new mcl_3dl::pf::ParticleFilter<State, float, ParticleWeightedMeanQuat>(params_.num_particles)); | |||
| pf_.reset(new pf::ParticleFilter<State6DOF, float, ParticleWeightedMeanQuat>(params_.num_particles)); | |||
| pnh_.param("num_points", params_.num_points, 96); | |||
There was a problem hiding this comment.
migrate old parameters to new parameters in the model specific namespace
at-wat
commented
Sep 18, 2018
| typename pcl::PointCloud<POINT_TYPE>::Ptr output(new pcl::PointCloud<POINT_TYPE>); | ||
|
|
||
| output->width = 1; | ||
| output->height = 0; |
There was a problem hiding this comment.
this is done by pcl::PointCloud<>::push_back
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
This comment has been minimized.
| class LidarMeasurementModelBase | ||
| { | ||
| static_assert( | ||
| std::is_base_of<pf::ParticleBase<float>, STATE_TYPE>::value, |
There was a problem hiding this comment.
this is odd. how come you force to use only float? as it's template, a programmer might give other types, too.
There was a problem hiding this comment.
it's very complicated to static_assert templated class.
allowing pf::ParticleBase<float> and also pf::ParticleBase<double> would be enough for now.
There was a problem hiding this comment.
IMO, this interface class doesn't need to take STATE_TYPE as class templates.
STATE_TYPE is used only for an argument of measure function. So, for me up-casting to pf::ParticleBase by the function argument is sufficient.
There was a problem hiding this comment.
Basically, this makes sense for me.
However in this case, since pf::ParticleBase is templated, it is not allowed to be used as an argument of virtual function.
There was a problem hiding this comment.
If you bind interfaces by pure virtual function, yes. Taking a derived class via class template parameter, and static cast from this pointer to a derived class for each member functions allows you to make common function interfaces that has own template parameters.
There was a problem hiding this comment.
Come to think of it, these lidar measurement models are available only for State6DOF, not for general pf::ParticleBase.
So, just support State6DOF would be enough for this.
include/mcl_3dl/lidar_measurement_models/lidar_measurement_model_beam.h
Outdated
Show resolved
Hide resolved
| public: | ||
| LidarMeasurementModelBeam(const float x, const float y, const float z) | ||
| { | ||
| map_grid_min_ = std::min(std::min(x, y), z); |
| LidarMeasurementModelBeam(const float x, const float y, const float z) | ||
| { | ||
| map_grid_min_ = std::min(std::min(x, y), z); | ||
| map_grid_max_ = std::max(std::max(x, y), z); |
[#517] PASSED on indigoAll tests passed[#517] PASSED on kineticAll tests passed[#517] PASSED on melodicAll tests passed |
|
|
||
| namespace mcl_3dl | ||
| { | ||
| template <class POINT_TYPE> |
There was a problem hiding this comment.
move this template to sample().
|
|
||
| namespace mcl_3dl | ||
| { | ||
| template <class STATE_TYPE, class POINT_TYPE> |
There was a problem hiding this comment.
LidarMeasurementModelBeam looks like not independent from the specific point type as it access to intensity field in the logic.
There was a problem hiding this comment.
when you add support new point type, proper way can take slightly different, so that I think it's ok to create different classes depending on point type. do you think so?
There was a problem hiding this comment.
In my previous opinion, I would like to add specialized nodes for each input point type (PointXYZ and PointXYZI) to increase run-time performance as far as possible, reducing type conversion.
But this is not very much critical and simply converting point type at the subscriber callback should be reasonable.
[#518] FAILED on indigo
[#518] FAILED on kinetic
[#518] FAILED on melodic
|
[#519] PASSED on indigoAll tests passed[#519] PASSED on kineticAll tests passed[#519] PASSED on melodicAll tests passed |
[#520] PASSED on kineticAll tests passed[#520] PASSED on melodicAll tests passed[#520] PASSED on indigoAll tests passed |
|
@DaikiMaekawa addressed your comments. PTAL |
include/mcl_3dl/state_6dof.h
Outdated
| } | ||
| static State6DOF generateNoise( | ||
| std::default_random_engine &engine_, | ||
| State6DOF mean, State6DOF sigma) |
There was a problem hiding this comment.
why do you need copy?
include/mcl_3dl/state_6dof.h
Outdated
| } | ||
| size_t size() const override | ||
| { | ||
| return 10; |
There was a problem hiding this comment.
what kind of size is it?
There was a problem hiding this comment.
size allowed in operator[]
this must be 13 for current state model
There was a problem hiding this comment.
that makes sense to me if it's 13.
| case 11: | ||
| return odom_err_integ_ang.y; | ||
| case 12: | ||
| return odom_err_integ_ang.z; |
There was a problem hiding this comment.
This is intended to be only used by ParticleFilter class and range must be limited by the return value of the below size().
Since this is one of the most called function, range for this operator is better to be managed by the caller side. (like stl container)
Adding assert would be good for here.
[#521] PASSED on indigoAll tests passed[#521] PASSED on kineticAll tests passed[#521] PASSED on melodicAll tests passed |
| for (const auto &p_ref : points_ref) | ||
| { | ||
| if (p_ref[0] == p.x && p_ref[1] == p.y && p_ref[2] == p.z) | ||
| found = true; |
There was a problem hiding this comment.
once found become true, you don't have to continue for loop.
include/mcl_3dl/state_6dof.h
Outdated
| } | ||
| float operator[](const size_t i) const | ||
| { | ||
| assert(i < 13); |
There was a problem hiding this comment.
why don't you use default?
[#522] FAILED on indigo
[#522] FAILED on kinetic
[#522] FAILED on melodic
|
[#523] PASSED on kineticAll tests passed[#523] PASSED on melodicAll tests passed[#523] PASSED on indigoAll tests passed |
|
@DaikiMaekawa addressed your comments. PTAL |
DaikiMaekawa
approved these changes
Sep 25, 2018
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
No description provided.