Devel/nimisbert #399
Devel/nimisbert #399
Conversation
|
Can one of the admins verify this patch? |
|
ok to test |
| @@ -86,15 +86,15 @@ typename PointMatcher<T>::OutlierWeights PointMatcher<T>::OutlierFilters::comput | |||
| else | |||
| { | |||
| // apply filters, they should take care of infinite distances | |||
| //LOG_INFO_STREAM("Applying " << this->size() << " Outlier filters" ); | |||
| LOG_INFO_STREAM("Applying " << this->size() << " Outlier filters" ); | |||
There was a problem hiding this comment.
Do you have a way to log to debug? As ROS does. That way you would not need to comment this line out.
| @@ -578,6 +583,23 @@ typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::RobustOutlierFil | |||
| w = p * (k + d) * (k + e2).inverse(); | |||
| break; | |||
| } | |||
| case RobustFctId::SparseICP: | |||
| w = (k)* e2.sqrt().pow(k-2); | |||
There was a problem hiding this comment.
How often will you run this operation?
If very often, and with an integer exponent, maybe it makes sense to directly implement multiplication in a loop or a different way.
See this link: https://baptiste-wicht.com/posts/2017/09/cpp11-performance-tip-when-to-use-std-pow.html
Pow is a very general function, which resolves the number-to-power problem for integer and fractional values.
| case RobustFctId::EMICP: | ||
| aboveThres = (1/(e2.sqrt())); | ||
| w = (e2.sqrt() >= 3).select(aboveThres,0); | ||
| break; |
There was a problem hiding this comment.
From the above operations I see a few values
- w
- k
- e2
- 3.14 (does it make sense to declare this contant higher up? It is currently a magic number)
Are all of these computed in this step?
| w = (1 + e2.sqrt()/k).inverse(); | ||
| break; | ||
| case RobustFctId::Logistic: | ||
| w = (((e2.sqrt()/k).exp()-(-e2.sqrt()/k).exp())*k)*((((e2.sqrt()/k).exp()+(-e2.sqrt()/k).exp())*e2.sqrt()).inverse()); |
There was a problem hiding this comment.
This is a very long operation.
Could you write a comment on top, that shows what is the operation intended? Maybe in raw text or in latex (you can quickly build it using online compilers as https://www.codecogs.com/latex/eqneditor.php )
| case RobustFctId::EMICP: | ||
| aboveThres = (1/(e2.sqrt())); | ||
| w = (e2.sqrt() >= 3).select(aboveThres,0); | ||
| break; | ||
| default: | ||
| break; |
There was a problem hiding this comment.
What is the dimensionality of the objects to which you are applying inverse, sqrt, pow?
| @@ -578,6 +583,23 @@ typename PointMatcher<T>::OutlierWeights OutlierFiltersImpl<T>::RobustOutlierFil | |||
| w = p * (k + d) * (k + e2).inverse(); | |||
| break; | |||
| } | |||
| case RobustFctId::SparseICP: | |||
| w = (k)* e2.sqrt().pow(k-2); | |||
There was a problem hiding this comment.
You cannot implement sparse-ICP with our current minimizer. Also, you need to compute the p-norm of the error and you can not do that at this stage, since you don't have the individual distances
| break; | ||
| case RobustFctId::Andrew: | ||
| belowThres = (k/(e2.sqrt()))*(e2.sqrt()/k); | ||
| w = (e2.sqrt() >= k*3.14).select(0.0, belowThres); |
There was a problem hiding this comment.
You should use M_PI instead of 3.14.
| w = (e2.sqrt() >= k*3.14).select(0.0, belowThres); | ||
| break; | ||
| case RobustFctId::EMICP: | ||
| aboveThres = (1/(e2.sqrt())); |
There was a problem hiding this comment.
I don't think EM-ICP should be included. It is just L1 follow by a distance threshold, you can already implement it without modifying the library. EM-ICP only makes sense when minimizing point cluster.
Hello,
I added 5 robust weight functions. They are the weight functions of Sparse ICP, Fair, Logistic, Andrew, EM-ICP.