diff --git a/CHANGELOG.md b/CHANGELOG.md index c78a6fbb..395870be 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,21 @@ # Changelog +## [1.13.0] - 2020-03-16 +### Added +- post-processing to improve ambient image uniformity in visualizer +- make timestamp mode configurable via the client (PR #97) + +### Changed +- turn on position-independent code by default to make using code in libraries + easier (PR #65) +- use random ports for lidar and imu data by default when unspecified + +### Fixed +- prevent legacy tf prefix from making invalid frame names (PR #56) +- use `iterator_traits` to make `batch_to_iter` work with more types (PR #70) +- use correct name for json dependency in `package.xml` (PR #116) +- handle udp socket creation error gracefully in client + ## [1.12.0] - 2019-05-02 ### Added - install directives for `ouster_ros` build (addresses #50) diff --git a/README.md b/README.md index f698d621..ecb1e61c 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,14 @@ -# OS-1 Example Code -Sample code for connecting to and configuring the OS-1, reading and visualizing +# OS1 Example Code +Sample code for connecting to and configuring the OS1, reading and visualizing data, and interfacing with ROS. See the `README.md` in each subdirectory for details. ## Contents -* [ouster_client/](ouster_client/README.md) contains an example C++ client for the OS-1 sensor -* [ouster_viz/](ouster_viz/README.md) contains a visualizer for the OS-1 sensor +* [ouster_client/](ouster_client/README.md) contains an example C++ client for the OS1 sensor +* [ouster_viz/](ouster_viz/README.md) contains a visualizer for the OS1 sensor * [ouster_ros/](ouster_ros/README.md) contains example ROS nodes for publishing point cloud messages ## Sample Data * Sample sensor output usable with the provided ROS code is available - [here](https://data.ouster.io/sample-data-1.12) + [here](https://data.ouster.io/sample-data-1.13) diff --git a/ouster_client/CMakeLists.txt b/ouster_client/CMakeLists.txt index 7b73a16c..391a77ec 100644 --- a/ouster_client/CMakeLists.txt +++ b/ouster_client/CMakeLists.txt @@ -26,6 +26,7 @@ endif() add_library(ouster_client STATIC src/os1.cpp src/os1_util.cpp) +set_target_properties(ouster_client PROPERTIES POSITION_INDEPENDENT_CODE ON) target_link_libraries(ouster_client jsoncpp) target_include_directories(ouster_client PUBLIC include) target_include_directories(ouster_client SYSTEM PRIVATE ${jsoncpp_INCLUDE_DIRS}) diff --git a/ouster_client/README.md b/ouster_client/README.md index b253d85d..dbaf6285 100644 --- a/ouster_client/README.md +++ b/ouster_client/README.md @@ -1,24 +1,24 @@ -# OS-1 Example Client +# OS1 Example Client ## Contents -* `ouster_client/` contains a simple C++ client for the OS-1 sensor +* `ouster_client/` contains a simple C++ client for the OS1 sensor that + prints lidar data to the terminal * can be built both with and without ROS. See the instructions in - `ouster_ros` for building in a ROS environment + [ouster_ros/](../ouster_ros/README.md) for building in a ROS environment ## Building the Sample Client * The sample client requires a compiler supporting C++11 or newer and CMake * Build with `cd /path/to/ouster_example/ouster_client && mkdir build - && cd build && cmake .. && make` + && cd build && cmake .. && make` where `/path/to/ouster_example` is where you've cloned the repository ## Running the Sample Client -* The sample client includes a small driver program that just prints - some data to the terminal -* Make sure the OS-1 is connected to the network and has obtained a - DHCP lease. See accompanying documentation for more details -* You should see a binary called `ouster_client_example` in your build +* Make sure the OS1 is connected to the network and has obtained a + DHCP lease. See section 3.1 in the accompanying + [software user guide](https://www.ouster.io/downloads) for more details +* An executable called `ouster_client_example` is generated in the build directory on success -* Run `ouster_client_example ` where - `` is the hostname or IP address of the OS-1 sensor, +* Run `./ouster_client_example ` where + `` is the hostname or IP address of the OS1 sensor, and `` is the IP to which the sensor should send lidar data diff --git a/ouster_client/include/ouster/os1.h b/ouster_client/include/ouster/os1.h index 29b4ab60..da5eba80 100644 --- a/ouster_client/include/ouster/os1.h +++ b/ouster_client/include/ouster/os1.h @@ -35,6 +35,13 @@ enum class lidar_mode { MODE_INVALID }; +enum class timestamp_mode { + TIME_FROM_INTERNAL_OSC = 1, + TIME_FROM_SYNC_PULSE_IN, + TIME_FROM_PTP_1588, + MODE_INVALID +}; + struct version { int16_t major; int16_t minor; @@ -107,6 +114,20 @@ lidar_mode lidar_mode_of_string(const std::string& s); */ int n_cols_of_lidar_mode(lidar_mode mode); +/** + * Get string representation of a timestamp mode + * @param timestamp_mode + * @return string representation of the timestamp mode, or "UNKNOWN" + */ +std::string to_string(timestamp_mode mode); + +/** + * Get timestamp mode from string + * @param string + * @return timestamp mode corresponding to the string, or 0 on error + */ +timestamp_mode timestamp_mode_of_string(const std::string& s); + /** * Listen for OS1 data on the specified ports * @param lidar_port port on which the sensor will send lidar data @@ -126,8 +147,8 @@ std::shared_ptr init_client(int lidar_port = 7502, int imu_port = 7503); std::shared_ptr init_client(const std::string& hostname, const std::string& udp_dest_host, lidar_mode mode = lidar_mode::MODE_1024x10, - const uint16_t lidar_port = 7502u, - const uint16_t imu_port = 7503u); + timestamp_mode ts_mode = timestamp_mode::TIME_FROM_INTERNAL_OSC, + int lidar_port = 7502u, int imu_port = 7503u); /** * Block for up to timeout_sec until either data is ready or an error occurs. @@ -167,8 +188,7 @@ std::string get_metadata(const client& cli); /** * Parse metadata text blob from the sensor into a sensor_info struct. String * and vector fields will have size 0 if the parameter cannot be found or - * parsed, - * while lidar_mode will be set to 0 (invalid). + * parsed, while lidar_mode will be set to 0 (invalid) * @throw runtime_error if the text is not valid json * @param metadata a text blob returned by get_metadata above * @return a sensor_info struct populated with a subset of the metadata diff --git a/ouster_client/include/ouster/os1_util.h b/ouster_client/include/ouster/os1_util.h index e78164c4..a225ac1e 100644 --- a/ouster_client/include/ouster/os1_util.h +++ b/ouster_client/include/ouster/os1_util.h @@ -82,14 +82,14 @@ std::vector get_px_offset(int W); template std::function batch_to_iter( const std::vector& xyz_lut, int W, int H, - const typename iterator_type::value_type& empty, C&& c, F&& f) { + const typename std::iterator_traits::value_type& empty, + C&& c, F&& f) { int next_m_id{W}; int32_t cur_f_id{-1}; int64_t scan_ts{-1L}; return [=](const uint8_t* packet_buf, iterator_type it) mutable { - for (int icol = 0; icol < OS1::columns_per_buffer; icol++) { const uint8_t* col_buf = OS1::nth_col(icol, packet_buf); const uint16_t m_id = OS1::col_measurement_id(col_buf); diff --git a/ouster_client/package.xml b/ouster_client/package.xml index d5933f47..e956f413 100644 --- a/ouster_client/package.xml +++ b/ouster_client/package.xml @@ -5,9 +5,9 @@ The os1_client package ouster developers BSD - jsoncpp + libjsoncpp catkin - jsoncpp + libjsoncpp diff --git a/ouster_client/src/os1.cpp b/ouster_client/src/os1.cpp index b0fc73f0..e6fbaebc 100644 --- a/ouster_client/src/os1.cpp +++ b/ouster_client/src/os1.cpp @@ -46,6 +46,29 @@ const std::array, 5> lidar_mode_strings = { {lidar_mode::MODE_1024x20, "1024x20"}, {lidar_mode::MODE_2048x10, "2048x10"}}}; +const std::array, 3> + timestamp_mode_strings = { + {{timestamp_mode::TIME_FROM_INTERNAL_OSC, "TIME_FROM_INTERNAL_OSC"}, + {timestamp_mode::TIME_FROM_SYNC_PULSE_IN, "TIME_FROM_SYNC_PULSE_IN"}, + {timestamp_mode::TIME_FROM_PTP_1588, "TIME_FROM_PTP_1588"}}}; + +int32_t get_sock_port(int sock_fd) { + struct sockaddr_storage ss; + socklen_t addrlen = sizeof ss; + + if (getsockname(sock_fd, (struct sockaddr*)&ss, &addrlen) < 0) { + std::cerr << "udp getsockname(): " << std::strerror(errno) << std::endl; + return -1; + } + + if (ss.ss_family == AF_INET) + return ntohs(((struct sockaddr_in*)&ss)->sin_port); + else if (ss.ss_family == AF_INET6) + return ntohs(((struct sockaddr_in6*)&ss)->sin6_port); + else + return -1; +} + static int udp_data_socket(int port) { struct addrinfo hints, *info_start, *ai; @@ -91,6 +114,7 @@ static int udp_data_socket(int port) { if (fcntl(sock_fd, F_SETFL, fcntl(sock_fd, F_GETFL, 0) | O_NONBLOCK) < 0) { std::cerr << "udp fcntl(): " << std::strerror(errno) << std::endl; + close(sock_fd); return -1; } @@ -173,7 +197,7 @@ bool do_tcp_cmd(int sock_fd, const std::vector& cmd_tokens, void update_json_obj(Json::Value& dst, const Json::Value& src) { for (const auto& key : src.getMemberNames()) dst[key] = src[key]; } -} +} // namespace std::string to_string(version v) { if (v == invalid_version) return "UNKNOWN"; @@ -232,6 +256,28 @@ int n_cols_of_lidar_mode(lidar_mode mode) { } } +std::string to_string(timestamp_mode mode) { + auto end = timestamp_mode_strings.end(); + auto res = + std::find_if(timestamp_mode_strings.begin(), end, + [&](const std::pair& p) { + return p.first == mode; + }); + + return res == end ? "UNKNOWN" : res->second; +} + +timestamp_mode timestamp_mode_of_string(const std::string& s) { + auto end = timestamp_mode_strings.end(); + auto res = + std::find_if(timestamp_mode_strings.begin(), end, + [&](const std::pair& p) { + return p.second == s; + }); + + return res == end ? timestamp_mode::MODE_INVALID : res->first; +} + std::string get_metadata(const client& cli) { Json::StreamWriterBuilder builder; builder["enableYAMLCompatibility"] = true; @@ -285,18 +331,26 @@ sensor_info parse_metadata(const std::string& meta) { std::shared_ptr init_client(int lidar_port, int imu_port) { auto cli = std::make_shared(); - int lidar_fd = udp_data_socket(lidar_port); - int imu_fd = udp_data_socket(imu_port); - cli->lidar_fd = lidar_fd; - cli->imu_fd = imu_fd; + cli->lidar_fd = udp_data_socket(lidar_port); + cli->imu_fd = udp_data_socket(imu_port); + + if (cli->lidar_fd < 0 || cli->imu_fd < 0) + return std::shared_ptr(); + return cli; } std::shared_ptr init_client(const std::string& hostname, const std::string& udp_dest_host, - lidar_mode mode, const uint16_t lidar_port, - const uint16_t imu_port) { - auto cli = init_client(lidar_port, imu_port); + lidar_mode mode, timestamp_mode ts_mode, + int lidar_port, int imu_port) { + auto cli = init_client(lidar_port, imu_port); + if (!cli) return std::shared_ptr(); + + // update requested ports to actual bound ports + lidar_port = get_sock_port(cli->lidar_fd); + imu_port = get_sock_port(cli->imu_fd); + if (lidar_port == -1 || imu_port == -1) return std::shared_ptr(); int sock_fd = cfg_socket(hostname.c_str()); @@ -314,9 +368,10 @@ std::shared_ptr init_client(const std::string& hostname, do_tcp_cmd(sock_fd, {"set_config_param", "udp_ip", udp_dest_host}, res); success &= res == "set_config_param"; - success &= do_tcp_cmd(sock_fd, {"set_config_param", "udp_port_lidar", - std::to_string(lidar_port)}, - res); + success &= do_tcp_cmd( + sock_fd, + {"set_config_param", "udp_port_lidar", std::to_string(lidar_port)}, + res); success &= res == "set_config_param"; success &= do_tcp_cmd( @@ -328,6 +383,11 @@ std::shared_ptr init_client(const std::string& hostname, sock_fd, {"set_config_param", "lidar_mode", to_string(mode)}, res); success &= res == "set_config_param"; + success &= do_tcp_cmd( + sock_fd, {"set_config_param", "timestamp_mode", to_string(ts_mode)}, + res); + success &= res == "set_config_param"; + success &= do_tcp_cmd(sock_fd, {"get_sensor_info"}, res); success &= reader->parse(res.c_str(), res.c_str() + res.size(), &cli->meta, &errors); @@ -380,15 +440,15 @@ client_state poll_client(const client& c, const int timeout_sec) { std::cerr << "select: " << std::strerror(errno) << std::endl; res = client_state(res | client_state::ERROR); } else if (retval) { - if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | client_state::LIDAR_DATA); - if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | client_state::IMU_DATA); + if (FD_ISSET(c.lidar_fd, &rfds)) res = client_state(res | LIDAR_DATA); + if (FD_ISSET(c.imu_fd, &rfds)) res = client_state(res | IMU_DATA); } return res; } -static bool recv_fixed(int fd, void* buf, size_t len) { - ssize_t n = recvfrom(fd, buf, len + 1, 0, NULL, NULL); - if (n == (ssize_t)len) +static bool recv_fixed(int fd, void* buf, ssize_t len) { + ssize_t n = recv(fd, (char*)buf, len + 1, 0); + if (n == len) return true; else if (n == -1) std::cerr << "recvfrom: " << std::strerror(errno) << std::endl; @@ -404,5 +464,6 @@ bool read_lidar_packet(const client& cli, uint8_t* buf) { bool read_imu_packet(const client& cli, uint8_t* buf) { return recv_fixed(cli.imu_fd, buf, imu_packet_bytes); } -} -} + +} // namespace OS1 +} // namespace ouster diff --git a/ouster_ros/CMakeLists.txt b/ouster_ros/CMakeLists.txt index e54205e2..727dcde9 100644 --- a/ouster_ros/CMakeLists.txt +++ b/ouster_ros/CMakeLists.txt @@ -48,6 +48,7 @@ catkin_package( ) add_library(ouster_ros STATIC src/os1_ros.cpp) +set_target_properties(ouster_ros PROPERTIES POSITION_INDEPENDENT_CODE ON) target_link_libraries(ouster_ros ${catkin_LIBRARIES}) add_dependencies(ouster_ros ${PROJECT_NAME}_gencpp) diff --git a/ouster_ros/README.md b/ouster_ros/README.md index 473f0708..17822f01 100644 --- a/ouster_ros/README.md +++ b/ouster_ros/README.md @@ -1,35 +1,45 @@ -# OS-1 Example ROS Node +# OS1 Example ROS Node ## Contents -* `ouster_ros/` contains sample code for publishing OS-1 data as standard ROS - topics -* Tested with ROS Kinetic on Ubuntu 16.04 +* `ouster_ros/` contains sample code for publishing OS1 data as standard + ROS topics + +## Operating System Support +* The visualizer has been tested on [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu) on Ubuntu 16.04 + * additionally requires `ros-kinetic-pcl-ros`, `ros-kinetic-tf2-geometry-msgs` + and, optionally, `ros-kinetic-rviz` for visualization using ROS +* The visualizer has been tested on [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04 + * additionally requires `ros-melodic-pcl-ros`, `ros-melodic-tf2-geometry-msgs` + and, optionally, `ros-melodic-rviz` for visualization using ROS ## Building the Sample ROS Node -* Supports Ubuntu 16.04 with ROS Kinetic (for ouster_ros) -* ROS installation instructions can be found - [here](http://wiki.ros.org/kinetic/Installation/Ubuntu) -* Additionally requires `ros-kinetic-pcl-ros`, `ros-kinetic-tf2-geometry-msgs` - and, optionally, `ros-kinetic-rviz` for visualization using ROS -* Be sure to source the ROS setup script before building. For example:`source - /opt/ros/kinetic/setup.bash` +* In the following instruction steps, `/path/to/ouster_example` is where you've cloned the repository +* Run the following command `export CMAKE_PREFIX_PATH=/path/to/ouster_example` +* Be sure to source the ROS setup script before building. For example: +`source /opt/ros/[kinetic_or_melodic]/setup.bash` * Build with `mkdir -p myworkspace/src && cd myworkspace && ln -s /path/to/ouster_example ./src/ && catkin_make -DCMAKE_BUILD_TYPE=Release` ## Running the Sample ROS Nodes -* Make sure to set up the ROS environment with `source - /path/to/myworkspace/devel/setup.bash` in a new terminal for each command - below -* To publish ROS topics from a running sensor: - - Run `roslaunch ouster_ros os1.launch os1_hostname:= - os1_udp_dest:= lidar_mode:=` where - `` can be the hostname or IP of the OS-1 device, - `` is the IP to which the sensor should send data, and - `` is one of 512x10, 512x20, 1024x10, 1024x20, or 2048x10 - - To record raw sensor output, run `rosbag record /os1_node/imu_packets - /os1_node/lidar_packets` in another terminal -* To publish ROS topics from recorded data: - - Run `roslaunch ouster_ros os1.launch replay:=true +* Make sure the OS1 is connected to the network and has obtained a DHCP lease. See section 3.1 in the accompanying [software user guide](https://www.ouster.io/downloads) for more details +* In each new terminal for each command below: + - Make sure to source ROS environment with `source + /path/to/myworkspace/devel/setup.bash` where `/path/to/myworkspace` is where the path to the workspace directory that was created + when building the ROS nodes +* To publish ROS topics from a running sensor from within the `ouster_ros` directory: + - Run `roslaunch os1.launch os1_hostname:= + os1_udp_dest:= lidar_mode:= viz:=`where: + - `` can be the hostname (os1-991xxxxxxxxx) or IP of the OS1 + - `` is the IP to which the sensor should send data + - `` is one of `512x10`, `512x20`, `1024x10`, `1024x20`, or `2048x10` + - `` is either `true` or `false`. If true, a window should open and start + displaying data after a few seconds +* To record raw sensor output + - In another terminal instance, run `rosbag record /os1_node/imu_packets + /os1_node/lidar_packets` + - This will save a .bag file of recorded data in that directory +* To publish ROS topics from recorded data from withint the `ouster_ros` directory: + - Run `roslaunch os1.launch replay:=true os1_hostname:=` - In a second terminal run `rosbag play --clock ` - Note: `os1_node` reads and writes metadata to `${ROS_HOME}` to enable @@ -40,13 +50,6 @@ 1024x10. This can be overridden with the `lidar_mode` parameter. Visualizer output will only be correct if the same `lidar_mode` parameter is used for both recording and replay -* To display sensor output using the provided visualizer: - - To visualize the published OS-1 point cloud data using the provided - visualizer, add `viz:=true` to either of the `roslaunch` commands above - - A window should open and start displaying data after a few seconds. This - should work with a running sensor or replayed data - - See the [README.md](../ouster_viz/README.md) in the `ouster_viz` directory - for details on keyboard and mouse controls * To display sensor output using ROS tools (rviz): - Follow the instructions above for running the example ROS code with a sensor or recorded data @@ -54,3 +57,22 @@ in another terminal - To view lidar intensity/noise/range images, add `image:=true` to either of the `roslaunch` commands above + +## Key bindings +| key | what it does | +| ----| ------------ | +| `o` | Increase point size | +| `p` | Decrease point size | +| `m` | Cycle point cloud coloring by z-height / intensity / z-height plus intensity / range | +| `c` | Cycle color scheme for range image | +| `shift c` | Cycle color scheme for point cloud | +| `v` | Toggle color cycling in range image | +| `n` | Display ambient image from the sensor| +| `r` | Reset camera position +| `0` (zero) | Toggle parallel projection and reset camera | +| `d` | Cycle through fraction of the window height used for displaying range and intensity image + +## Mouse control +* Click and drag rotates the view +* Middle click and drag pans the view +* Scroll adjusts how far away the camera is from the vehicle diff --git a/ouster_ros/os1.launch b/ouster_ros/os1.launch index afe4bda1..3f3cfe2e 100755 --- a/ouster_ros/os1.launch +++ b/ouster_ros/os1.launch @@ -6,12 +6,15 @@ + + + @@ -24,6 +27,7 @@ + diff --git a/ouster_ros/src/os1_cloud_node.cpp b/ouster_ros/src/os1_cloud_node.cpp index a5742416..34be06e4 100644 --- a/ouster_ros/src/os1_cloud_node.cpp +++ b/ouster_ros/src/os1_cloud_node.cpp @@ -43,14 +43,14 @@ int main(int argc, char** argv) { ros::NodeHandle nh("~"); auto tf_prefix = nh.param("tf_prefix", std::string{}); - auto sensor_frame = tf_prefix + "/os1_sensor"; - auto imu_frame = tf_prefix + "/os1_imu"; - auto lidar_frame = tf_prefix + "/os1_lidar"; + if (!tf_prefix.empty() && tf_prefix.back() != '/') + tf_prefix.append("/"); + auto sensor_frame = tf_prefix + "os1_sensor"; + auto imu_frame = tf_prefix + "os1_imu"; + auto lidar_frame = tf_prefix + "os1_lidar"; auto replay = false; nh.param("replay", replay, false); - - ouster_ros::OS1ConfigSrv cfg{}; auto client = nh.serviceClient("os1_config"); client.waitForExistence(); diff --git a/ouster_ros/src/os1_node.cpp b/ouster_ros/src/os1_node.cpp index b623c92b..c3be6d3b 100644 --- a/ouster_ros/src/os1_node.cpp +++ b/ouster_ros/src/os1_node.cpp @@ -180,6 +180,7 @@ int main(int argc, char** argv) { auto imu_port = nh.param("os1_imu_port", 7502); auto replay = nh.param("replay", false); auto lidar_mode = nh.param("lidar_mode", std::string{}); + auto timestamp_mode = nh.param("timestamp_mode", std::string{}); auto scan_dur = ns(nh.param("scan_dur_ns", 100000000)); // fall back to metadata file name based on hostname, if available @@ -197,6 +198,15 @@ int main(int argc, char** argv) { return EXIT_FAILURE; } + if (not timestamp_mode.size()) { + timestamp_mode = OS1::to_string(OS1::timestamp_mode::TIME_FROM_INTERNAL_OSC); + } + + if (OS1::timestamp_mode_of_string(timestamp_mode) == OS1::timestamp_mode::MODE_INVALID) { + ROS_ERROR("Invalid timestamp mode %s", timestamp_mode.c_str()); + return EXIT_FAILURE; + } + if (!replay && (!hostname.size() || !udp_dest.size())) { ROS_ERROR("Must specify both hostname and udp destination"); return EXIT_FAILURE; @@ -225,6 +235,7 @@ int main(int argc, char** argv) { auto cli = OS1::init_client(hostname, udp_dest, OS1::lidar_mode_of_string(lidar_mode), + OS1::timestamp_mode_of_string(timestamp_mode), lidar_port, imu_port); if (!cli) { diff --git a/ouster_viz/README.md b/ouster_viz/README.md index 6db21629..703eaf06 100644 --- a/ouster_viz/README.md +++ b/ouster_viz/README.md @@ -1,10 +1,10 @@ -# OS-1 Example Visualizer +# OS1 Example Visualizer ## Contents -* 'ouster_viz/' contains a basic visualizer that can be used to +* `ouster_viz/` contains a basic visualizer that can be used to display point clouds and range/intensity/ambient images -* can be built both with and without ROS. See the instructions in - `ouster_ros` for building in a ROS environment +* Can be built both with and without ROS. See the instructions in + [ouster_ros](../ouster_ros/README.md) for building in a ROS environment ## Operating System Support * The visualizer has been tested on: Ubuntu 16.04, 17.1, and 18.04, as @@ -16,25 +16,26 @@ and CMake 3.1 or newer * Requires VTK6 and Eigen3 libraries * Using Ubuntu: sudo apt-get install libvtk6-dev libeigen3-dev -* Using Fedora: sudo apt-get update yum install vtk-devel.x86_64 - eigen3-devel.noarch +* Using Fedora: sudo yum install vtk-devel.x86_64 eigen3-devel.noarch ## Building the Visualizer: -* Build with 'cd /path/to/ouster_example/ouster_viz && mkdir build && - cd build && cmake -DCMAKE_BUILD_TYPE=Release .. && make' +* In the following instruction steps, `/path/to/ouster_example` is where you've cloned the repository +* Run the following command `export CMAKE_PREFIX_PATH=/path/to/ouster_example` +* Build with `cd /path/to/ouster_example/ouster_viz && mkdir build && + cd build && cmake -DCMAKE_BUILD_TYPE=Release .. && make` ## Running the Visualizer -* An executable called "viz" is generated in the build directory +* An executable called `viz` is generated in the build directory * Note: if compiling in an environment with ROS, the location of the executable will be different * To run: `./viz ` -* For help, run ./viz -h +* For help, run `./viz -h` ## Command Line Arguments -* `` the hostname or IP address of the OS-1 sensor +* `` the hostname or IP address of the OS1 sensor * `` the IP to which the sensor should send data * `-m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10>` flag to set the lidar - mode (horizontal resolution and rate). Defaults to 1024x10. + mode (horizontal resolution x rotation rate). Defaults to 1024x10. ## Key bindings | key | what it does | diff --git a/ouster_viz/include/ouster/autoexposure.h b/ouster_viz/include/ouster/autoexposure.h new file mode 100644 index 00000000..f56123db --- /dev/null +++ b/ouster_viz/include/ouster/autoexposure.h @@ -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 +#include +#include + +struct AutoExposure { + private: + double lo_state = -1.0; + double hi_state = -1.0; + + public: + void operator()(Eigen::Ref key_eigen) { + const size_t n = key_eigen.rows(); + const size_t kth_extreme = n / 100; + std::vector 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); + } +}; diff --git a/ouster_viz/include/ouster/beam_uniformity.h b/ouster_viz/include/ouster/beam_uniformity.h new file mode 100644 index 00000000..3159d7e3 --- /dev/null +++ b/ouster_viz/include/ouster/beam_uniformity.h @@ -0,0 +1,84 @@ +/** + * @file + * @brief Corrects beam uniformity by minimizing median difference between rows + * + */ + +#pragma once + +#include +#include +#include + +class BeamUniformityCorrector { + private: + std::vector dark_count; + + std::vector compute_dark_count( + const Eigen::Ref image) { + const size_t image_h = image.rows(); + const size_t image_w = image.cols(); + + std::vector tmp(image_w); + std::vector 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> 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 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(new_dark_count.data(), image_h, 1)); + + Eigen::Map(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(new_dark_count.data(), image_h, 1) -= min_el; + return new_dark_count; + } + + public: + void correct(Eigen::Ref 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(dark_count.data(), image_h) *= 0.95; + Eigen::Map(dark_count.data(), image_h) += + Eigen::Map(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; + }); + } + } +}; diff --git a/ouster_viz/include/ouster/lidar_scan.h b/ouster_viz/include/ouster/lidar_scan.h index b8bf3bce..2e292d66 100644 --- a/ouster_viz/include/ouster/lidar_scan.h +++ b/ouster_viz/include/ouster/lidar_scan.h @@ -9,6 +9,7 @@ #include #include #include +#include namespace ouster { diff --git a/ouster_viz/src/main.cpp b/ouster_viz/src/main.cpp index 5b376d92..d0ce0ab4 100644 --- a/ouster_viz/src/main.cpp +++ b/ouster_viz/src/main.cpp @@ -31,6 +31,8 @@ void print_help() { << "Options:\n" << " -m <512x10 | 512x20 | 1024x10 | 1024x20 | 2048x10> : lidar mode, " "default 1024x10\n" + << " -l : use specified port for lidar data\n" + << " -i : use specified port for imu data \n" << " -f : use provided metadata file; do not configure via TCP" << std::endl; } @@ -57,11 +59,13 @@ int main(int argc, char** argv) { OS1::lidar_mode mode = OS1::lidar_mode::MODE_1024x10; bool do_config = true; // send tcp commands to configure sensor std::string metadata{}; + int lidar_port = 0; + int imu_port = 0; try { int c = 0; - while ((c = getopt(argc, argv, "hm:f:")) != -1) { + while ((c = getopt(argc, argv, "hm:l:i:f:")) != -1) { switch (c) { case 'h': print_help(); @@ -79,6 +83,12 @@ int main(int argc, char** argv) { std::exit(EXIT_FAILURE); } break; + case 'l': + lidar_port = std::stoi(optarg); + break; + case 'i': + imu_port = std::stoi(optarg); + break; case 'f': do_config = false; metadata = read_metadata(optarg); @@ -104,12 +114,19 @@ int main(int argc, char** argv) { std::shared_ptr cli; if (do_config) { - std::cout << "Configuring sensor: " << argv[optind] - << " UDP Destination:" << argv[optind + 1] << std::endl; - cli = OS1::init_client(argv[optind], argv[optind + 1], mode); + std::string os1_host = argv[optind]; + std::string os1_udp_dest = argv[optind + 1]; + std::cout << "Configuring sensor: " << os1_host + << " UDP Destination:" << os1_udp_dest << std::endl; + cli = + OS1::init_client(os1_host, os1_udp_dest, mode, + OS1::timestamp_mode::TIME_FROM_INTERNAL_OSC, lidar_port, imu_port); } else { - std::cout << "Listening for sensor data" << std::endl; - cli = OS1::init_client(); + if (lidar_port == 0) lidar_port = 7502; + if (imu_port == 0) imu_port = 7503; + std::cout << "Listening for sensor data on udp ports " << lidar_port + << " and " << imu_port << std::endl; + cli = OS1::init_client(lidar_port, imu_port); } if (!cli) { diff --git a/ouster_viz/src/viz.cpp b/ouster_viz/src/viz.cpp index 89b745b1..cadf8400 100644 --- a/ouster_viz/src/viz.cpp +++ b/ouster_viz/src/viz.cpp @@ -39,6 +39,8 @@ #include #include "colormaps.h" +#include "ouster/autoexposure.h" +#include "ouster/beam_uniformity.h" #include "ouster/lidar_scan.h" #include "ouster/os1_util.h" #include "ouster/viz.h" @@ -70,12 +72,19 @@ const std::vector>> /** * Specify what quantity to color in the point cloud visualization **/ -enum ColorMode { COLOR_Z, COLOR_INTENSITY, COLOR_ZINTENSITY, COLOR_RANGE }; +enum ColorMode { + COLOR_Z, + COLOR_INTENSITY, + COLOR_ZINTENSITY, + COLOR_RINTENSITY, + COLOR_RANGE +}; const std::vector> color_modes = { {"Z", COLOR_Z}, {"INTENSITY", COLOR_INTENSITY}, {"Z+INTENSITY", COLOR_ZINTENSITY}, + {"INTENSITY_TIMES_RANGE", COLOR_RINTENSITY}, {"RANGE", COLOR_RANGE}}; /** @@ -90,13 +99,22 @@ struct VisualizerConfig { int palette; // image color palette bool image_noise; // display noise image - double intensity_scale; double range_scale; - double noise_scale; int fraction_3d; // percent of window displaying cloud }; +/** + * Mutable state for visualizer + */ +struct VisualizerState { + AutoExposure color_intensity; + AutoExposure color_zintensity; + AutoExposure color_rintensity; + AutoExposure color_noise; + BeamUniformityCorrector buc; +}; + struct LidarScanBuffer { std::mutex ls_mtx; bool ls_dirty = true; // false if the 'back' scan is new @@ -109,6 +127,7 @@ struct LidarScanBuffer { **/ struct VizHandle { VisualizerConfig config; + VisualizerState state; LidarScanBuffer lsb; int W; int H; @@ -127,16 +146,6 @@ void update(viz::VizHandle& vh, std::unique_ptr& ls) { ls_guard.unlock(); } -/** - * Applies filter for scaling the intensity scaling factor based on their - * intensity - **/ -void color_intensity(Eigen::Ref key_eigen, - const VisualizerConfig& config) { - key_eigen *= config.intensity_scale; - key_eigen = key_eigen.max(0.0).sqrt(); -} - /** * Applies filter for scaling the intensity scaling factor based on their range **/ @@ -151,13 +160,6 @@ void color_range(Eigen::Ref range, } } -void color_noise(Eigen::Ref key_eigen, - const VisualizerConfig& config) { - double noise_scale = config.image_noise ? config.noise_scale : 0.0; - key_eigen *= noise_scale * 0.002; - key_eigen = key_eigen.max(0.0).sqrt(); -} - /** * Generates a point cloud from a lidar scan, by multiplying each pixel in the * lidar scan by a vector pointing radially outward @@ -174,8 +176,8 @@ void lidar_scan_to_point_cloud(ouster::LidarScan& ls, Points& xyz) { /** * Update scalars used to color points **/ -void update_color_key(const VisualizerConfig& config, const Points& xyz, - Eigen::Ref intensity, +void update_color_key(const VisualizerConfig& config, VisualizerState& state, + const Points& xyz, Eigen::Ref intensity, Eigen::Ref range, std::vector& color_key) { const int n = xyz.rows(); @@ -191,12 +193,18 @@ void update_color_key(const VisualizerConfig& config, const Points& xyz, break; case COLOR_INTENSITY: key_eigen = Eigen::Map(intensity.data(), n); - color_intensity(key_eigen, config); + state.color_intensity(key_eigen); break; case COLOR_ZINTENSITY: key_eigen = Eigen::Map(intensity.data(), n); - color_intensity(key_eigen, config); key_eigen += ((1.5 + xyz.col(2)) * 0.05).abs().sqrt(); + state.color_zintensity(key_eigen); + break; + case COLOR_RINTENSITY: + key_eigen = + (Eigen::Map(range.data(), n) + 3.0) * + Eigen::Map(intensity.data(), n); + state.color_rintensity(key_eigen); break; case COLOR_RANGE: key_eigen = Eigen::Map(range.data(), n); @@ -213,7 +221,8 @@ void update_color_key(const VisualizerConfig& config, const Points& xyz, void update_images( ouster::LidarScan& ls, Eigen::Array& arr, - const std::vector& px_offset, const VisualizerConfig& config) { + const std::vector& px_offset, const VisualizerConfig& config, + VisualizerState& state) { using MapXXd = Eigen::Map; using MapXXdr = Eigen::Map< Eigen::Array>; @@ -235,10 +244,16 @@ void update_images( n.row(u).head(ofs); } + Eigen::ArrayXXd noise_image = dst.bottomRows(ls.H); + state.buc.correct(noise_image); + const int N = ls.W * ls.H; color_range(Eigen::Map{arr.data(), N}, config); - color_intensity(Eigen::Map{arr.data() + N, N}, config); - color_noise(Eigen::Map{arr.data() + 2 * N, N}, config); + state.color_intensity(Eigen::Map{arr.data() + N, N}); + if (config.image_noise) { + state.color_noise(Eigen::Map{noise_image.data(), N}); + dst.bottomRows(ls.H) = noise_image; + } }; class KeyPressInteractorStyle : public vtkInteractorStyleTrackballCamera { @@ -342,7 +357,8 @@ vtkSmartPointer init_cloud_actor( } void fill_viewport(vtkSmartPointer renderer, - vtkSmartPointer image) { + vtkSmartPointer image, + const VisualizerConfig& config) { auto camera = renderer->GetActiveCamera(); double* origin = image->GetOrigin(); @@ -351,8 +367,13 @@ void fill_viewport(vtkSmartPointer renderer, double xc = origin[0] + 0.5 * (extent[0] + extent[1]) * spacing[0]; double yc = origin[1] + 0.5 * (extent[2] + extent[3]) * spacing[1]; - // double s = (extent[1] - extent[0] + 1) * spacing[0] * 0.5; double s = (extent[3] - extent[2] + 1) * spacing[1] * 0.5; + + // if the noise image is turned off, we only show the bottom two thirds + if (!config.image_noise) { + yc = origin[1] + 1.0 / 3 * (extent[2] + extent[3]) * spacing[1]; + s = (extent[3] - extent[2] + 1) * spacing[1] / 3.0; + } double d = camera->GetDistance(); camera->ParallelProjectionOn(); @@ -425,7 +446,7 @@ void run_viz(VizHandle& vh) { render_window->AddRenderer(renderer_2d); // fit image to viewport - fill_viewport(renderer_2d, image_change->GetOutput()); + fill_viewport(renderer_2d, image_change->GetOutput(), vh.config); // vtk callback that calls a std::function in client data auto fn_cb = [](vtkObject*, long unsigned int, void* clientData, void*) { @@ -436,7 +457,6 @@ void run_viz(VizHandle& vh) { // render callback std::function on_render = [&]() { - // check if we're exiting if (vh.exit) render_window_interactor->ExitCallback(); @@ -451,9 +471,11 @@ void run_viz(VizHandle& vh) { // update data backing visualization: pc_render, color_key, image_data lidar_scan_to_point_cloud(*vh.lsb.front, pc_render); - update_color_key(vh.config, pc_render, vh.lsb.front->intensity(), - vh.lsb.front->range(), color_key); - update_images(*vh.lsb.front, image_data, px_offset, vh.config); + update_color_key(vh.config, vh.state, pc_render, + vh.lsb.front->intensity(), vh.lsb.front->range(), + color_key); + update_images(*vh.lsb.front, image_data, px_offset, vh.config, + vh.state); points->Modified(); color->Modified(); @@ -481,6 +503,8 @@ void run_viz(VizHandle& vh) { renderer_2d->SetViewport(0, vh.config.fraction_3d / 100.0, 1, 1); renderer->SetViewport(0, 0, 1, vh.config.fraction_3d / 100.0); + fill_viewport(renderer_2d, image_change->GetOutput(), vh.config); + if (camera->GetParallelProjection() != vh.config.parallel) { camera->SetParallelProjection(vh.config.parallel); renderer->ResetCamera(); @@ -521,17 +545,15 @@ std::shared_ptr init_viz(int W, int H) { vh->W = W; vh->exit = false; - vh->config.palette = 1; + vh->config.palette = 5; vh->config.c_palette = 1; vh->config.point_size = 2; vh->config.color_mode = 2; vh->config.cycle_range = false; vh->config.parallel = false; - vh->config.image_noise = false; - vh->config.intensity_scale = 0.002; + vh->config.image_noise = true; vh->config.range_scale = 0.005; - vh->config.noise_scale = 1.0; vh->lsb.back = std::unique_ptr(new ouster::LidarScan(W, H)); @@ -541,5 +563,5 @@ std::shared_ptr init_viz(int W, int H) { return vh; } -} -} +} // namespace viz +} // namespace ouster