JeVoisBase  1.22
JeVois Smart Embedded Machine Vision Toolkit Base Modules
Share this page:
Loading...
Searching...
No Matches
DemoDMP.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/Engine.H>
22
24#include <linux/videodev2.h>
25#include <list>
26
27#include <Eigen/Geometry>
28#include <opencv2/calib3d/calib3d.hpp>
29#include <opencv2/core/eigen.hpp>
30
31// icon by icons8
32
33//! Plot results of processing IMU data with the on-chip Digital Motion Processor (DMP)
34/*! As an optional hardware upgrade, one can install a global shutter sensor into JeVois (an OnSemi AR0135 1.2MP), which
35 also includes on its custom circuit board for JeVois an inertial measurement unit (IMU). The IMU is a
36 9-degrees-of-freedom (9DOF) TDK InvenSense ICM-20948 (with 3-axis accelerometer, 3-axis gyroscope, and 3-axis
37 magnetometer). This IMU also includes a digital motion processing unit (small programmable processor inside the IMU
38 chip), which allows it to compute and filter quaternions directly inside the IMU chip.
39
40 This module only works with optional JeVois sensors that include an IMU! The base JeVois-A33 smart camera does not
41 have an onboard IMU.
42
43 \youtube{MFGpN_Vp7mg}
44
45 This module demonstrates the digital motion processing (DMP) mode of the IMU.
46
47 The specifications of this chip are quite impressive:
48 - 3-axis 16-bit accelerometer with full-range sensitivity selectable to +/-2g, +/-4g, +/-8g, and +/-16g.
49 - Accelerometer data rate from 4 Hz to 1125 Hz.
50 - 3-axis 16-bit gyroscope with full-range sensitivity selectable to +/-250dps (degrees/s), +/-500dps,
51 +/-1000dps, and +/-2000dps.
52 - Gyroscope data rate from 4 Hz to 1125 Hz.
53 - 3-axis 16-bit magnetometer (compass) with wide range of +/-4900uT (micro Tesla).
54 - Magnetometer data rates 10 Hz, 20 Hz, 50 Hz, or 100 Hz.
55 - 16-bit temperature sensor with readout rate of up to 8 kHz.
56 - RAW data mode (get current sensor values at any time), buffered (FIFO) data mode (sensor values accumulate into
57 a FIFO at a fixed rate), and digital motion processing mode (DMP; raw data is processed on-chip).
58 - On-chip digital motion processor (DMP) can compute, inside the IMU chip itself:
59 + quaternion 6 (uses accel + gyro),
60 + quaternion 9 (uses accel + gyro + compass),
61 + geomag quaternion (uses accel + compass),
62 + flip/pickup detection,
63 + step detection and counting,
64 + basic activity recognition: drive, walk, run, bike, tilt, still.
65
66 With quaternions computed on-chip, with an algorithm that gets sensor data at a highly accurate, fixed rate, and
67 applies various calibrations, drift corrections, and compensations on the fly, one gets highly accurate real-time
68 estimate of the sensor's pose in the 3D world and of how it is moving.
69
70 Note that communication with the IMU is over a 400kHz I2C bus, which may limit data readout rate depending on
71 which data is requested from the IMU.
72
73 This IMU has 3 basic modes of operation (parameter mode, which can only be set in params.cfg):
74
75 - RAW: One can access the latest raw sensor data at any time using the getRaw() or get() functions. This is the
76 simplest mode of operation. One disadvantage is that if you are not calling get() at a perfectly regular
77 interval, there will be some time jitter in your readouts. The IMU does not provide any time stamps for its
78 data.
79
80 - FIFO: In this mode, data from the sensor is piled up into a 1 kbyte FIFO buffer at a precise, constant rate
81 (when all three of accelerometer, gyroscope, and magnetometer are on, the gyro rate determines the FIFO
82 buffering rate). Main advantage is that you can then read out the data without having to worry about calling
83 getRaw() or get() at a highly precise interval. But you need to be careful that the FIFO can fill up and
84 overflow very quickly when using high sensor data rates.
85
86 - DMP: In this mode, data is captured from the sensor at an accurate, fixed rate, and is fed to the on-chip
87 digital motion processor (DMP). The DMP then computes quaternions, activity recognition, etc and pushes data
88 packets into the FIFO as results from these algorithms become available.
89
90 Low-level tweaking of the IMU and DMP
91 -------------------------------------
92
93 The ICM-20948 IMU chip is a very sophisticated and complex computer on its own right. There are many settings and
94 modes that are supported. Unfortunately, the full documentation is not open. But you can obtain it after
95 registration with TDK InvenSense. We provide the following user-level functions in the command-line interface of
96 JeVois to allow you to get/set raw registers of the IMU and DMP:
97
98 - setimureg \a reg \a val - set raw IMU register \a reg to value \a val
99 - getimureg \a reg - get value of raw IMU register \a reg
100 - setimuregs \a reg \a num \a val1 ... \a valn - set array of raw IMU register values
101 - getimuregs \a reg \a num - get array of raw IMU register values
102 - setdmpreg \a reg \a val - set raw DMP register \a reg to value \a val
103 - getdmpreg \a reg - get value of raw DMP register \a reg
104 - setdmpregs \a reg \a num \a val1 ... \a valn - set array of raw DMP register values
105 - getdmpregs \a reg \a num - get array of raw DMP register values
106
107 The registers and values can be specified in decimal, hex (with prefix 0x), or octal (with prefix 0 - beware of this
108 and be careful). The values returned are always in hex (with no prefix). Note that the above functions are not
109 activated by default, to prevent mistakes by rookie users. To activate them, issue a `setpar camreg 1` on the JeVois
110 console.
111
112 For example, to set the output data rate (ODR) of the DMP quaternion-9; this is in DMP register DMP_ODR_QUAT9 = 168
113 (see definitions in file ICM20948_regs.H of the JeVois core software):
114
115 \verbatim
116 setpar camreg 1
117 getdmpreg 168 # shows default divider value of 5; ODR is DMP engine rate / (1 + divider)
118 setdmpreg 168 2
119 \endverbatim
120
121 To see the effect, you should first turn on FSYNC as one of the DMP outputs (e.g., set `dmp` parameter to QF). Then
122 turn on parameter `pktdbg` and turn on log messages to USB see how many quaternion packets (long packets that start
123 with 4 0) you get between two FSYNC packets (short packets that start with 0 8 8 0). By default it is about 2 (with
124 camera sensor at 25fps and DMP engine running at 56Hz). Then increase the `arate` and `grate` to 112 Hz. You should
125 still get about 2 quaternion packets between two FSYNC packets. Finally, apply the above DMP register change. You
126 should now see 4 quaternion packets between two FSYNC packets. Note that sometimes quaternion and FSYNC packets get
127 combined.
128
129 As you experiment, keep in mind that the IMU can easily saturate the 400 kHz bandwidth of the i2c bus that connects
130 it to the JeVois main processor. If you start getting errors in the console from IMUdata::parsePacket() then likely
131 your data rate is too high for the i2c bus. One of the nice things about the DMP is that you can still run the
132 quaternion computation very fast (e.g., at 450 Hz or even 1125 Hz) while maintaining a low output data rate.
133
134 @author Laurent Itti
135
136 @videomapping YUYV 640 520 25.0 YUYV 640 480 25.0 JeVois DemoDMP
137 @email itti\@usc.edu
138 @address University of Southern California, HNB-07A, 3641 Watt Way, Los Angeles, CA 90089-2520, USA
139 @copyright Copyright (C) 2018 by Laurent Itti, iLab and the University of Southern California
140 @mainurl http://jevois.org
141 @supporturl http://jevois.org/doc
142 @otherurl http://iLab.usc.edu
143 @license GPL v3
144 @distribution Unrestricted
145 @restrictions None
146 \ingroup modules */
148{
149 public:
150 //! Constructor
151 DemoDMP(std::string const & instance) : jevois::Module(instance)
152 {
153 itsIMU = addSubComponent<jevois::ICM20948>("imu");
154 }
155
156 //! Load camera calibration on init
157 void postInit()
158 {
159 itsCalib = engine()->loadCameraCalibration();
160 }
161
162 //! Virtual destructor for safe inheritance
163 virtual ~DemoDMP() { }
164
165 //! Processing function
166 virtual void process(jevois::InputFrame && inframe, jevois::OutputFrame && outframe) override
167 {
168 // Wait for next available camera image:
169 jevois::RawImage const inimg = inframe.get(true);
170 int const w = inimg.width, h = inimg.height;
171
172 // Wait for an image from our gadget driver into which we will put our results:
173 jevois::RawImage outimg = outframe.get();
174
175 // Enforce that the input and output formats and image sizes match:
176 outimg.require("output", w, h + 40, inimg.fmt);
177
178 // Just copy the pixel data over:
179 memcpy(outimg.pixelsw<void>(), inimg.pixels<void>(), std::min(inimg.buf->length(), outimg.buf->length()));
181
182 // Let camera know we are done processing the input image:
183 inframe.done();
184
185 // Note: we get different packets for steps, activity, and for quaternions. So let's cache the data:
186 static long step_ts = 0;
187 static long steps = 0;
188 static std::vector<std::string> bac;
189 static cv::Vec3d rvec(0.0, 0.0, 0.0); // Euler angles computed from quaternion
190 static cv::Vec3d tvec(0.0, 0.0, 0.5); // Place cube 0.5 meters in front of the camera
191 float const hsz = 0.075F; // half size of cube, in meters
192 static int pickup = 0; // Number of frames remaining to show pickup message
193 long quat[3] = { 0, 0, 0 }; // Our latest quaternion
194 std::string qtype;
196 bool has_accel = false, has_gyro = false, has_cpass = false;
197 float fsync = -1.0F;
198
199 // Get as many IMU readings as we can so we keep the FIFO empty:
200 jevois::DMPdata d; int npkt = 0;
201 while (itsIMU->dataReady())
202 {
203 d = itsIMU->getDMP(); ++npkt;
204
205 // ---------- Quaternions using 9 DOF:
207 {
208 quat[0] = d.quat9[0]; quat[1] = d.quat9[1]; quat[2] = d.quat9[2];
209 qtype = "Quat-9";
210 }
211
212 // ---------- Quaternions using 6 DOF:
214 {
215 quat[0] = d.quat6[0]; quat[1] = d.quat6[1]; quat[2] = d.quat6[2];
216 qtype = "Quat-6";
217 }
218
219 // ---------- Quaternions using GeoMag:
221 {
222 quat[0] = d.geomag[0]; quat[1] = d.geomag[1]; quat[2] = d.geomag[2];
223 qtype = "GeoMag";
224 }
225
226 // ---------- Step detection:
227 if (d.header1 & JEVOIS_DMP_PED_STEPDET) { step_ts = d.stepts; steps += d.steps; }
228
229 // ---------- Activity recognition:
230 if (d.header2 & JEVOIS_DMP_ACT_RECOG) bac = d.activity2();
231
232 // ---------- Pickup/flip detection:
233 if (d.header2 & JEVOIS_DMP_FLIP_PICKUP) pickup = 20;
234
235 // ---------- FSYNC detection:
236 if (d.header2 & JEVOIS_DMP_FSYNC) fsync = d.fsync_us();
237
238 // ---------- Raw accel, gyro, mag:
240 { has_accel = true; rd.ax() = d.accel[0]; rd.ay() = d.accel[1]; rd.az() = d.accel[2]; }
241
242 if (d.header1 & JEVOIS_DMP_GYRO)
243 { has_gyro = true; rd.gx() = d.gyro[0]; rd.gy() = d.gyro[1]; rd.gz() = d.gyro[2]; }
244
246 { has_cpass = true; rd.mx() = d.cpass[0]; rd.my() = d.cpass[1]; rd.mz() = d.cpass[2]; }
247 }
248
249 // ---------- Display a cube rotated by our latest quaternion:
250 if (qtype.empty() == false)
251 {
252 Eigen::Quaterniond q(0.0, d.fix2float(quat[0]), d.fix2float(quat[1]), d.fix2float(quat[2]));
253
254 // Compute the scalar part, which is not returned by the IMU, to get a normalized quaternion:
255 q.w() = std::sqrt(1.0 - q.squaredNorm());
256
257 // Display it:
258 jevois::rawimage::writeText(outimg, qtype +
259 jevois::sformat(": x=%+09.6f y=%+09.6f z=%+09.6f", q.x(), q.y(), q.z()),
260 3, h + 3, jevois::yuyv::White);
261
262 // Compute OpenCV rvec for use with projectPoints() below:
263 // You could also compute Euler angles: Eigen::Vector3f eig_rvec = q.toRotationMatrix().eulerAngles(0, 1, 2);
264 Eigen::Matrix3d eig_rmat = q.toRotationMatrix();
265 cv::Mat rmat(3, 3, CV_64FC1); cv::eigen2cv(eig_rmat, rmat);
266 cv::Rodrigues(rmat, rvec);
267
268 // Project axis points:
269 std::vector<cv::Point3f> axisPoints;
270 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, 0.0F));
271 axisPoints.push_back(cv::Point3f(hsz * 1.5F, 0.0F, 0.0F));
272 axisPoints.push_back(cv::Point3f(0.0F, hsz * 1.5F, 0.0F));
273 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, hsz * 1.5F));
274 axisPoints.push_back(cv::Point3f(hsz * 1.55F, 0.0F, 0.0F));
275 axisPoints.push_back(cv::Point3f(0.0F, hsz * 1.55F, 0.0F));
276 axisPoints.push_back(cv::Point3f(0.0F, 0.0F, hsz * 1.55F));
277
278 std::vector<cv::Point2f> imagePoints;
279 cv::projectPoints(axisPoints, rvec, tvec, itsCalib.camMatrix, itsCalib.distCoeffs, imagePoints);
280
281 // Draw axis lines
282 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
283 int(imagePoints[1].x + 0.5F), int(imagePoints[1].y + 0.5F),
285 jevois::rawimage::writeText(outimg, "X", int(imagePoints[4].x - 4.0F), int(imagePoints[4].y - 9.5F),
287
288 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
289 int(imagePoints[2].x + 0.5F), int(imagePoints[2].y + 0.5F),
291 jevois::rawimage::writeText(outimg, "Y", int(imagePoints[5].x - 4.0F), int(imagePoints[5].y - 9.5F),
293
294 jevois::rawimage::drawLine(outimg, int(imagePoints[0].x + 0.5F), int(imagePoints[0].y + 0.5F),
295 int(imagePoints[3].x + 0.5F), int(imagePoints[3].y + 0.5F),
297 jevois::rawimage::writeText(outimg, "Z", int(imagePoints[6].x - 4.0F), int(imagePoints[6].y - 9.5F),
299
300 // Also draw a cube:
301 std::vector<cv::Point3f> cubePoints;
302 cubePoints.push_back(cv::Point3f(-hsz, -hsz, -hsz));
303 cubePoints.push_back(cv::Point3f(hsz, -hsz, -hsz));
304 cubePoints.push_back(cv::Point3f(hsz, hsz, -hsz));
305 cubePoints.push_back(cv::Point3f(-hsz, hsz, -hsz));
306 cubePoints.push_back(cv::Point3f(-hsz, -hsz, hsz));
307 cubePoints.push_back(cv::Point3f(hsz, -hsz, hsz));
308 cubePoints.push_back(cv::Point3f(hsz, hsz, hsz));
309 cubePoints.push_back(cv::Point3f(-hsz, hsz, hsz));
310
311 std::vector<cv::Point2f> cuf;
312 cv::projectPoints(cubePoints, rvec, tvec, itsCalib.camMatrix, itsCalib.distCoeffs, cuf);
313
314 // Round all the coordinates:
315 std::vector<cv::Point> cu;
316 for (auto const & p : cuf) cu.push_back(cv::Point(int(p.x + 0.5F), int(p.y + 0.5F)));
317
318 // Draw cube lines:
319 jevois::rawimage::drawLine(outimg, cu[0].x, cu[0].y, cu[1].x, cu[1].y, 2, jevois::yuyv::LightGreen);
320 jevois::rawimage::drawLine(outimg, cu[1].x, cu[1].y, cu[2].x, cu[2].y, 2, jevois::yuyv::LightGreen);
321 jevois::rawimage::drawLine(outimg, cu[2].x, cu[2].y, cu[3].x, cu[3].y, 2, jevois::yuyv::LightGreen);
322 jevois::rawimage::drawLine(outimg, cu[3].x, cu[3].y, cu[0].x, cu[0].y, 2, jevois::yuyv::LightGreen);
323 jevois::rawimage::drawLine(outimg, cu[4].x, cu[4].y, cu[5].x, cu[5].y, 2, jevois::yuyv::LightGreen);
324 jevois::rawimage::drawLine(outimg, cu[5].x, cu[5].y, cu[6].x, cu[6].y, 2, jevois::yuyv::LightGreen);
325 jevois::rawimage::drawLine(outimg, cu[6].x, cu[6].y, cu[7].x, cu[7].y, 2, jevois::yuyv::LightGreen);
326 jevois::rawimage::drawLine(outimg, cu[7].x, cu[7].y, cu[4].x, cu[4].y, 2, jevois::yuyv::LightGreen);
327 jevois::rawimage::drawLine(outimg, cu[0].x, cu[0].y, cu[4].x, cu[4].y, 2, jevois::yuyv::LightGreen);
328 jevois::rawimage::drawLine(outimg, cu[1].x, cu[1].y, cu[5].x, cu[5].y, 2, jevois::yuyv::LightGreen);
329 jevois::rawimage::drawLine(outimg, cu[2].x, cu[2].y, cu[6].x, cu[6].y, 2, jevois::yuyv::LightGreen);
330 jevois::rawimage::drawLine(outimg, cu[3].x, cu[3].y, cu[7].x, cu[7].y, 2, jevois::yuyv::LightGreen);
331 }
332
333 // ---------- Step detection:
334 jevois::rawimage::writeText(outimg, jevois::sformat("Steps: %6zu (last at %010zu)", steps, step_ts),
335 3, h + 15, jevois::yuyv::White);
336
337 // ---------- FSYNC detection:
338 if (fsync >= 0.0F) jevois::rawimage::writeText(outimg, jevois::sformat("FSYNC: %4d", int(fsync + 0.499F)),
339 w - 11*6, h + 15, jevois::yuyv::White);
340
341 // ---------- Activity recognition:
342 if (bac.size()) jevois::rawimage::writeText(outimg, jevois::join(bac, ", "), 290, h + 15, jevois::yuyv::White);
343
344 // ---------- Pickup/flip detection:
345 if (pickup) jevois::rawimage::writeText(outimg, "pickup/flip", 214, h + 15, jevois::yuyv::White);
346
347 // ---------- Show raw accel/gyro/compass:
348 if (has_accel || has_gyro || has_cpass)
349 {
350 // Convert from raw data to scaled data:
351 jevois::IMUdata dd(rd, itsIMU->arange::get(), itsIMU->grange::get());
352
353 // Display the ones that are on:
354 if (has_accel)
356 jevois::sformat("Acc: x=%+06.2fg y=%+06.2fg z=%+06.2fg",
357 dd.ax(), dd.ay(), dd.az()),
358 333, h + 3, jevois::yuyv::White);
359 if (has_gyro)
361 jevois::sformat("Gyr: x=%+07.1fdps y=%+07.1fdps z=%+07.1fdps",
362 dd.gx(), dd.gy(), dd.gz()),
363 3, h + 27, jevois::yuyv::White);
364 if (has_cpass)
366 jevois::sformat("Mag: %+09.2fuT %+09.2fuT %+09.2fuT",
367 dd.mx(), dd.my(), dd.mz()),
368 333, h + 27, jevois::yuyv::White);
369 }
370
371 // ---------- Show number of IMU packets processed:
372 jevois::rawimage::writeText(outimg, jevois::sformat("%03d pkts", npkt), 592, h + 3, jevois::yuyv::White);
373
374 // Decay counters for some ephemerous messages:
375 if (pickup > 0) --pickup;
376
377 // Send the output image with our processing results to the host over USB:
378 outframe.send();
379 }
380
381 private:
382 std::shared_ptr<jevois::ICM20948> itsIMU;
384};
385
386// Allow the module to be loaded as a shared object (.so) file:
JEVOIS_REGISTER_MODULE(ArUcoBlob)
int h
Plot results of processing IMU data with the on-chip Digital Motion Processor (DMP)
Definition DemoDMP.C:148
virtual ~DemoDMP()
Virtual destructor for safe inheritance.
Definition DemoDMP.C:163
void postInit()
Load camera calibration on init.
Definition DemoDMP.C:157
virtual void process(jevois::InputFrame &&inframe, jevois::OutputFrame &&outframe) override
Processing function.
Definition DemoDMP.C:166
DemoDMP(std::string const &instance)
Constructor.
Definition DemoDMP.C:151
friend friend class Module
unsigned int fmt
T const * pixels() const
unsigned int width
unsigned int height
void require(char const *info, unsigned int w, unsigned int h, unsigned int f) const
std::shared_ptr< VideoBuf > buf
#define JEVOIS_DMP_ACT_RECOG
#define JEVOIS_DMP_ACCEL
#define JEVOIS_DMP_QUAT6
#define JEVOIS_DMP_GEOMAG
#define JEVOIS_DMP_PED_STEPDET
#define JEVOIS_DMP_GYRO
#define JEVOIS_DMP_CPASS
#define JEVOIS_DMP_FLIP_PICKUP
#define JEVOIS_DMP_QUAT9
#define JEVOIS_DMP_FSYNC
void writeText(RawImage &img, std::string const &txt, int x, int y, unsigned int col, Font font=Font6x10)
void drawFilledRect(RawImage &img, int x, int y, unsigned int w, unsigned int h, unsigned int col)
void drawLine(RawImage &img, int x1, int y1, int x2, int y2, unsigned int thick, unsigned int col)
std::string join(std::vector< T > const &tokens, std::string const &delimiter)
std::string sformat(char const *fmt,...) __attribute__((format(__printf__
unsigned short constexpr MedPurple
unsigned short constexpr Black
unsigned short constexpr White
unsigned short constexpr MedGreen
unsigned short constexpr MedGrey
unsigned short constexpr LightGreen
unsigned short steps
std::vector< std::string > activity2()
unsigned short header1
unsigned short header2
unsigned long stepts
float fsync_us() const
static float fix2float(long val)