28 #include <Eigen/src/Geometry/Transform.h>
30 #include <opencv2/calib3d.hpp>
31 #include <opencv2/core/eigen.hpp>
32 #include <opencv2/highgui.hpp>
35 #include <SimoxUtility/algorithm/string.h>
45 #include <Calibration/Calibration.h>
46 #include <Image/IplImageAdaptor.h>
64 p.imageProviderName,
"img.ImageProviderName",
"Name of the image provider to use.");
66 p.referenceFrame,
"img.ReferenceFrameName",
"Name of the ReferenceFrameName");
67 defs->optional(p.agentName,
"img.AgentName",
"Name of the agent");
68 defs->optional(p.cameraIndex,
"img.cameraIndex",
"Camera index. Left = 0, Right = 1, ...");
70 std::string pattern =
"[ k3 [, k4, k5, k6 [, s1, s2, s3, s4 ] ] ]";
72 defs->defineOptionalProperty<std::vector<std::string>>(
73 "cam.ExtraDistortionCoeffs",
75 "Optional extra distortion coefficients (which cannot be retrieved from the image "
77 "\nThe expected parameters are: " +
80 "\nThat is, you must specifiy 1 (k3), 4 (k3-k6), or 8 (k3-k6, s1-s4) elements."
81 "\nNote: k1, k2, p1, p2 are retrieved from the image provider.")
84 defs->defineOptionalProperty<
float>(
85 "ar.MarkerSize", p.markerSize.load(),
"The side length of the marker(s)");
86 defs->optional(p.dictionary,
"ar.Dictionary",
"The ArUco dictionary.")
87 .map(
"4X4_50", cv::aruco::DICT_4X4_50)
88 .map(
"4X4_100", cv::aruco::DICT_4X4_100)
89 .map(
"4X4_250", cv::aruco::DICT_4X4_250)
90 .map(
"4X4_1000", cv::aruco::DICT_4X4_1000)
91 .map(
"5X5_50", cv::aruco::DICT_5X5_50)
92 .map(
"5X5_100", cv::aruco::DICT_5X5_100)
93 .map(
"5X5_250", cv::aruco::DICT_5X5_250)
94 .map(
"5X5_1000", cv::aruco::DICT_5X5_1000)
95 .map(
"6X6_50", cv::aruco::DICT_6X6_50)
96 .map(
"6X6_100", cv::aruco::DICT_6X6_100)
97 .map(
"6X6_250", cv::aruco::DICT_6X6_250)
98 .map(
"6X6_1000", cv::aruco::DICT_6X6_1000)
99 .map(
"7X7_50", cv::aruco::DICT_7X7_50)
100 .map(
"7X7_100", cv::aruco::DICT_7X7_100)
101 .map(
"7X7_250", cv::aruco::DICT_7X7_250)
102 .map(
"7X7_1000", cv::aruco::DICT_7X7_1000)
103 .map(
"ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL);
105 defs->defineOptionalProperty<std::string>(
"ar.MarkerGeneration",
106 "https://chev.me/arucogen/",
107 "You can generate ArUco markers here.");
109 defs->defineOptionalProperty<
bool>(
110 "visu.Enabled", p.visuEnabled.load(),
"If true, visualize marker poses.");
123 return "ArMarkerLocalizerOpenCV";
130 *arucoDictionary = cv::aruco::getPredefinedDictionary(p.dictionary);
131 p.markerSize = getProperty<float>(
"ar.MarkerSize").getValue();
134 p.visuEnabled = getProperty<bool>(
"visu.Enabled").getValue();
137 std::string propName =
"cam.ExtraDistortionCoeffs";
138 std::vector<std::string> coeffsStr;
139 getProperty(coeffsStr,
"cam.ExtraDistortionCoeffs");
141 p.extraDistortionCoeffs.clear();
142 for (
const auto& coeffStr : coeffsStr)
146 p.extraDistortionCoeffs.push_back(std::stof(coeffStr));
148 catch (
const std::invalid_argument&)
150 ARMARX_WARNING <<
"Could not parse '" << coeffStr <<
"' as float in property "
151 << propName <<
"." <<
"\nIgnoring extra parameters.";
152 p.extraDistortionCoeffs.clear();
164 robot = virtualRobotReaderPlugin->get().getRobot(p.agentName);
167 referenceNode = robot->getRobotNode(p.referenceFrame);
173 StereoCalibrationInterfacePrx calibrationProvider =
174 StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.
proxy);
176 std::unique_ptr<CStereoCalibration> stereoCalibration(
178 CCalibration::CCameraParameters ivtCameraParameters =
179 stereoCalibration->GetLeftCalibration()->GetCameraParameters();
188 std::set<int> allowedSizes = {4, 5, 8, 12};
190 <<
" extra distortion coefficients.";
192 int num = int(4 + p.extraDistortionCoeffs.size());
196 simox::alg::multi_to_string(allowedSizes.begin(), allowedSizes.end()),
" ")
197 <<
"\n num = " << num <<
" = 4 + " << p.extraDistortionCoeffs.size()
198 <<
" = 4 + #extra coeffs";
200 cv::Mat distortionParameters(num, 1, cv::DataType<float>::type);
202 if (!calibrationProvider->getImagesAreUndistorted())
205 for (
int i = 0; i < 4; ++i)
207 distortionParameters.at<
float>(i, 0) = ivtCameraParameters.distortion[i];
210 for (
int i = 0; size_t(i) < p.extraDistortionCoeffs.size(); ++i)
212 distortionParameters.at<
float>(4 + i, 0) =
213 p.extraDistortionCoeffs.at(
size_t(i));
218 for (
int i = 0; i < distortionParameters.rows; ++i)
220 distortionParameters.at<
float>(i, 0) = 0;
224 this->distortionCoeffs = distortionParameters;
227 ARMARX_VERBOSE <<
"Distortion coefficients: \n" << this->distortionCoeffs;
230 cameraImages =
new CByteImage*[2];
252 ArMarkerLocalizationResultList result = localizeAllMarkersInternal();
254 std::unique_lock lock(resultMutex);
255 lastLocalizationResult = result;
262 const Eigen::Isometry3f global_T_frame{referenceNode->getGlobalPose()};
264 for (
const auto& r : result)
266 const Eigen::Isometry3f frame_T_marker{
267 armarx::FramedPosePtr::dynamicCast(r.pose)->toEigen()};
268 const Eigen::Isometry3f global_T_marker = global_T_frame * frame_T_marker;
278 ArMarkerLocalizationResultList
279 ArMarkerLocalizerOpenCV::localizeAllMarkersInternal()
285 cameraImages[0]->bytesPerPixel = 3;
286 cameraImages[1]->bytesPerPixel = 3;
288 IplImage* imageIpl = IplImageAdaptor::Adapt(cameraImages[p.cameraIndex]);
289 cv::Mat imageOpenCV = cv::cvarrToMat(imageIpl);
298 std::vector<int> markerIDs;
299 std::vector<std::vector<cv::Point2f>> markerCorners;
300 cv::aruco::detectMarkers(
301 imageOpenCV, arucoDictionary, markerCorners, markerIDs, arucoParameters);
306 ARMARX_VERBOSE <<
"Localized " << markerIDs.size() <<
" markers: "
307 << simox::alg::join(simox::alg::multi_to_string(markerIDs),
" ");
310 cv::Mat outputImage = imageOpenCV.clone();
311 if (not markerIDs.empty())
313 cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIDs);
321 std::vector<cv::Vec3d> rvecs, tvecs;
322 cv::aruco::estimatePoseSingleMarkers(
323 markerCorners, p.markerSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
326 ARMARX_VERBOSE <<
"Estimated " << rvecs.size() <<
" marker poses.";
330 visionx::ArMarkerLocalizationResultList resultList;
332 for (
size_t i = 0; i < markerIDs.size(); ++i)
334 visionx::ArMarkerLocalizationResult& result = resultList.emplace_back();
337 Eigen::Vector3f position = Eigen::Vector3d(tvec(0), tvec(1), tvec(2)).cast<
float>();
339 cv::Mat cvOrientation;
342 cv::Rodrigues(rvecs.at(i), cvOrientation);
344 catch (
const std::exception& e)
346 ARMARX_ERROR <<
"Caught exception when running cv::aruco::Rodrigues(): \n"
353 Eigen::Matrix3d orientation;
357 orientation.cast<
float>(), position, p.referenceFrame, p.agentName);
358 result.id = markerIDs.at(i);
360 cv::drawFrameAxes(outputImage, cameraMatrix, distortionCoeffs, rvecs[i], tvecs[i], 0.1);
363 CByteImage* output_cimage =
364 new CByteImage(outputImage.size[0], outputImage.size[1], CByteImage::ImageType::eRGB24);
367 CByteImage* result_images[2] = {cameraImages[0], output_cimage};
369 delete output_cimage;
373 visionx::ArMarkerLocalizationResultList
376 if (waitForImages(500))
378 getImages(cameraImages);
379 return localizeAllMarkersInternal();
388 ArMarkerLocalizationResultList
391 std::unique_lock lock(resultMutex);
392 return lastLocalizationResult;
403 tab.markerSize.setRange(1.0, 1000.0);
404 tab.markerSize.setSteps(1000);
405 tab.markerSize.setDecimals(1);
406 tab.markerSize.setValue(p.markerSize);
408 tab.visuEnabled.setValue(p.visuEnabled);
410 grid.
add(
Label(
"Marker size:"), {row, 0}).add(tab.markerSize, {row, 1});
412 grid.
add(
Label(
"Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
422 if (tab.markerSize.hasValueChanged())
424 p.markerSize = tab.markerSize.getValue();
426 if (tab.visuEnabled.hasValueChanged())
428 p.visuEnabled = tab.visuEnabled.getValue();