27 #include <opencv2/calib3d.hpp>
28 #include <opencv2/core/eigen.hpp>
33 #include <Calibration/Calibration.h>
34 #include <Image/IplImageAdaptor.h>
36 #include <SimoxUtility/algorithm/string.h>
58 defs->optional(p.imageProviderName,
"img.ImageProviderName",
"Name of the image provider to use.");
59 defs->optional(p.referenceFrame,
"img.ReferenceFrameName",
"Name of the ReferenceFrameName");
60 defs->optional(p.agentName,
"img.AgentName",
"Name of the agent");
62 std::string pattern =
"[ k3 [, k4, k5, k6 [, s1, s2, s3, s4 ] ] ]";
64 defs->defineOptionalProperty<std::vector<std::string>>(
"cam.ExtraDistortionCoeffs", {},
65 "Optional extra distortion coefficients (which cannot be retrieved from the image provider)."
66 "\nThe expected parameters are: " + pattern +
""
67 "\nThat is, you must specifiy 1 (k3), 4 (k3-k6), or 8 (k3-k6, s1-s4) elements."
68 "\nNote: k1, k2, p1, p2 are retrieved from the image provider.")
71 defs->defineOptionalProperty<
float>(
"ar.MarkerSize", p.markerSize.load(),
72 "The side length of the marker(s)");
73 defs->optional(p.dictionary,
"ar.Dictionary",
"The ArUco dictionary.")
74 .map(
"4X4_50", cv::aruco::DICT_4X4_50)
75 .map(
"4X4_100", cv::aruco::DICT_4X4_100)
76 .map(
"4X4_250", cv::aruco::DICT_4X4_250)
77 .map(
"4X4_1000", cv::aruco::DICT_4X4_1000)
78 .map(
"5X5_50", cv::aruco::DICT_5X5_50)
79 .map(
"5X5_100", cv::aruco::DICT_5X5_100)
80 .map(
"5X5_250", cv::aruco::DICT_5X5_250)
81 .map(
"5X5_1000", cv::aruco::DICT_5X5_1000)
82 .map(
"6X6_50", cv::aruco::DICT_6X6_50)
83 .map(
"6X6_100", cv::aruco::DICT_6X6_100)
84 .map(
"6X6_250", cv::aruco::DICT_6X6_250)
85 .map(
"6X6_1000", cv::aruco::DICT_6X6_1000)
86 .map(
"7X7_50", cv::aruco::DICT_7X7_50)
87 .map(
"7X7_100", cv::aruco::DICT_7X7_100)
88 .map(
"7X7_250", cv::aruco::DICT_7X7_250)
89 .map(
"7X7_1000", cv::aruco::DICT_7X7_1000)
90 .map(
"ARUCO_ORIGINAL", cv::aruco::DICT_ARUCO_ORIGINAL)
93 defs->defineOptionalProperty<std::string>(
"ar.MarkerGeneration",
"https://chev.me/arucogen/",
94 "You can generate ArUco markers here.");
96 defs->defineOptionalProperty<
bool>(
"visu.Enabled", p.visuEnabled.load(),
97 "If true, visualize marker poses.");
104 return "ArMarkerLocalizerOpenCV";
111 *arucoDictionary = cv::aruco::getPredefinedDictionary(p.dictionary);
112 p.markerSize = getProperty<float>(
"ar.MarkerSize").getValue();
113 p.visuEnabled = getProperty<bool>(
"visu.Enabled").getValue();
116 std::string propName =
"cam.ExtraDistortionCoeffs";
117 std::vector<std::string> coeffsStr;
118 getProperty(coeffsStr,
"cam.ExtraDistortionCoeffs");
120 p.extraDistortionCoeffs.clear();
121 for (
const auto& coeffStr : coeffsStr)
125 p.extraDistortionCoeffs.push_back(std::stof(coeffStr));
127 catch (
const std::invalid_argument&)
129 ARMARX_WARNING <<
"Could not parse '" << coeffStr <<
"' as float in property " << propName <<
"."
130 <<
"\nIgnoring extra parameters.";
131 p.extraDistortionCoeffs.clear();
144 StereoCalibrationInterfacePrx calibrationProvider = StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.
proxy);
146 std::unique_ptr<CStereoCalibration> stereoCalibration(
visionx::tools::convert(calibrationProvider->getStereoCalibration()));
147 CCalibration::CCameraParameters ivtCameraParameters = stereoCalibration->GetLeftCalibration()->GetCameraParameters();
156 std::set<int> allowedSizes = {4, 5, 8, 12};
157 ARMARX_VERBOSE <<
"Got " << p.extraDistortionCoeffs.size() <<
" extra distortion coefficients.";
159 int num = int(4 + p.extraDistortionCoeffs.size());
161 <<
"Allowed sizes: " << simox::alg::join(simox::alg::multi_to_string(allowedSizes.begin(), allowedSizes.end()),
" ")
162 <<
"\n num = " << num <<
" = 4 + " << p.extraDistortionCoeffs.size() <<
" = 4 + #extra coeffs";
164 cv::Mat distortionParameters(num, 1, cv::DataType<float>::type);
166 if (!calibrationProvider->getImagesAreUndistorted())
169 for (
int i = 0; i < 4; ++i)
171 distortionParameters.at<
float>(i, 0) = ivtCameraParameters.distortion[i];
174 for (
int i = 0; size_t(i) < p.extraDistortionCoeffs.size(); ++i)
176 distortionParameters.at<
float>(4 + i, 0) = p.extraDistortionCoeffs.at(
size_t(i));
181 for (
int i = 0; i < distortionParameters.rows; ++i)
183 distortionParameters.at<
float>(i, 0) = 0;
187 this->distortionCoeffs = distortionParameters;
190 ARMARX_VERBOSE <<
"Distortion coefficients: \n" << this->distortionCoeffs;
193 cameraImages =
new CByteImage*[2];
214 ArMarkerLocalizationResultList result = localizeAllMarkersInternal();
216 std::unique_lock lock(resultMutex);
217 lastLocalizationResult = result;
224 for (
const auto& r : result)
227 .
pose(armarx::FramedPosePtr::dynamicCast(r.pose)->toEigen()));
234 ArMarkerLocalizationResultList ArMarkerLocalizerOpenCV::localizeAllMarkersInternal()
238 IplImage* imageIpl = IplImageAdaptor::Adapt(cameraImages[0]);
239 cv::Mat imageOpenCV = cv::cvarrToMat(imageIpl);
245 std::vector<int> markerIDs;
246 std::vector<std::vector<cv::Point2f>> markerCorners;
247 cv::aruco::detectMarkers(imageOpenCV, arucoDictionary, markerCorners, markerIDs, arucoParameters);
252 ARMARX_VERBOSE <<
"Localized " << markerIDs.size() <<
" markers: "
253 << simox::alg::join(simox::alg::multi_to_string(markerIDs),
" ");
256 cv::Mat outputImage = imageOpenCV.clone();
257 if (not markerIDs.empty())
259 cv::aruco::drawDetectedMarkers(outputImage, markerCorners, markerIDs);
267 std::vector<cv::Vec3d> rvecs, tvecs;
268 cv::aruco::estimatePoseSingleMarkers(markerCorners, p.markerSize, cameraMatrix, distortionCoeffs, rvecs, tvecs);
271 ARMARX_VERBOSE <<
"Estimated " << rvecs.size() <<
" marker poses.";
275 visionx::ArMarkerLocalizationResultList resultList;
277 for (
size_t i = 0; i < markerIDs.size(); ++i)
279 visionx::ArMarkerLocalizationResult& result = resultList.emplace_back();
282 Eigen::Vector3f position = Eigen::Vector3d(tvec(0), tvec(1), tvec(2)).cast<
float>();
284 cv::Mat cvOrientation;
287 cv::Rodrigues(rvecs.at(i), cvOrientation);
289 catch (
const std::exception& e)
291 ARMARX_ERROR <<
"Caught exception when running cv::aruco::Rodrigues(): \n" << e.what();
297 Eigen::Matrix3d orientation;
300 result.pose =
new armarx::FramedPose(orientation.cast<
float>(), position, p.referenceFrame, p.agentName);
301 result.id = markerIDs.at(i);
303 cv::drawFrameAxes(outputImage, cameraMatrix, distortionCoeffs, rvecs[i], tvecs[i], 0.1);
306 CByteImage* output_cimage =
new CByteImage();
309 CByteImage* result_images[2] = {cameraImages[0], output_cimage};
311 delete output_cimage;
318 if (waitForImages(500))
320 getImages(cameraImages);
321 return localizeAllMarkersInternal();
333 std::unique_lock lock(resultMutex);
334 return lastLocalizationResult;
344 tab.markerSize.setRange(1.0, 1000.0);
345 tab.markerSize.setSteps(1000);
346 tab.markerSize.setDecimals(1);
347 tab.markerSize.setValue(p.markerSize);
349 tab.visuEnabled.setValue(p.visuEnabled);
351 grid.
add(
Label(
"Marker size:"), {row, 0}).add(tab.markerSize, {row, 1});
353 grid.
add(
Label(
"Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
362 if (tab.markerSize.hasValueChanged())
364 p.markerSize = tab.markerSize.getValue();
366 if (tab.visuEnabled.hasValueChanged())
368 p.visuEnabled = tab.visuEnabled.getValue();