docs(*): update docs v2.3.8

* release/2.3.7:
  docs(*): update changelog
  chore(*): update version
  docs(*): update ORB build
  docs(*): update slam

3rdparty/easyloggingpp/.gitignore

@@ -0,0 +1,7 @@
+build-*
+*.pro.user
+.DS_Store
+release.info
+bin/*
+logs/*
+experiments/*

3rdparty/easyloggingpp/LICENSE

@@ -0,0 +1,25 @@
+The MIT License (MIT)
+
+Copyright (c) 2012-2018 Zuhd Web Services
+Copyright (c) 2012-2018 @abumusamq
+
+https://github.com/zuhd-org/
+https://zuhd.org
+https://muflihun.com
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+the Software, and to permit persons to whom the Software is furnished to do so,
+subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

CMakeLists.txt

@@ -14,7 +14,7 @@
 
 cmake_minimum_required(VERSION 3.0)
 
-project(mynteye VERSION 2.3.7 LANGUAGES C CXX)
+project(mynteye VERSION 2.3.8 LANGUAGES C CXX)
 
 include(cmake/Common.cmake)
 

README.md

@@ -1,6 +1,6 @@
 # MYNT® EYE S SDK
 
-[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.7-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
+[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.3.8-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
 
 ## Overview
 

docs/Doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.3.5
+PROJECT_NUMBER         = 2.3.8
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

docs/conf.py

@@ -24,7 +24,7 @@ copyright = '2018, MYNTAI'
 author = 'MYNTAI'
 
 # The short X.Y version
-version = '2.3.7'
+version = '2.3.8'
 # The full version, including alpha/beta/rc tags
 release = version
 

docs/requirements.txt

@@ -1,5 +1,5 @@
-breathe>=4.11.1
+breathe>=4.13
-restructuredtext-lint>=1.1.3
+restructuredtext-lint>=1.3.0
-Sphinx>=1.8.1
+Sphinx>=2.0.1
 sphinx-intl>=0.9.11
 sphinx-rtd-theme>=0.4.2

docs/src/control/imu_range.rst

@@ -15,7 +15,7 @@ To set the range of accelerometer and gyroscope, set ``Option::ACCELEROMETER_RAN
 
   For mynteye s2100/s210a, the available settings are:
   
-  * The effective range of accelerometer(unit:g): 6, 12, 24, 32.
+  * The effective range of accelerometer(unit:g): 6, 12, 24, 48.
   * The effective range of gyroscope(unit:deg/s): 250, 500, 1000, 2000, 4000.
 
 Reference Code:
@@ -50,7 +50,7 @@ s2100/s210a：
   if (!ok) return 1;
   api->ConfigStreamRequest(request);
 
-  // ACCELEROMETER_RANGE values: 6, 12, 24, 32
+  // ACCELEROMETER_RANGE values: 6, 12, 24, 48
   api->SetOptionValue(Option::ACCELEROMETER_RANGE, 6);
   // GYROSCOPE_RANGE values: 250, 500, 1000, 2000, 4000
   api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000);

docs/src/firmware/auxiliary_chip_update.rst

@@ -1,6 +1,6 @@
 .. _firmware_stm_update:
 
-How to upgrade the auxiliary chip
+How to update the Auxiliary Chip
 ==================================
 
 Update auxiliary chip(Only Support S2100/S210A)

docs/src/firmware/upgrade.rst

@@ -1,9 +1,9 @@
 .. _firmware_upgrade:
 
-How to upgrade the firmware
+How to update Main Processing Chip
-============================
+====================================
 
-Please use the MYNT EYE TOOL to upgrade the firmware.
+Please use the MYNT EYE TOOL to update main processing chip.
 
 You can download the firmware and MYNT EYE TOOL installation package in the ``Firmwares`` folder of `MYNTEYE_BOX(Download Link) <http://www.myntai.com/mynteye/s/download>`_ . The file structure is as follows:
 

docs/src/sdk/changelog.rst

@@ -3,6 +3,12 @@
 Changelog
 =========
 
+2019-05-20(v2.3.8)
+-------------------
+1. Improve VINS-Fusion supporting
+2. Improve VINS-MONO supporting
+3. Fix left/right rect image order error
+
 2019-04-19(v2.3.7)
 -------------------
 1. Improve VINS-Fusion supporting
@@ -21,7 +27,7 @@ Changelog
 
 1. Improve camera info.
 
-2. Modify SGBM/BM parameters by yaml file.
+2. Modify image algorithm parameters by yaml file.
 
 3. Add opening multi devices launch file in ROS.
 
@@ -37,9 +43,9 @@ Changelog
 2019-03-18(v2.3.4)
 -------------------
 
-1. Add API to get subsidiary chip&ISP's version(Depend on S2100/S210A 1.1 firmware & 1.0 subsidiary chip firmware).
+1. Add API to get auxiliary chip&ISP's version(Depend on S2100/S210A 1.1 firmware & 1.0 subsidiary chip firmware).
 
-2. Fix point fragment issue in BM algorithm.
+2. Fix point fragment issue in image algorithm.
 
 3. Add 376*240 resolution support to S1030(Depend on 2.4.0 firmware of S1030).
 

docs/src/sdk/platform.rst

@@ -8,7 +8,7 @@ SDK is built on CMake and can be used cross multiple platforms such as "Linux, W
 These are the platforms that can be used:
 
 * Windows 10
-* Ubuntu 18.04 / 16.04 / 14.04
+* Ubuntu 18.04.1 / 16.04.6 / 14.04.5
 * Jetson TX1/TX2 / Xavier
 * firefly RK3399
 

docs/src/slam/vins.rst

@@ -20,37 +20,40 @@ Install ROS Kinetic conveniently (if already installed, please ignore)
   wget https://raw.githubusercontent.com/oroca/oroca-ros-pkg/master/ros_install.sh && \
   chmod 755 ./ros_install.sh && bash ./ros_install.sh catkin_ws kinetic
 
-Install Ceres
+Install Docker
---------------
+---------------
 
 .. code-block:: bash
 
-  cd ~
+  sudo apt-get update
-  git clone https://ceres-solver.googlesource.com/ceres-solver
+  sudo apt-get install \
-  sudo apt-get -y install cmake libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev
+      apt-transport-https \
-  sudo add-apt-repository ppa:bzindovic/suitesparse-bugfix-1319687
+      ca-certificates \
-  sudo apt-get update && sudo apt-get install libsuitesparse-dev
+      curl \
-  mkdir ceres-bin
+      gnupg-agent \
-  cd ceres-bin
+      software-properties-common
-  cmake ../ceres-solver
+  curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add -
-  make -j3
+  sudo add-apt-repository \
-  sudo make install
+     "deb [arch=amd64] https://download.docker.com/linux/ubuntu \
+     $(lsb_release -cs) \
+     stable"
+  sudo apt-get update
+  sudo apt-get install docker-ce docker-ce-cli containerd.io
+
+Then add your account to ``docker`` group by ``sudo usermod -aG docker $YOUR_USER_NAME`` . Relaunch the terminal or logout and re-login if you get ``Permission denied`` error.
+
+To complie with docker,we recommend that you should use more than 16G RAM, or ensure that the RAM and virtual memory space is greater than 16G.
 
 Install MYNT-EYE-VINS-Sample
 ------------------------------
 
 .. code-block:: bash
 
-  mkdir -p ~/catkin_ws/src
+  git clone -b docker_feat https://github.com/slightech/MYNT-EYE-VINS-Sample.git
-  cd ~/catkin_ws/src
+  cd MYNT-EYE-VINS-Sample/docker
-  git clone https://github.com/slightech/MYNT-EYE-VINS-Sample.git
+  make build
-  cd ..
-  catkin_make
-  source devel/setup.bash
-  echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
-  source ~/.bashrc
 
-(if you fail in this step, try to find another computer with clean system or reinstall Ubuntu and ROS)
+Note that the docker building process may take a while depends on your network and machine. After VINS-Mono successfully started, open another terminal and play your bag file, then you should be able to see the result. If you need modify the code, simply run ``./run.sh LAUNCH_FILE_NAME`` after your changes.
 
 Run VINS-Mono with MYNT® EYE
 -----------------------------
@@ -61,11 +64,12 @@ Run VINS-Mono with MYNT® EYE
 
   cd (local path of MYNT-EYE-S-SDK)
   source ./wrappers/ros/devel/setup.bash
-  roslaunch mynt_eye_ros_wrapper mynteye.launch
+  roslaunch mynt_eye_ros_wrapper vins_mono.launch
 
 2. Open another terminal and run vins
 
 .. code-block:: bash
 
-  cd ~/catkin_ws
+  cd path/to/VINS-Mono/docker
-  roslaunch vins_estimator mynteye_s.launch
+  ./run.sh mynteye_s.launch
+  # ./run.sh mynteye_s2100.launch  # mono with s2100

docs/src/slam/vins_fusion.rst

@@ -20,35 +20,41 @@ Install ROS Kinetic conveniently (if already installed, please ignore)
   wget https://raw.githubusercontent.com/oroca/oroca-ros-pkg/master/ros_install.sh && \
   chmod 755 ./ros_install.sh && bash ./ros_install.sh catkin_ws kinetic
 
-Install Ceres
+Install Docker
---------------
+---------------
 
 .. code-block:: bash
 
-  cd ~
+  sudo apt-get update
-  git clone https://ceres-solver.googlesource.com/ceres-solver
+  sudo apt-get install \
-  sudo apt-get -y install cmake libgoogle-glog-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev
+      apt-transport-https \
-  sudo add-apt-repository ppa:bzindovic/suitesparse-bugfix-1319687
+      ca-certificates \
-  sudo apt-get update && sudo apt-get install libsuitesparse-dev
+      curl \
-  mkdir ceres-bin
+      gnupg-agent \
-  cd ceres-bin
+      software-properties-common
-  cmake ../ceres-solver
+  curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add -
-  make -j3
+  sudo add-apt-repository \
-  sudo make install
+     "deb [arch=amd64] https://download.docker.com/linux/ubuntu \
+     $(lsb_release -cs) \
+     stable"
+  sudo apt-get update
+  sudo apt-get install docker-ce docker-ce-cli containerd.io
+
+Then add your account to ``docker`` group by ``sudo usermod -aG docker $YOUR_USER_NAME`` . Relaunch the terminal or logout and re-login if you get ``Permission denied`` error.
+
+To complie with docker,we recommend that you should use more than 16G RAM, or ensure that the RAM and virtual memory space is greater than 16G.
+
 
 Install MYNT-EYE-VINS-FUSION-Samples
 -------------------------------------
 
 .. code-block:: bash
 
-  mkdir -p ~/catkin_ws/src
-  cd ~/catkin_ws/src
   git clone https://github.com/slightech/MYNT-EYE-VINS-FUSION-Samples.git
-  cd ..
+  cd MYNT-EYE-VINS-FUSION-Samples/docker
-  catkin_make
+  make build
-  source ~/catkin_ws/devel/setup.bash
 
-(if you fail in this step, try to find another computer with clean system or reinstall Ubuntu and ROS)
+Note that the docker building process may take a while depends on your network and machine. After VINS-Mono successfully started, open another terminal and play your bag file, then you should be able to see the result. If you need modify the code, simply run ``./run.sh LAUNCH_FILE_NAME`` after your changes.
 
 Run VINS-FUSION with MYNT® EYE
 -------------------------------
@@ -65,9 +71,7 @@ Run VINS-FUSION with MYNT® EYE
 
 .. code-block:: bash
 
-  cd ~/catkin_ws/src
+  cd path/to/this_repo/docker
-  source ./devel/setup.bash
+  ./run.sh mynteye-s/mynt_stereo_imu_config.yaml  # Stereo fusion
-  roslaunch vins mynteye-s-stereo.launch  # Stereo fusion / Stereo+imu fusion
+  # ./run.sh mynteye-s2100/mynt_stereo_config.yaml # Stereo fusion with mynteye-s2100
-  # roslaunch vins mynteye-s-mono-imu.launch  # mono+imu fusion
+  # ./run.sh mynteye-s2100/mynt_stereo_imu_config.yaml # Stereo+imu fusion with mynteye-s2100
-  # roslaunch vins mynteye-s2100-mono-imu.launch  # mono+imu fusion with mynteye-s2100
-  # roslaunch vins mynteye-s2100-stereo.launch  # Stereo fusion / Stereo+imu fusion with mynteye-s2100

docs/src/wrapper/ros.rst

@@ -51,9 +51,17 @@ The ROS file is structured like follows:
   <sdk>/wrappers/ros/
   ├─src/
   │  └─mynt_eye_ros_wrapper/
+  │     ├─config/
+  │     │  ├─device/
+  │     │     ├─standard.yaml   # S1030
+  │     │     └─standard2.yaml  # S2100/S210A
+  │     │  ├─laserscan/
+  │     │  ├─process/
+  │     │  └─...
   │     ├─launch/
   │     │  ├─display.launch
   │     │  └─mynteye.launch
+  │     │  └─...
   │     ├─msg/
   │     ├─rviz/
   │     ├─src/
@@ -64,35 +72,41 @@ The ROS file is structured like follows:
   │     └─package.xml
   └─README.md
 
-In ``mynteye.launch``, you can configure the topics and frame_ids, decide which data to enable, and set the control options. Please set ``gravity`` to the local gravity acceleration.
+In ``mynteye.launch``, you can configure the topics and frame_ids, decide which data to enable, ``standard.yaml`` (standard2.yaml is S2100/S210A config file) can set parameters for device. Please set ``gravity`` to the local gravity acceleration.
+
+
+standard.yaml/standard2.yaml:
 
 .. code-block:: xml
 
   # s2100/s210a modify frame/resolution
-  <arg name="request_index" default="$(arg index_s2_2)" />
+  standard2/request_index: 2
 
   # s1030 modify frame/imu hz
-  <!-- standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60} -->
+  # standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60}
-  <arg name="standard/frame_rate" default="-1" />
+  standard/frame_rate: -1
-  <!-- <arg name="standard/frame_rate" default="25" /> -->
+  # standard/frame_rate: 25
 
-  <!-- standard/imu_frequency range: {100,200,250,333,500} -->
+  # standard/imu_frequency range: {100,200,250,333,500}
-  <arg name="standard/imu_frequency" default="-1" />
+  standard/imu_frequency: -1
-  <!-- <arg name="standard/imu_frequency" default="200" /> -->
+  # standard/imu_frequency: 200
   ...
 
   # s2100 modify brightness
-  <!-- standard2/brightness range: [0,240] -->
+  # standard2/brightness range: [0,240]
-  <arg name="standard2/brightness" default="-1" />
+  standard2/brightness: -1
-  <!-- <arg name="standard2/brightness" default="120" /> -->
+  # standard2/brightness: 120
   ...
 
   # s210a modify brightness
-  <!-- standard210a/brightness range: [0,240] -->
+  # standard210a/brightness range: [0,240]
-  <arg name="standard210a/brightness" default="-1" />
+  standard210a/brightness: -1
-  <!-- <arg name="standard210a/brightness" default="120" /> -->
+  # standard210a/brightness: 120
   ...
 
+
+mynteye.launch:
+
 .. code-block:: xml
 
   <arg name="gravity" default="9.8" />

platforms/projects/cmake/CMakeLists.txt

@@ -0,0 +1,74 @@
+cmake_minimum_required(VERSION 3.0)
+
+project(mynteye_demo VERSION 1.0.0 LANGUAGES C CXX)
+
+# flags
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3")
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c++11 -march=native")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -march=native")
+
+# packages
+
+if(MSVC)
+  set(SDK_ROOT "$ENV{MYNTEYES_SDK_ROOT}")
+  if(SDK_ROOT)
+    message(STATUS "MYNTEYES_SDK_ROOT: ${SDK_ROOT}")
+    list(APPEND CMAKE_PREFIX_PATH
+      "${SDK_ROOT}/lib/cmake"
+      "${SDK_ROOT}/3rdparty/opencv/build"
+    )
+  else()
+    message(FATAL_ERROR "MYNTEYES_SDK_ROOT not found, please install SDK firstly")
+  endif()
+endif()
+
+## mynteye
+
+find_package(mynteye REQUIRED)
+message(STATUS "Found mynteye: ${mynteye_VERSION}")
+
+# When SDK build with OpenCV, we can add WITH_OPENCV macro to enable some
+# features depending on OpenCV, such as ToMat().
+if(mynteye_WITH_OPENCV)
+  add_definitions(-DWITH_OPENCV)
+endif()
+
+## OpenCV
+
+# Set where to find OpenCV
+#set(OpenCV_DIR "/usr/share/OpenCV")
+
+# When SDK build with OpenCV, we must find the same version here.
+find_package(OpenCV REQUIRED)
+message(STATUS "Found OpenCV: ${OpenCV_VERSION}")
+
+# targets
+
+include_directories(
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+## mynteye_demo
+
+add_executable(mynteye_demo mynteye_demo.cc)
+target_link_libraries(mynteye_demo mynteye ${OpenCV_LIBS})
+
+
+# Build
+#   mkdir _build
+#   cd _build
+#
+#   # win 
+#   cmake -G "Visual Studio 15 2017 Win64" ..
+#   msbuild.exe ALL_BUILD.vcxproj /property:Configuration=Release
+#
+#   .\Release\mynteye_demo.exe
+#   
+#   # unix
+#   cmake ..
+#   make
+#
+#   ./mynteye_demo

platforms/projects/cmake/mynteye_demo.cc

@@ -0,0 +1,61 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include <stdio.h>
+#include <opencv2/highgui/highgui.hpp>
+
+#include "mynteye/api/api.h"
+
+MYNTEYE_USE_NAMESPACE
+
+int main(int argc, char const *argv[]) {
+  auto &&api = API::Create(0, nullptr);
+  if (!api) return 1;
+
+  bool ok;
+  auto &&request = api->SelectStreamRequest(&ok);
+  if (!ok) return 1;
+  api->ConfigStreamRequest(request);
+  api->Start(Source::VIDEO_STREAMING);
+
+  double fps;
+  double t = 0.01;
+  std::cout << "fps:" << std::endl;
+
+  cv::namedWindow("frame");
+
+  while (true) {
+    api->WaitForStreams();
+
+    auto &&left_data = api->GetStreamData(Stream::LEFT);
+    auto &&right_data = api->GetStreamData(Stream::RIGHT);
+
+    cv::Mat img;
+    if (!left_data.frame.empty() && !right_data.frame.empty()) {
+      double t_c = cv::getTickCount() / cv::getTickFrequency();
+      fps = 1.0/(t_c - t);
+      printf("\b\b\b\b\b\b\b\b\b%.2f", fps);
+      t = t_c;
+      cv::hconcat(left_data.frame, right_data.frame, img);
+      cv::imshow("frame", img);
+    }
+
+    char key = static_cast<char>(cv::waitKey(1));
+    if (key == 27 || key == 'q' || key == 'Q') {  // ESC/Q
+      break;
+    }
+  }
+
+  api->Stop(Source::VIDEO_STREAMING);
+  return 0;
+}

samples/tutorials/control/imu_range.cc

@@ -42,7 +42,7 @@ int main(int argc, char *argv[]) {
 
   // Set imu range for S2000/S2100/S210A
   if (model == Model::STANDARD2 || model == Model::STANDARD210A) {
-    // ACCELEROMETER_RANGE values: 6, 12, 24, 32
+    // ACCELEROMETER_RANGE values: 6, 12, 24, 48
     api->SetOptionValue(Option::ACCELEROMETER_RANGE, 6);
     // GYROSCOPE_RANGE values: 250, 500, 1000, 2000, 4000
     api->SetOptionValue(Option::GYROSCOPE_RANGE, 1000);

src/mynteye/api/processor.cc

@@ -300,10 +300,10 @@ api::StreamData Processor::GetStreamData(const Stream &stream) {
       }
       auto streams = getTargetStreams();
       if (output != nullptr) {
-        int num = 0;
         for (auto it : streams) {
           if (it.stream == stream) {
-            if (num == 1) {
+            if (it.stream == Stream::LEFT ||
+                it.stream == Stream::LEFT_RECTIFIED) {
               if (!is_enable_cd) {
                 if (output->first_data &&
                   last_frame_id_cd == output->first_data->frame_id) {
@@ -313,7 +313,8 @@ api::StreamData Processor::GetStreamData(const Stream &stream) {
                 last_frame_id_cd = output->first_data->frame_id;
               }
               return obj_data_first(output);
-            } else {
+            } else if (it.stream == Stream::RIGHT ||
+                       it.stream == Stream::RIGHT_RECTIFIED) {
               // last_frame_id_cd = output->second_data->frame_id;
               if (!is_enable_cd) {
                 if (output->second_data &&
@@ -325,7 +326,6 @@ api::StreamData Processor::GetStreamData(const Stream &stream) {
               return obj_data_second(output);
             }
           }
-          num++;
         }
       }
       VLOG(2) << "Frame not ready now";

src/mynteye/device/channel/bytes.cc

@@ -122,6 +122,13 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
       }
     }
     i += 72;
+  } else {
+    // assembly
+    for (std::size_t j = 0; j < 3; j++) {
+      for (std::size_t k = 0; k < 3; k++) {
+        in->assembly[j][k] = 0.0;
+      }
+    }
   }
   // drift
   for (std::size_t j = 0; j < 3; j++) {
@@ -155,6 +162,20 @@ std::size_t from_data(ImuIntrinsics *in, const std::uint8_t *data,
       in->z[j] = _from_data<double>(data + i + j * 8);
     }
     i += 16;
+  } else {
+    // temperature drift
+    // x
+    for (std::size_t j = 0; j < 2; j++) {
+      in->x[j] = 0.0;
+    }
+    // y
+    for (std::size_t j = 0; j < 2; j++) {
+      in->y[j] = 0.0;
+    }
+    // z
+    for (std::size_t j = 0; j < 2; j++) {
+      in->z[j] = 0.0;
+    }
   }
 
   return i;

src/mynteye/types.cc

@@ -340,14 +340,6 @@ std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfo &info) {
 std::ostream &operator<<(std::ostream &os, const CameraROSMsgInfoPair &info) {
   os << "left:\n" << info.left << std::endl;
   os << "right:\n" << info.right << std::endl;
-  os << "base R:";
-  for (size_t i = 0; i < 9; i++)
-    os << info.R[i] << ",";
-  os << std::endl << "base P:";
-  for (size_t i = 0; i < 12; i++)
-    os << info.P[i] << ",";
-  os << std::endl;
-
   return os;
 }
 

wrappers/ros/src/mynt_eye_ros_wrapper/config/slam/vins_fusion.yaml

@@ -19,4 +19,4 @@
 standard2/request_index: 1
 
 # standard/frame_rate range: {10,15,20,25,30,35,40,45,50,55,60}
-standard/frame_rate: 20
+standard/frame_rate: 15

wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch

@@ -103,6 +103,16 @@
         - 'image_transport/compressedDepth'
       </rosparam>
     </group>
+    <group ns="/mynteye/right_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="/mynteye/left_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
     <group ns="$(arg left_mono_topic)">
       <rosparam param="disable_pub_plugins">
         - 'image_transport/compressedDepth'

wrappers/ros/src/mynt_eye_ros_wrapper/launch/slam/vins_mono.launch

@@ -0,0 +1,129 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="mynteye" default="mynteye" />
+
+  <!-- node params -->
+
+  <arg name="left_topic" default="left/image_raw" />
+  <arg name="right_topic" default="right/image_raw" />
+  <arg name="left_rect_topic" default="left_rect/image_rect" />
+  <arg name="right_rect_topic" default="right_rect/image_rect" />
+  <arg name="disparity_topic" default="disparity/image_raw" />
+  <arg name="disparity_norm_topic" default="disparity/image_norm" />
+  <arg name="depth_topic" default="depth/image_raw" />
+  <arg name="points_topic" default="points/data_raw" />
+
+  <arg name="left_mono_topic" default="left/image_mono" />
+  <arg name="right_mono_topic" default="right/image_mono" />
+
+  <arg name="imu_topic" default="imu/data_raw" />
+  <arg name="temperature_topic" default="temperature/data_raw" />
+
+  <arg name="base_frame_id" default="$(arg mynteye)_link" />
+  <arg name="left_frame_id" default="$(arg mynteye)_left_frame" />
+  <arg name="right_frame_id" default="$(arg mynteye)_right_frame" />
+  <arg name="left_rect_frame_id" default="$(arg mynteye)_left_rect_frame" />
+  <arg name="right_rect_frame_id" default="$(arg mynteye)_right_rect_frame" />
+  <arg name="disparity_frame_id" default="$(arg mynteye)_disparity_frame" />
+  <arg name="disparity_norm_frame_id" default="$(arg mynteye)_disparity_norm_frame" />
+  <arg name="points_frame_id" default="$(arg mynteye)_points_frame" />
+  <arg name="depth_frame_id" default="$(arg mynteye)_depth_frame" />
+
+  <arg name="temperature_frame_id" default="$(arg mynteye)_temperature_frame" />
+
+  <arg name="gravity" default="9.8" />
+
+  <arg name="mesh_file" default="S1030-0315.obj" />
+
+  <!-- Push down all topics/nodelets into "mynteye" namespace -->
+  <group ns="$(arg mynteye)">
+
+    <!-- mynteye_wrapper_node -->
+    <node name="mynteye_wrapper_node" pkg="mynt_eye_ros_wrapper" type="mynteye_wrapper_node" output="screen" respawn="true" respawn_delay="5">
+
+      <!-- node params -->
+
+      <param name="left_topic" value="$(arg left_topic)" />
+      <param name="right_topic" value="$(arg right_topic)" />
+      <param name="left_rect_topic" value="$(arg left_rect_topic)" />
+      <param name="right_rect_topic" value="$(arg right_rect_topic)" />
+      <param name="disparity_topic" value="$(arg disparity_topic)" />
+      <param name="disparity_norm_topic" value="$(arg disparity_norm_topic)" />
+      <param name="points_topic" value="$(arg points_topic)" />
+      <param name="depth_topic" value="$(arg depth_topic)" />
+      <param name="mesh_file" value="$(arg mesh_file)" />
+
+      <param name="left_mono_topic" value="$(arg left_mono_topic)" />
+      <param name="right_mono_topic" value="$(arg right_mono_topic)" />
+
+      <param name="imu_topic" value="$(arg imu_topic)" />
+      <param name="temperature_topic" value="$(arg temperature_topic)" />
+
+      <param name="base_frame_id" value="$(arg base_frame_id)" />
+      <param name="left_frame_id" value="$(arg left_frame_id)" />
+      <param name="right_frame_id" value="$(arg right_frame_id)" />
+      <param name="left_rect_frame_id" value="$(arg left_rect_frame_id)" />
+      <param name="right_rect_frame_id" value="$(arg right_rect_frame_id)" />
+      <param name="disparity_frame_id" value="$(arg disparity_frame_id)" />
+      <param name="disparity_norm_frame_id" value="$(arg disparity_norm_frame_id)" />
+      <param name="points_frame_id" value="$(arg points_frame_id)" />
+      <param name="depth_frame_id" value="$(arg depth_frame_id)" />
+
+      <param name="temperature_frame_id" value="$(arg temperature_frame_id)" />
+
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard.yaml" command="load" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/device/standard2.yaml" command="load" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/process/process_config.yaml" command="load" />
+      <rosparam file="$(find mynt_eye_ros_wrapper)/config/slam/vins_fusion.yaml" command="load" />
+
+      <param name="gravity" value="$(arg gravity)" />
+    </node>
+
+    <!-- disable compressed depth plugin for image topics -->
+    <group ns="$(arg left_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg left_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_mono_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg right_rect_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg disparity_norm_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="$(arg depth_topic)">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+  </group> <!-- mynteye -->
+</launch>

wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_1.launch

@@ -103,6 +103,16 @@
         - 'image_transport/compressedDepth'
       </rosparam>
     </group>
+    <group ns="/mynteye/right_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="/mynteye/left_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
     <group ns="$(arg left_mono_topic)">
       <rosparam param="disable_pub_plugins">
         - 'image_transport/compressedDepth'

wrappers/ros/src/mynt_eye_ros_wrapper/launch/sub/mynteye_2.launch

@@ -103,6 +103,16 @@
         - 'image_transport/compressedDepth'
       </rosparam>
     </group>
+    <group ns="/mynteye/right_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
+    <group ns="/mynteye/left_rect_mono">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressedDepth'
+      </rosparam>
+    </group>
     <group ns="$(arg left_mono_topic)">
       <rosparam param="disable_pub_plugins">
         - 'image_transport/compressedDepth'

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -104,7 +104,7 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
   ros::Time hardTimeToSoftTime(std::uint64_t _hard_time) {
     static bool isInited = false;
-    static double soft_time_begin(0);
+    static std::uint32_t soft_time_begin(0);
     static std::uint64_t hard_time_begin(0);
 
     if (false == isInited) {
@@ -113,11 +113,31 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       isInited = true;
     }
 
+    std::uint64_t time_ns_detal = (_hard_time - hard_time_begin);
+    std::uint64_t time_ns_detal_s = time_ns_detal / 1000000;
+    std::uint64_t time_ns_detal_ns = time_ns_detal % 1000000;
+
     return ros::Time(
-        static_cast<double>(soft_time_begin +
+                     soft_time_begin + time_ns_detal_s,
-        static_cast<double>(_hard_time - hard_time_begin) * 0.000001f));
+                     time_ns_detal_ns * 1000);
   }
 
+  // ros::Time hardTimeToSoftTime(std::uint64_t _hard_time) {
+  //   static bool isInited = false;
+  //   static double soft_time_begin(0);
+  //   static std::uint64_t hard_time_begin(0);
+
+  //   if (false == isInited) {
+  //     soft_time_begin = ros::Time::now().toSec();
+  //     hard_time_begin = _hard_time;
+  //     isInited = true;
+  //   }
+
+  //   return ros::Time(
+  //       static_cast<double>(soft_time_begin +
+  //       static_cast<double>(_hard_time - hard_time_begin) * 0.000001f));
+  // }
+
   inline bool is_overflow(std::uint64_t now,
       std::uint64_t pre) {
     static std::uint64_t unit =
@@ -1302,6 +1322,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     sensor_msgs::CameraInfo *camera_info = new sensor_msgs::CameraInfo();
     camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info);
     auto info_pair = api_->GetCameraROSMsgInfoPair();
+    camera_info->width = info_pair->left.width;
+    camera_info->height = info_pair->left.height;
     if (is_intrinsics_enable_) {
       if (stream == Stream::RIGHT ||
           stream == Stream::RIGHT_RECTIFIED) {