179 memcpy(outimg.
pixelsw<
void>(), inimg.
pixels<
void>(), std::min(inimg.
buf->length(), outimg.
buf->length()));
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);
190 static cv::Vec3d tvec(0.0, 0.0, 0.5);
191 float const hsz = 0.075F;
192 static int pickup = 0;
193 long quat[3] = { 0, 0, 0 };
196 bool has_accel =
false, has_gyro =
false, has_cpass =
false;
201 while (itsIMU->dataReady())
203 d = itsIMU->getDMP(); ++npkt;
250 if (qtype.empty() ==
false)
255 q.w() = std::sqrt(1.0 - q.squaredNorm());
259 jevois::sformat(
": x=%+09.6f y=%+09.6f z=%+09.6f", q.x(), q.y(), q.z()),
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);
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));
278 std::vector<cv::Point2f> imagePoints;
279 cv::projectPoints(axisPoints, rvec, tvec, itsCalib.
camMatrix, itsCalib.
distCoeffs, imagePoints);
283 int(imagePoints[1].x + 0.5F),
int(imagePoints[1].y + 0.5F),
289 int(imagePoints[2].x + 0.5F),
int(imagePoints[2].y + 0.5F),
295 int(imagePoints[3].x + 0.5F),
int(imagePoints[3].y + 0.5F),
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));
311 std::vector<cv::Point2f> cuf;
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)));
348 if (has_accel || has_gyro || has_cpass)
357 dd.
ax(), dd.
ay(), dd.
az()),
362 dd.
gx(), dd.
gy(), dd.
gz()),
367 dd.
mx(), dd.
my(), dd.
mz()),
375 if (pickup > 0) --pickup;