25#include <opencv2/core.hpp>
26#include <opencv2/core/utility.hpp>
27#include <opencv2/imgproc.hpp>
28#include <opencv2/calib3d.hpp>
29#include <opencv2/objdetect/charuco_detector.hpp>
39 Pattern::ChessBoard, Pattern_Values, ParamCateg);
43 aruco::Dict::D4X4_50, aruco::Dict_Values, ParamCateg);
47 "unit used here is the one that will be used once calibrated to report 3D coordinates "
48 "of objects relative to the camera",
53 "etc). The unit used here is the one that will be used once calibrated to report 3D "
54 "coordinates of objects relative to the camera",
59 "vertical tiles/disks. (Note: for asymmetric circle grid, count the number of "
60 "disks on each row, then the number of rows). The product width * height "
61 "should be the total number of tiles/disks on the grid.)",
62 { 11, 7 }, ParamCateg);
70 "not try to optimize them, i.e., assume board is exactly planar",
79 "produces significant barrel distortion, otherwise false",
104 "detected patterns so far. Click this for at least 5 images showing your calibration board "
105 "under various viewpoints. When enough boards have been successfully captured, click "
111 "enough good views of your calibration board before you can calibrate.",
176 public jevois::Parameter<pattern, dictionary, squareSize, markerSize, boardSize,
177 aspectRatio, zeroTangentDist, fixPrincipalPoint, winSize,
178 fishEye, fixK1, fixK2, fixK3, showUndistorted, grab, calibrate>
203 std::vector<cv::Point2f> pointBuf;
204 int chessBoardFlags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE;
207 if (fishEye::get() ==
false) chessBoardFlags |= cv::CALIB_CB_FAST_CHECK;
210 cv::Size bs = boardSize::get();
213 switch (pattern::get())
215 case Pattern::ChessBoard:
216 found = cv::findChessboardCorners(view, cv::Size(bs.width - 1, bs.height - 1), pointBuf, chessBoardFlags);
221 cv::cvtColor(view, viewGray, cv::COLOR_BGR2GRAY);
222 cv::cornerSubPix(viewGray, pointBuf, cv::Size(winSize::get(), winSize::get()), cv::Size(-1,-1),
223 cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.0001));
227 case Pattern::ChArUcoBoard:
230 if (! itsChArUcoDetector)
234 cv::aruco::CharucoBoard ch_board({boardSize::get().width, boardSize::get().height}, squareSize::get(),
235 markerSize::get(), dico);
236 cv::aruco::DetectorParameters detector_params;
237 detector_params.cornerRefinementMethod = int(cv::aruco::CORNER_REFINE_NONE);
239 cv::aruco::CharucoParameters charuco_params;
240 charuco_params.tryRefineMarkers =
true;
241 charuco_params.cameraMatrix = itsCameraMatrix;
242 charuco_params.distCoeffs = itsDistCoeffs;
243 itsChArUcoDetector.reset(
new cv::aruco::CharucoDetector(ch_board, charuco_params, detector_params));
247 std::vector<int> markerIds, charucoIds;
248 std::vector<std::vector<cv::Point2f> > markerCorners;
249 itsChArUcoDetector->detectBoard(view, pointBuf, charucoIds, markerCorners, markerIds);
250 size_t needed_num = (size_t)((bs.height - 1)*(bs.width - 1));
251 LINFO(
"ChArUco: detected "<< pointBuf.size() <<
" out of " << needed_num <<
" corners");
252 found = (pointBuf.size() == needed_num);
257 LINFO(
"ChArUco calibration board successfully detected");
259 itsImagePoints.push_back(pointBuf);
262 itsLastGoodView = view.clone();
263 itsLastGoodTime = std::chrono::steady_clock::now();
264 if (markerIds.size() > 0)
265 cv::aruco::drawDetectedMarkers(itsLastGoodView, markerCorners);
266 if (charucoIds.size() > 0)
267 cv::aruco::drawDetectedCornersCharuco(itsLastGoodView, pointBuf, charucoIds, cv::Scalar(255, 0, 0));
272 case Pattern::CirclesGrid:
273 found = findCirclesGrid(view, bs, pointBuf);
276 case Pattern::AsymmetricCirclesGrid:
277 found = findCirclesGrid(view, bs, pointBuf, cv::CALIB_CB_ASYMMETRIC_GRID);
282 if (pattern::get() != Pattern::ChArUcoBoard)
285 LINFO(
"Calibration board successfully detected");
287 itsImagePoints.push_back(pointBuf);
290 itsLastGoodView = view.clone();
291 itsLastGoodTime = std::chrono::steady_clock::now();
292 drawChessboardCorners(itsLastGoodView, cv::Size(bs.width - 1, bs.height - 1), cv::Mat(pointBuf), found);
295 else engine()->reportError(
"Calibration board was not detected. Make sure it is in full view with no "
296 "occlusions, reflections, strong shadows, etc.");
300 if (calibrate::get())
302 calibrate::set(
false);
304 if (itsImagePoints.size() < 5)
305 engine()->reportError(
"Need at least 5 good board views before calibration");
310 if (itsCalibrated ==
false)
312 engine()->reportError(
"Calibration failed. Let's try again.");
321 if (showUndistorted::get())
323 cv::Mat temp = view.clone();
327 cv::fisheye::estimateNewCameraMatrixForUndistortRectify(itsCameraMatrix, itsDistCoeffs, itsImageSize,
328 cv::Matx33d::eye(), newCamMat, 1);
329 cv::fisheye::undistortImage(temp, itsLastGoodView, itsCameraMatrix, itsDistCoeffs, newCamMat);
331 else cv::undistort(temp, itsLastGoodView, itsCameraMatrix, itsDistCoeffs);
333 else itsLastGoodView = view;
340 itsCalibrated =
false;
346 itsFlag = cv::fisheye::CALIB_FIX_SKEW | cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
347 if (fixK1::get()) itsFlag |= cv::fisheye::CALIB_FIX_K1;
348 if (fixK2::get()) itsFlag |= cv::fisheye::CALIB_FIX_K2;
349 if (fixK3::get()) itsFlag |= cv::fisheye::CALIB_FIX_K3;
350 if (fixPrincipalPoint::get()) itsFlag |= cv::fisheye::CALIB_FIX_PRINCIPAL_POINT;
354 if (fixPrincipalPoint::get()) itsFlag |= cv::CALIB_FIX_PRINCIPAL_POINT;
355 if (zeroTangentDist::get()) itsFlag |= cv::CALIB_ZERO_TANGENT_DIST;
356 if (aspectRatio::get()) itsFlag |= cv::CALIB_FIX_ASPECT_RATIO;
357 if (fixK1::get()) itsFlag |= cv::CALIB_FIX_K1;
358 if (fixK2::get()) itsFlag |= cv::CALIB_FIX_K2;
359 if (fixK3::get()) itsFlag |= cv::CALIB_FIX_K3;
362 std::vector<cv::Mat> rvecs, tvecs;
363 std::vector<float> reprojErrs;
364 std::vector<cv::Point3f> newObjPoints;
366 itsCalibrated =
runCalibration(rvecs, tvecs, reprojErrs, itsAvgErr, newObjPoints);
372 calib.
sensor = engine()->camerasens::get();
373 calib.
lens = engine()->cameralens::get();
374 calib.
w = itsImageSize.width;
375 calib.
h = itsImageSize.height;
380 engine()->saveCameraCalibration(calib);
386 std::vector<cv::Mat>
const & rvecs, std::vector<cv::Mat>
const & tvecs,
387 std::vector<float> & perViewErrors)
389 std::vector<cv::Point2f> imagePoints2;
390 size_t totalPoints = 0;
391 double totalErr = 0, err;
392 perViewErrors.resize(objectPoints.size());
394 for (
size_t i = 0; i < objectPoints.size(); ++i )
397 cv::fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], itsCameraMatrix, itsDistCoeffs);
399 cv::projectPoints(objectPoints[i], rvecs[i], tvecs[i], itsCameraMatrix, itsDistCoeffs, imagePoints2);
401 err = cv::norm(itsImagePoints[i], imagePoints2, cv::NORM_L2);
403 size_t n = objectPoints[i].size();
404 perViewErrors[i] = (float) std::sqrt(err*err/n);
409 return std::sqrt(totalErr / totalPoints);
417 switch (pattern::get())
419 case Pattern::ChessBoard:
420 case Pattern::CirclesGrid:
421 for (
int i = 0; i < boardsiz.height - 1; ++i)
422 for (
int j = 0; j < boardsiz.width - 1; ++j)
423 corners.push_back(cv::Point3f(j*squaresiz, i*squaresiz, 0));
426 case Pattern::ChArUcoBoard:
427 for (
int i = 0; i < boardsiz.height - 1; ++i)
428 for (
int j = 0; j < boardsiz.width - 1; ++j)
429 corners.push_back(cv::Point3f(j*squaresiz, i*squaresiz, 0));
432 case Pattern::AsymmetricCirclesGrid:
433 for (
int i = 0; i < boardsiz.height - 1; i++)
434 for (
int j = 0; j < boardsiz.width - 1; j++)
435 corners.push_back(cv::Point3f((2 * j + i % 2)*squaresiz, i*squaresiz, 0));
441 bool runCalibration(std::vector<cv::Mat> & rvecs, std::vector<cv::Mat> & tvecs, std::vector<float> & reprojErrs,
442 double & totalAvgErr, std::vector<cv::Point3f> & newObjPoints)
444 itsCameraMatrix = cv::Mat::eye(3, 3, CV_64F);
445 if (fishEye::get() ==
false && itsFlag & cv::CALIB_FIX_ASPECT_RATIO)
446 itsCameraMatrix.at<
double>(0,0) = aspectRatio::get();
447 if (fishEye::get()) itsDistCoeffs = cv::Mat::zeros(4, 1, CV_64F);
448 else itsDistCoeffs = cv::Mat::zeros(8, 1, CV_64F);
450 std::vector<std::vector<cv::Point3f> > objectPoints(1);
452 newObjPoints = objectPoints[0];
454 objectPoints.resize(itsImagePoints.size(), objectPoints[0]);
461 cv::Mat _rvecs, _tvecs;
462 rms = cv::fisheye::calibrate(objectPoints, itsImagePoints, itsImageSize, itsCameraMatrix, itsDistCoeffs,
463 _rvecs, _tvecs, itsFlag);
465 rvecs.reserve(_rvecs.rows);
466 tvecs.reserve(_tvecs.rows);
467 for (
int i = 0; i < int(objectPoints.size()); i++)
469 rvecs.push_back(_rvecs.row(i));
470 tvecs.push_back(_tvecs.row(i));
475 int iFixedPoint = -1;
476 rms = cv::calibrateCameraRO(objectPoints, itsImagePoints, itsImageSize, iFixedPoint,
477 itsCameraMatrix, itsDistCoeffs, rvecs, tvecs, newObjPoints,
478 itsFlag | cv::CALIB_USE_LU);
481 LINFO(
"Re-projection error reported by calibrateCamera: "<< rms);
483 bool ok = cv::checkRange(itsCameraMatrix) && cv::checkRange(itsDistCoeffs);
485 objectPoints.clear();
486 objectPoints.resize(itsImagePoints.size(), newObjPoints);
506 inimg.
require(
"input", w,
h, V4L2_PIX_FMT_YUYV);
509 itsImageSize.width = w; itsImageSize.height =
h;
514 outimg = outframe.get();
515 outimg.
require(
"output", w, 2*
h + 15, V4L2_PIX_FMT_YUYV);
534 int y = 15, y2 =
h + 3;
537 if (itsLastGoodView.empty() ==
false)
544 if (showUndistorted::get())
557 if (itsImagePoints.size() >= 5)
571 std::string
const & fpscpu = timer.
stop();
587 unsigned short winw, winh;
591 int x = 0, y = 0;
unsigned short iw = 0, ih = 0;
593 helper.
itext(
"JeVois-Pro Camera Calibration");
596 cv::Mat cvimg = inframe.getCvRGBp();
597 itsImageSize.width = cvimg.cols;
598 itsImageSize.height = cvimg.rows;
609 helper.
itext(
"Camera is now calibrated - average reprojection error = " + std::to_string(itsAvgErr));
610 if (showUndistorted::get())
611 helper.
itext(
"Showing undistorted view. Turn off showUndistorted to see normal view");
613 helper.
itext(
"Showing normal view. Turn on showUndistorted to see undistorted view");
615 int vx = 0, vy = 0;
unsigned short vw = 0, vh = 0;
616 helper.
drawImage(
"v", itsLastGoodView,
true , vx, vy, vw, vh,
false,
true);
621 helper.
itext(
"Good calibration board views so far: " + std::to_string(itsImagePoints.size()));
622 if (itsImagePoints.size() >= 5)
623 helper.
itext(
"Enough good board views acquired for calibration, you may grab more to improve accuracy,"
624 " or calibrate now.");
626 helper.
itext(
"Need at least 5 good board views for calibration, keep grabbing, use varied viewpoints");
629 std::chrono::duration<float>
const elapsed = std::chrono::steady_clock::now() - itsLastGoodTime;
630 if (elapsed.count() < 2.0F && itsLastGoodView.empty() ==
false)
632 helper.
itext(
"Calibration board successfully detected");
633 int vx = 0, vy = 0;
unsigned short vw = 0, vh = 0;
634 helper.
drawImage(
"v", itsLastGoodView,
true , vx, vy, vw, vh,
false,
true);
641 ImGui::SetNextWindowPos(ImVec2(100, 100), ImGuiCond_FirstUseEver);
642 ImGui::SetNextWindowSize(ImVec2(500, 400), ImGuiCond_FirstUseEver);
645 ImGui::PushStyleColor(ImGuiCol_WindowBg, 0xf0ffe0e0);
647 if (ImGui::Begin(
"Calibrate Camera Controls"))
649 int wrap = ImGui::GetWindowSize().x - ImGui::GetFontSize() * 2.0f;
650 ImGui::PushTextWrapPos(wrap);
654 ImGui::TextUnformatted(
"Camera is now calibrated and calibration was saved to disk.");
655 ImGui::TextUnformatted(
"");
656 ImGui::Text(
"Average reprojection error: %f (lower is better)", itsAvgErr);
657 ImGui::TextUnformatted(
"");
660 if (ImGui::Button(
"Grab more views")) itsCalibrated =
false;
662 else if (itsReady ==
false)
664 if (itsImagePoints.empty())
666 ImGui::TextUnformatted(
"To get started, switch to the Parameters tab of the main window, "
667 "and set some parameters.");
668 ImGui::TextUnformatted(
"");
669 ImGui::TextUnformatted(
"Critical parameters are:");
670 ImGui::TextUnformatted(
"");
672 ImGui::Text(
"pattern - choose a board type (chessboard, ChArUco, etc)");
674 ImGui::Text(
"fishEye - whether you are using a fish-eye (wide angle) lens");
676 ImGui::Text(
"squareSize - physical size in your chosen units (mm, inch, etc) of one board square");
678 ImGui::Text(
"markerSize - if using ChArUco, physical size of markers in your units");
680 ImGui::Text(
"boardSize - horizontal and vertical number of tiles");
682 ImGui::TextUnformatted(
"");
685 itsReady = ImGui::Button(
"Ready!");
689 ImGui::TextUnformatted(
"Point the camera so that you get a full view of the calibration board, "
691 if (itsImagePoints.empty() ==
false)
693 ImGui::TextUnformatted(
"");
694 ImGui::TextUnformatted(
"Move the camera around to get a variety of viewpoints.");
695 ImGui::TextUnformatted(
"Need at least 5 valid viewpoints. The more the better.");
696 ImGui::TextUnformatted(
"");
697 ImGui::Text(
"Good board views so far: %zu", itsImagePoints.size());
699 ImGui::TextUnformatted(
"");
701 if (ImGui::Button(
"Grab")) grab::set(
true);
702 if (itsImagePoints.size() >= 5)
704 ImGui::SameLine(); ImGui::TextUnformatted(
" "); ImGui::SameLine();
705 if (ImGui::Button(
"Calibrate now")) calibrate::set(
true);
712 ImGui::SameLine(); ImGui::TextUnformatted(
" "); ImGui::SameLine();
713 if (ImGui::Button(
"Start over"))
restart();
716 ImGui::PopTextWrapPos();
720 ImGui::PopStyleColor();
723 std::string
const & fpscpu = timer.
stop();
724 helper.
iinfo(inframe, fpscpu, winw, winh);
735 itsChArUcoDetector.reset();
738 itsCalibrated =
false;
739 itsImagePoints.clear();
748 bool itsReady =
false;
749 bool itsCalibrated =
false;
751 cv::Size itsImageSize;
752 std::vector<std::vector<cv::Point2f> > itsImagePoints;
753 cv::Mat itsCameraMatrix = cv::Mat::eye(3, 3, CV_64F);
754 cv::Mat itsDistCoeffs = cv::Mat::zeros(8, 1, CV_64F);
755 cv::Mat itsLastGoodView;
756 std::chrono::time_point<std::chrono::steady_clock> itsLastGoodTime;
757 std::shared_ptr<cv::aruco::CharucoDetector> itsChArUcoDetector;
758 double itsAvgErr = 0.0;
JEVOIS_REGISTER_MODULE(ArUcoBlob)
static cv::aruco::Dictionary getDictionary(aruco::Dict d)
Static method to allow anyone to get the ArUco dictionary for a given value of our dictionary paramet...
Helper module to calibrate a given sensor+lens combo, which allows ArUco and other modules to do 3D p...
JEVOIS_DECLARE_PARAMETER(fishEye, bool, "Use fisheye model. Should be true if using a wide-angle lens that " "produces significant barrel distortion, otherwise false", false, ParamCateg)
Parameter.
JEVOIS_DEFINE_ENUM_CLASS(Pattern,(ChessBoard)(ChArUcoBoard)(CirclesGrid)(AsymmetricCirclesGrid))
Enum for parameter.
JEVOIS_DECLARE_PARAMETER(aspectRatio, float, "Fixed aspect ratio value to use when non-zero, or auto when 0.0", 0.0F, ParamCateg)
Parameter.
void process_frame(cv::Mat const &view)
Process one captured image.
void calcBoardCornerPositions(cv::Size boardsiz, float squaresiz, std::vector< cv::Point3f > &corners)
JEVOIS_DECLARE_PARAMETER(fixPrincipalPoint, bool, "Fix principal point at center, otherwise find its location", true, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(fixK1, bool, "Fix (do not try to optimize) K1 radial distortion parameter", false, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(winSize, unsigned char, "Half of search window size for sub-pixel corner refinement", 11, ParamCateg)
Parameter.
double computeReprojectionErrors(std::vector< std::vector< cv::Point3f > > const &objectPoints, std::vector< cv::Mat > const &rvecs, std::vector< cv::Mat > const &tvecs, std::vector< float > &perViewErrors)
JEVOIS_DECLARE_PARAMETER(fixK2, bool, "Fix (do not try to optimize) K2 radial distortion parameter", false, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(fixK3, bool, "Fix (do not try to optimize) K3 radial distortion parameter", false, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER_WITH_CALLBACK(dictionary, aruco::Dict, "ArUco dictionary to use", aruco::Dict::D4X4_50, aruco::Dict_Values, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(showUndistorted, bool, "Once calibrated, show undistorted image instead of original capture", false, ParamCateg)
Parameter.
virtual void onParamChange(pattern const &par, Pattern const &val)
Restart when some critical params are changed:
virtual void process(jevois::InputFrame &&inframe, jevois::GUIhelper &helper) override
Processing function with zero-copy and GUI on JeVois-Pro.
bool runCalibration(std::vector< cv::Mat > &rvecs, std::vector< cv::Mat > &tvecs, std::vector< float > &reprojErrs, double &totalAvgErr, std::vector< cv::Point3f > &newObjPoints)
JEVOIS_DECLARE_PARAMETER(squareSize, float, "Size of each tile (check) in user-chosen units (e.g., mm, inch, etc). The " "unit used here is the one that will be used once calibrated to report 3D coordinates " "of objects relative to the camera", 23.0f, ParamCateg)
Parameter.
virtual void process(jevois::InputFrame &&inframe, jevois::OutputFrame &&outframe) override
Processing function with video output to USB.
JEVOIS_DECLARE_PARAMETER(markerSize, float, "ChArUco marker size in user-chosen units (e.g., mm, inch, " "etc). The unit used here is the one that will be used once calibrated to report 3D " "coordinates of objects relative to the camera", 27.0f, ParamCateg)
Parameter.
virtual void onParamChange(dictionary const &par, aruco::Dict const &val)
JEVOIS_DECLARE_PARAMETER_WITH_CALLBACK(boardSize, cv::Size, "Board size [width height] in number of horizontal and " "vertical tiles/disks. (Note: for asymmetric circle grid, count the number of " "disks on each row, then the number of rows). The product width * height " "should be the total number of tiles/disks on the grid.)", { 11, 7 }, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(calibrate, bool, "Calibrate using all the grabbed boards so far. You first need to grab " "enough good views of your calibration board before you can calibrate.", false, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER_WITH_CALLBACK(pattern, Pattern, "Type of calibration board pattern to use", Pattern::ChessBoard, Pattern_Values, ParamCateg)
Parameter.
JEVOIS_DECLARE_PARAMETER(grab, bool, "Grab one image, try to detect the calibration pattern, and add to list of " "detected patterns so far. Click this for at least 5 images showing your calibration board " "under various viewpoints. When enough boards have been successfully captured, click " "'calibrate'", false, ParamCateg)
Parameter.
virtual ~CalibrateCamera()
Virtual destructor for safe inheritance.
JEVOIS_DECLARE_PARAMETER(zeroTangentDist, bool, "Assume zero tangential distortion coefficients P1, P2 and do " "not try to optimize them, i.e., assume board is exactly planar", true, ParamCateg)
Parameter.
virtual void onParamChange(boardSize const &par, cv::Size const &val)
jevois::CameraSensor sensor
friend friend class Module
void drawInputFrame(char const *name, InputFrame const &frame, int &x, int &y, unsigned short &w, unsigned short &h, bool noalias=false, bool casync=false)
bool startFrame(unsigned short &w, unsigned short &h)
void iinfo(jevois::InputFrame const &inframe, std::string const &fpscpu, unsigned short winw=0, unsigned short winh=0)
void itext(char const *txt, ImU32 const &col=IM_COL32_BLACK_TRANS, int line=-1)
void drawImage(char const *name, RawImage const &img, int &x, int &y, unsigned short &w, unsigned short &h, bool noalias=false, bool isoverlay=false)
void require(char const *info, unsigned int w, unsigned int h, unsigned int f) const
std::string const & stop(double *seconds)
void paste(RawImage const &src, RawImage &dest, int dx, int dy)
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)
cv::Mat convertToCvBGR(RawImage const &src)
int itext(RawImage &img, std::string const &txt, int y=3, unsigned int col=jevois::yuyv::White, Font font=Font6x10)
void pasteBGRtoYUYV(cv::Mat const &src, RawImage &dst, int dx, int dy)
std::future< std::invoke_result_t< std::decay_t< Function >, std::decay_t< Args >... > > async(Function &&f, Args &&... args)
unsigned short constexpr Black
unsigned short constexpr White