forked from ouster-lidar/ouster-sdk
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Update for fw 1.13. See changelog for details
* Use random udp ports by default to simplify running multiple clients * Dynamically adjust ambient and intensity exposure in visualizer * Fix crash on failed udp socket call
- Loading branch information
Dima Garbuzov
committed
Mar 17, 2020
1 parent
1cf6faf
commit 68ad03c
Showing
12 changed files
with
295 additions
and
77 deletions.
There are no files selected for viewing
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
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
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
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
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
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
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
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,52 @@ | ||
| /** | ||
| * @file | ||
| * @brief Adjust brightness image brightness and apply gamma correction | ||
| * | ||
| * Functor that adjusts brightness so that 1st percentile pixel is black | ||
| * and 99th percentile pixel is white, while applying basic gamma correction | ||
| * of 2.0. | ||
| * Stores state of the black and white points so that it does not flicker | ||
| * rapidly. | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <Eigen/Eigen> | ||
| #include <vector> | ||
| #include <cstdlib> | ||
|
|
||
| struct AutoExposure { | ||
| private: | ||
| double lo_state = -1.0; | ||
| double hi_state = -1.0; | ||
|
|
||
| public: | ||
| void operator()(Eigen::Ref<Eigen::ArrayXd> key_eigen) { | ||
| const size_t n = key_eigen.rows(); | ||
| const size_t kth_extreme = n / 100; | ||
| std::vector<size_t> indices(n); | ||
| for (size_t i = 0; i < n; i++) { | ||
| indices[i] = i; | ||
| } | ||
| auto cmp = [&](const size_t a, const size_t b) { | ||
| return key_eigen(a) < key_eigen(b); | ||
| }; | ||
| std::nth_element(indices.begin(), indices.begin() + kth_extreme, | ||
| indices.end(), cmp); | ||
| const double lo = key_eigen[*(indices.begin() + kth_extreme)]; | ||
| std::nth_element(indices.begin() + kth_extreme, | ||
| indices.end() - kth_extreme, indices.end(), cmp); | ||
| const double hi = key_eigen[*(indices.end() - kth_extreme)]; | ||
| if (lo_state < 0) { | ||
| lo_state = lo; | ||
| hi_state = hi; | ||
| } | ||
| lo_state = 0.9 * lo_state + 0.1 * lo; | ||
| hi_state = 0.9 * hi_state + 0.1 * hi; | ||
| key_eigen -= lo; | ||
| key_eigen *= 1.0 / (hi - lo); | ||
|
|
||
| // gamma correction | ||
| key_eigen = key_eigen.max(0.0).sqrt().min(1.0); | ||
| } | ||
| }; |
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,84 @@ | ||
| /** | ||
| * @file | ||
| * @brief Corrects beam uniformity by minimizing median difference between rows | ||
| * | ||
| */ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <Eigen/Eigen> | ||
| #include <vector> | ||
| #include <algorithm> | ||
|
|
||
| class BeamUniformityCorrector { | ||
| private: | ||
| std::vector<double> dark_count; | ||
|
|
||
| std::vector<double> compute_dark_count( | ||
| const Eigen::Ref<Eigen::ArrayXXd> image) { | ||
| const size_t image_h = image.rows(); | ||
| const size_t image_w = image.cols(); | ||
|
|
||
| std::vector<double> tmp(image_w); | ||
| std::vector<double> new_dark_count(image_h, 0); | ||
|
|
||
| Eigen::ArrayXXd row_diffs = | ||
| image.bottomRows(image_h - 1) - image.topRows(image_h - 1); | ||
|
|
||
| // compute the median of differences between rows | ||
| for (size_t i = 1; i < image_h; i++) { | ||
| Eigen::Map<Eigen::Matrix<double, -1, 1>> tmp_map(tmp.data(), | ||
| image_w); | ||
| tmp_map = row_diffs.row(i - 1); | ||
| std::nth_element(tmp.begin(), tmp.begin() + image_w / 2, tmp.end()); | ||
| new_dark_count[i] = new_dark_count[i - 1] + tmp[image_w / 2]; | ||
| } | ||
|
|
||
| // remove gradients in the entire height of image by doing linear fit | ||
| Eigen::Matrix<double, -1, 2> A(image_h, 2); | ||
| for (size_t i = 0; i < image_h; i++) { | ||
| A(i, 0) = 1; | ||
| A(i, 1) = i; | ||
| } | ||
|
|
||
| Eigen::Vector2d x = A.fullPivLu().solve( | ||
| Eigen::Map<Eigen::VectorXd>(new_dark_count.data(), image_h, 1)); | ||
|
|
||
| Eigen::Map<Eigen::ArrayXd>(new_dark_count.data(), image_h, 1) -= | ||
| (A * x).array(); | ||
|
|
||
| // subtract minimum value | ||
| double min_el = | ||
| *std::min_element(new_dark_count.begin(), new_dark_count.end()); | ||
| Eigen::Map<Eigen::ArrayXd>(new_dark_count.data(), image_h, 1) -= min_el; | ||
| return new_dark_count; | ||
| } | ||
|
|
||
| public: | ||
| void correct(Eigen::Ref<Eigen::ArrayXXd> image) { | ||
| const size_t image_h = image.rows(); | ||
|
|
||
| if (dark_count.size() == 0) { | ||
| dark_count = compute_dark_count(image); | ||
| } else { | ||
| // update dark_count with a decaying weighted average | ||
| const auto new_dark_count = compute_dark_count(image); | ||
| Eigen::Map<Eigen::ArrayXd>(dark_count.data(), image_h) *= 0.95; | ||
| Eigen::Map<Eigen::ArrayXd>(dark_count.data(), image_h) += | ||
| Eigen::Map<const Eigen::ArrayXd>(new_dark_count.data(), | ||
| image_h) * | ||
| 0.05; | ||
| } | ||
|
|
||
| // apply the dark count correction row by row | ||
| for (size_t i = 0; i < image_h; i++) { | ||
| // contains a view of the current row | ||
| image.row(i) -= dark_count[i]; | ||
| image.row(i) = image.row(i).unaryExpr([](double x) { | ||
| x = std::max(x, 0.0); | ||
| x = std::min(x, (double)UINT32_MAX); | ||
| return x; | ||
| }); | ||
| } | ||
| } | ||
| }; |
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -9,6 +9,7 @@ | |
| #include <iterator> | ||
| #include <utility> | ||
| #include <vector> | ||
| #include <cstdlib> | ||
|
|
||
| namespace ouster { | ||
|
|
||
|
|
||
Oops, something went wrong.