JeVoisBase  1.18
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
DemoIMU.C
Go to the documentation of this file.
1 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 //
3 // JeVois Smart Embedded Machine Vision Toolkit - Copyright (C) 2016 by Laurent Itti, the University of Southern
4 // California (USC), and iLab at USC. See http://iLab.usc.edu and http://jevois.org for information about this project.
5 //
6 // This file is part of the JeVois Smart Embedded Machine Vision Toolkit. This program is free software; you can
7 // redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software
8 // Foundation, version 2. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
9 // without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public
10 // License for more details. You should have received a copy of the GNU General Public License along with this program;
11 // if not, write to the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
12 //
13 // Contact information: Laurent Itti - 3641 Watt Way, HNB-07A - Los Angeles, CA 90089-2520 - USA.
14 // Tel: +1 213 740 3527 - itti@pollux.usc.edu - http://iLab.usc.edu - http://jevois.org
15 // ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
16 /*! \file */
17 
18 #include <jevois/Core/Module.H>
19 #include <jevois/Core/ICM20948.H>
20 #include <jevois/Debug/Timer.H>
21 
23 #include <linux/videodev2.h>
24 #include <list>
25 
26 // icon by icons8
27 
28 static jevois::ParameterCategory const ParamCateg("DemoIMU Options");
29 
30 //! Parameter \relates DemoIMU
31 JEVOIS_DECLARE_PARAMETER(afac, float, "Factor applied to acceleration values for display, or 0 to not display",
32  100.0F, ParamCateg);
33 
34 //! Parameter \relates DemoIMU
35 JEVOIS_DECLARE_PARAMETER(gfac, float, "Factor applied to gyroscope values for display, or 0 to not display",
36  0.5F, ParamCateg);
37 
38 //! Parameter \relates DemoIMU
39 JEVOIS_DECLARE_PARAMETER(mfac, float, "Factor applied to magnetometer values for display, or 0 to not display",
40  3.0F, ParamCateg);
41 
42 //! Plot raw IMU readings on top of video
43 /*! As an optional hardware upgrade, one can install a global shutter sensor into JeVois (an OnSemi AR0135 1.2MP), which
44  also includes on its custom circuit board for JeVois an inertial measurement unit (IMU). The IMU is a
45  9-degrees-of-freedom (9DOF) TDK InvenSense ICM-20948 (with 3-axis accelerometer, 3-axis gyroscope, and 3-axis
46  magnetometer). This IMU also includes a digital motion processing unit (small programmable processor inside the IMU
47  chip), which allows it to compute and filter Euler angles or quaternions directly inside the IMU chip.
48 
49  This module only works with optional JeVois sensors that include an IMU! The base JeVois-A33 smart camera does not
50  have an onboard IMU.
51 
52  \youtube{MFGpN_Vp7mg}
53 
54  This module demonstrates the RAW and FIFO modes of the IMU.
55 
56  The specifications of this chip are quite impressive:
57  - 3-axis 16-bit accelerometer with full-range sensitivity selectable to +/-2g, +/-4g, +/-8g, and +/-16g.
58  - Accelerometer data rate from 4 Hz to 1125 Hz.
59  - 3-axis 16-bit gyroscope with full-range sensitivity selectable to +/-250dps (degrees/s), +/-500dps,
60  +/-1000dps, and +/-2000dps.
61  - Gyroscope data rate from 4 Hz to 1125 Hz.
62  - 3-axis 16-bit magnetometer (compass) with wide range of +/-4900uT (micro Tesla).
63  - Magnetometer data rates 10 Hz, 20 Hz, 50 Hz, or 100 Hz.
64  - 16-bit temperature sensor with readout rate of up to 8 kHz.
65  - RAW data mode (get current sensor values at any time), buffered (FIFO) data mode (sensor values accumulate into
66  a FIFO at a fixed rate), and digital motion processing mode (DMP; raw data is processed on-chip).
67  - On-chip digital motion processor (DMP) can compute, inside the IMU chip itself:
68  + quaternion 6 (uses accel + gyro),
69  + quaternion 9 (uses accel + gyro + compass),
70  + geomag quaternion (uses accel + compass),
71  + flip/pickup detection,
72  + step detection and counting,
73  + basic activity recognition: drive, walk, run, bike, tilt, still.
74 
75  With quaternions computed on-chip, with an algorithm that gets sensor data at a highly accurate, fixed rate, and
76  applies various calibrations, drift corrections, and compensations on the fly, one gets highly accurate real-time
77  estimate of the sensor's pose in the 3D world and of how it is moving.
78 
79  Note that communication with the IMU is over a 400kHz I2C bus, which may limit data readout rate depending on
80  which data is requested from the IMU.
81 
82  This IMU has 3 basic modes of operation (parameter mode, which can only be set in params.cfg):
83 
84  - RAW: One can access the latest raw sensor data at any time using the getRaw() or get() functions. This is the
85  simplest mode of operation. One disadvantage is that if you are not calling get() at a perfectly regular
86  interval, there will be some time jitter in your readouts. The IMU does not provide any time stamps for its
87  data.
88 
89  - FIFO: In this mode, data from the sensor is piled up into a 1 kbyte FIFO buffer at a precise, constant rate
90  (when all three of accelerometer, gyroscope, and magnetometer are on, the gyro rate determines the FIFO
91  buffering rate). Main advantage is that you can then read out the data without having to worry about calling
92  getRaw() or get() at a highly precise interval. But you need to be careful that the FIFO can fill up and
93  overflow very quickly when using high sensor data rates.
94 
95  - DMP: In this mode, data is captured from the sensor at an accurate, fixed rate, and is fed to the on-chip
96  digital motion processor (DMP). The DMP then computes quaternions, activity recognition, etc and pushes data
97  packets into the FIFO as results from these algorithms become available.
98 
99  @author Laurent Itti
100 
101  @videomapping YUYV 640 512 30.0 YUYV 640 480 30.0 JeVois DemoIMU
102  @email itti\@usc.edu
103  @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
104  @copyright Copyright (C) 2018 by Laurent Itti, iLab and the University of Southern California
105  @mainurl http://jevois.org
106  @supporturl http://jevois.org/doc
107  @otherurl http://iLab.usc.edu
108  @license GPL v3
109  @distribution Unrestricted
110  @restrictions None
111  \ingroup modules */
112 class DemoIMU : public jevois::Module, public jevois::Parameter<afac, gfac, mfac>
113 {
114  public:
115  //! Constructor
116  DemoIMU(std::string const & instance) : jevois::Module(instance)
117  { itsIMU = addSubComponent<jevois::ICM20948>("imu"); }
118 
119  //! Virtual destructor for safe inheritance
120  virtual ~DemoIMU() { }
121 
122  //! Processing function
123  virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
124  {
125  // Wait for next available camera image:
126  jevois::RawImage const inimg = inframe.get(true);
127  int const w = inimg.width, h = inimg.height;
128 
129  // Wait for an image from our gadget driver into which we will put our results:
130  jevois::RawImage outimg = outframe.get();
131 
132  // Enforce that the input and output formats and image sizes match:
133  outimg.require("output", w, h + 32, inimg.fmt);
134 
135  // Just copy the pixel data over:
136  memcpy(outimg.pixelsw<void>(), inimg.pixels<void>(), std::min(inimg.buf->length(), outimg.buf->length()));
137  jevois::rawimage::drawFilledRect(outimg, 0, h, w, outimg.height-h, jevois::yuyv::Black);
138 
139  // Let camera know we are done processing the input image:
140  inframe.done();
141 
142  // Get one or more IMU readings:
143  jevois::IMUdata d = itsIMU->get();
145  jevois::sformat("Accel: x=%+06.2fg y=%+06.2fg z=%+06.2fg Magn: %+09.2fuT %+09.2fuT %+09.2fuT",
146  d.ax(), d.ay(), d.az(), d.mx(), d.my(), d.mz()),
147  3, h + 3, jevois::yuyv::White);
148 
150  jevois::sformat("Gyro: x=%+07.1fdps y=%+07.1fdps z=%+07.1fdps Temp: %05.1fC %s",
151  d.gx(), d.gy(), d.gz(), d.temp(), d.magovf ? "Magn overflow" : " "),
152  3, h + 15, jevois::yuyv::White);
153 
154  itsIMUdata.push_front(d);
155 
156  // In FIFO mode at high data rates, we may have more samples in the FIFO; read a bunch:
157  while (itsIMU->dataReady() > 32) itsIMUdata.push_front(itsIMU->get());
158 
159  // Only keep as much data as we can display:
160  while (itsIMUdata.size() > w/2) itsIMUdata.pop_back();
161 
162  // Plot the IMU data:
163  float const hh = h * 0.5F; int const sz = itsIMUdata.size(); int x = w - 1;
164  // Plot so that positive values go up (so, negate all values):
165  float const a = -afac::get(); float const g = -gfac::get(); float const m = -mfac::get();
166  jevois::IMUdata const * pd = nullptr;
167 
168  for (jevois::IMUdata const & dd : itsIMUdata)
169  {
170  if (pd)
171  {
172  if (a)
173  {
174  jevois::rawimage::drawLine(outimg, x + 2, pd->ax()*a + hh, x, dd.ax()*a + hh, 1, jevois::yuyv::DarkGreen);
175  jevois::rawimage::drawLine(outimg, x + 2, pd->ay()*a + hh, x, dd.ay()*a + hh, 1, jevois::yuyv::MedGreen);
176  jevois::rawimage::drawLine(outimg, x + 2, pd->az()*a + hh, x, dd.az()*a + hh, 1, jevois::yuyv::LightGreen);
177  }
178 
179  if (g)
180  {
181  jevois::rawimage::drawLine(outimg, x + 2, pd->gx()*g + hh, x, dd.gx()*g + hh, 1, jevois::yuyv::DarkPink);
182  jevois::rawimage::drawLine(outimg, x + 2, pd->gy()*g + hh, x, dd.gy()*g + hh, 1, jevois::yuyv::MedPink);
183  jevois::rawimage::drawLine(outimg, x + 2, pd->gz()*g + hh, x, dd.gz()*g + hh, 1, jevois::yuyv::LightPink);
184  }
185 
186  if (m)
187  {
188  jevois::rawimage::drawLine(outimg, x + 2, pd->mx()*m + hh, x, dd.mx()*m + hh, 1, jevois::yuyv::DarkTeal);
189  jevois::rawimage::drawLine(outimg, x + 2, pd->my()*m + hh, x, dd.my()*m + hh, 1, jevois::yuyv::MedTeal);
190  jevois::rawimage::drawLine(outimg, x + 2, pd->mz()*m + hh, x, dd.mz()*m + hh, 1, jevois::yuyv::LightTeal);
191  }
192 
193  }
194  pd = &dd;
195  x -= 2;
196  }
197 
198  // Send the output image with our processing results to the host over USB:
199  outframe.send();
200  }
201 
202 #ifdef JEVOIS_PRO
203  //! Processing function with GUI output
204  virtual void process(jevois::InputFrame && inframe, jevois::GUIhelper & helper) override
205  {
206  static jevois::Timer timer("processing", 100, LOG_DEBUG);
207 
208  // Start the GUI frame:
209  unsigned short winw, winh;
210  bool idle = helper.startFrame(winw, winh);
211 
212  // Draw the camera frame:
213  int x = 0, y = 0; unsigned short iw = 0, ih = 0;
214  helper.drawInputFrame("camera", inframe, x, y, iw, ih);
215  helper.itext("JeVois-Pro Inertial Measurement Unit (IMU)");
216 
217  // Let camera know we are done processing the input image:
218  inframe.done();
219 
220  timer.start();
221 
222  // Get one or more IMU readings:
223  jevois::IMUdata d = itsIMU->get();
224  helper.itext(jevois::sformat("Accel: x=%+06.2fg y=%+06.2fg z=%+06.2fg "
225  "Magn: %+09.2fuT %+09.2fuT %+09.2fuT",
226  d.ax(), d.ay(), d.az(), d.mx(), d.my(), d.mz()));
227 
228  helper.itext(jevois::sformat("Gyro: x=%+07.1fdps y=%+07.1fdps z=%+07.1fdps Temp: %05.1fC %s",
229  d.gx(), d.gy(), d.gz(), d.temp(), d.magovf ? "Magn overflow" : " "));
230 
231  itsIMUdata.push_front(d);
232 
233  // In FIFO mode at high data rates, we may have more samples in the FIFO; read a bunch:
234  while (itsIMU->dataReady() > 32) itsIMUdata.push_front(itsIMU->get());
235 
236  // Only keep as much data as we can display:
237  int const count = 300;
238  while (itsIMUdata.size() > count) itsIMUdata.pop_back();
239 
240  float ax[count] = {}, ay[count] = {}, az[count] = {};
241  float gx[count] = {}, gy[count] = {}, gz[count] = {};
242  float mx[count] = {}, my[count] = {}, mz[count] = {};
243 
244  // Plot so that positive values go up (so, negate all values):
245  float const a = -afac::get(); float const g = -gfac::get(); float const m = -mfac::get();
246 
247  for (int i = count - 1; jevois::IMUdata const & dd : itsIMUdata)
248  {
249  if (a) { ax[i] = dd.ax() * a; ay[i] = dd.ay() * a; az[i] = dd.az() * a; }
250  if (g) { gx[i] = dd.gx() * g; gy[i] = dd.gy() * g; gz[i] = dd.gz() * g; }
251  if (m) { mx[i] = dd.mx() * m; my[i] = dd.my() * m; mz[i] = dd.mz() * m; }
252  --i;
253  }
254 
255  // Draw the data:
256  if (ImGui::Begin("IMU data"))
257  {
258  ImGui::PushItemWidth(ImGui::GetContentRegionAvail().x - 10.0F);
259  ImGui::Text("Acceleration X, Y, Z:");
260  ImGui::PlotLines("##AccelX", ax, count, 0, "Accel X", -100, 100);
261  ImGui::PlotLines("##AccelY", ay, count, 0, "Accel Y", -100, 100);
262  ImGui::PlotLines("##AccelZ", az, count, 0, "Accel Z", -100, 100);
263  ImGui::Separator();
264  ImGui::Text("Gyroscope X, Y, Z:");
265  ImGui::PlotLines("##GyroX", gx, count, 0, "Gyro X", -100, 100);
266  ImGui::PlotLines("##GyroY", gy, count, 0, "Gyro Y", -100, 100);
267  ImGui::PlotLines("##GyroZ", gz, count, 0, "Gyro Z", -100, 100);
268  ImGui::Separator();
269  ImGui::Text("Magnetometer X, Y, Z:");
270  ImGui::PlotLines("##MagnX", mx, count, 0, "Magn X", -100, 100);
271  ImGui::PlotLines("##MagnY", my, count, 0, "Magn Y", -100, 100);
272  ImGui::PlotLines("##MagnZ", mz, count, 0, "Magn Z", -100, 100);
273  ImGui::PopItemWidth();
274  }
275  ImGui::End();
276 
277  // Show processing fps:
278  std::string const & fpscpu = timer.stop();
279  helper.iinfo(inframe, fpscpu, winw, winh);
280 
281  // Render the image and GUI:
282  helper.endFrame();
283  }
284 #endif
285 
286  private:
287  std::shared_ptr<jevois::ICM20948> itsIMU;
288  std::list<jevois::IMUdata> itsIMUdata;
289 };
290 
291 // Allow the module to be loaded as a shared object (.so) file:
jevois::GUIhelper
jevois::RawImage::pixels
const T * pixels() const
jevois::OutputFrame
jevois::GUIhelper::startFrame
bool startFrame(unsigned short &w, unsigned short &h)
Timer.H
jevois::GUIhelper::itext
void itext(char const *txt, ImU32 const &col=IM_COL32_BLACK_TRANS, int line=-1)
Module.H
jevois::sformat
std::string sformat(char const *fmt,...) __attribute__((format(__printf__
jevois::IMUdata::gy
float & gy()
DemoIMU
Plot raw IMU readings on top of video.
Definition: DemoIMU.C:112
DemoIMU::~DemoIMU
virtual ~DemoIMU()
Virtual destructor for safe inheritance.
Definition: DemoIMU.C:120
jevois::GUIhelper::endFrame
void endFrame()
jevois::RawImage
jevois::ParameterCategory
jevois::RawImage::require
void require(char const *info, unsigned int w, unsigned int h, unsigned int f) const
jevois::RawImage::width
unsigned int width
jevois::RawImage::pixelsw
T * pixelsw()
jevois::rawimage::writeText
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
jevois
F
float F
ICM20948.H
jevois::rawimage::drawFilledRect
void drawFilledRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int col)
jevois::IMUdata::my
float & my()
jevois::Module
RawImageOps.H
jevois::GUIhelper::drawInputFrame
void drawInputFrame(char const *name, InputFrame const &frame, int &x, int &y, unsigned short &w, unsigned short &h, bool noalias=false, bool casync=false)
jevois::IMUdata::mz
float & mz()
DemoIMU::process
virtual void process(jevois::InputFrame &&inframe, jevois::OutputFrame &&outframe) override
Processing function.
Definition: DemoIMU.C:123
jevois::RawImage::height
unsigned int height
jevois::IMUdata::az
float & az()
jevois::GUIhelper::iinfo
void iinfo(jevois::InputFrame const &inframe, std::string const &fpscpu, unsigned short winw=0, unsigned short winh=0)
jevois::IMUdata::temp
float & temp()
jevois::InputFrame
jevois::RawImage::fmt
unsigned int fmt
DemoIMU::DemoIMU
DemoIMU(std::string const &instance)
Constructor.
Definition: DemoIMU.C:116
jevois::RawImage::buf
std::shared_ptr< VideoBuf > buf
h
int h
jevois::IMUdata::gz
float & gz()
jevois::IMUdata::ax
float & ax()
jevois::IMUdata::mx
float & mx()
jevois::rawimage::drawLine
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
ARtoolkit::JEVOIS_DECLARE_PARAMETER
JEVOIS_DECLARE_PARAMETER(camparams, std::string, "File stem of camera parameters, or empty. Camera resolution " "will be appended, as well as a .dat extension. For example, specifying 'camera_para' " "here and running the camera sensor at 320x240 will attempt to load " "camera_para320x240.dat from within the module's directory (if relative stem) or " "from the specified absolute location (if absolute stem).", JEVOIS_SHARE_PATH "/camera/camera_para", ParamCateg)
Parameter.
jevois::IMUdata::ay
float & ay()
jevois::IMUdata::gx
float & gx()
JEVOIS_REGISTER_MODULE
JEVOIS_REGISTER_MODULE(DemoIMU)
jevois::IMUdata::magovf
bool magovf
demo.w
w
Definition: demo.py:85
jevois::IMUdata
jevois::Timer
jevois::Component::Module
friend friend class Module