void cvTackBarEvents(int pos, void*)
{
(void)(pos);
if (iThresParam1 < 3)
iThresParam1 = 3;
if (iThresParam1 % 2 != 1)
iThresParam1++;
if (ThresParam2 < 1)
ThresParam2 = 1;
ThresParam1 = iThresParam1;
ThresParam2 = iThresParam2;
TheMarkerDetector.setThresholdParams(ThresParam1, ThresParam2);
// detect, print, get pose, and print
// detect
vector<aruco::Marker> detected_markers = TheMarkerDetector.detect(TheInputImage);
// print the markers detected that belongs to the markerset
for (auto idx : TheMarkerMapConfig.getIndices(detected_markers))
detected_markers[idx].draw(TheInputImageCopy, Scalar(0, 0, 255), 2);
// detect 3d info if possible
if (TheMSPoseTracker.isValid())
{
TheMSPoseTracker.estimatePose(detected_markers);
aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheCameraParameters, TheMSPoseTracker.getRvec(),
TheMSPoseTracker.getTvec(), TheMarkerMapConfig[0].getMarkerSize() * 2);
}
cv::imshow("in", TheInputImageCopy);
cv::imshow("thres", TheMarkerDetector.getThresholdedImage());
}
int main(int argc, char** argv)
{
try
{
CmdLineParser cml(argc, argv);
if (argc < 3 || cml["-h"])
{
cerr << "Invalid number of arguments" << endl;
cerr << "Usage: (in.avi|live) marksetconfig.yml [optional_arguments] \n\t[-c camera_intrinsics.yml] "
"\n\t[-s marker_size] \n\t[-pcd out_pcd_file_with_camera_poses] \n\t[-poses out_file_with_poses] "
"\n\t[-corner <corner_refinement_method> (0: LINES(default),1 SUBPIX) ][-h]"
<< endl;
return false;
}
TheMarkerMapConfig.readFromFile(argv[2]);
TheMarkerMapConfigFile = argv[2];
TheMarkerSize = stof(cml("-s", "1"));
// read from camera or from file
if (string(argv[1]) == "live")
{
TheVideoCapturer.open(0);
}
else
TheVideoCapturer.open(argv[1]);
// check video is open
if (!TheVideoCapturer.isOpened())
throw std::runtime_error("Could not open video");
// read first image to get the dimensions
TheVideoCapturer >> TheInputImage;
// read camera parameters if passed
if (cml["-c"])
{
TheCameraParameters.readFromXMLFile(cml("-c"));
TheCameraParameters.resize(TheInputImage.size());
}
// prepare the detector
string dict =
TheMarkerMapConfig
.getDictionary(); // see if the dictrionary is already indicated in the configuration file. It should!
if (dict.empty())
dict = "ARUCO";
TheMarkerDetector.setDictionary(
dict); /// DO NOT FORGET THAT!!! Otherwise, the ARUCO dictionary will be used by default!
if (stoi(cml("-corner", "0")) == 0)
TheMarkerDetector.setCornerRefinementMethod(MarkerDetector::LINES);
else
{
MarkerDetector::Params params = TheMarkerDetector.getParams();
params._cornerMethod = MarkerDetector::SUBPIX;
// search corner subpix in a 5x5 widow area
params._subpix_wsize = static_cast<int>((15.f / 2000.f) * float(TheInputImage.cols));
TheMarkerDetector.setParams(params);
}
// prepare the pose tracker if possible
// if the camera parameers are avaiable, and the markerset can be expressed in meters, then go
if (TheMarkerMapConfig.isExpressedInPixels() && TheMarkerSize > 0)
TheMarkerMapConfig = TheMarkerMapConfig.convertToMeters(TheMarkerSize);
cout << "TheCameraParameters.isValid()=" << TheCameraParameters.isValid() << " "
<< TheMarkerMapConfig.isExpressedInMeters() << endl;
if (TheCameraParameters.isValid() && TheMarkerMapConfig.isExpressedInMeters())
TheMSPoseTracker.setParams(TheCameraParameters, TheMarkerMapConfig);
// Create gui
cv::namedWindow("thres", 1);
cv::namedWindow("in", 1);
TheMarkerDetector.getThresholdParams(ThresParam1, ThresParam2);
iThresParam1 = static_cast<int>(ThresParam1);
iThresParam2 = static_cast<int>(ThresParam2);
cv::createTrackbar("ThresParam1", "in", &iThresParam1, 13, cvTackBarEvents);
cv::createTrackbar("ThresParam2", "in", &iThresParam2, 13, cvTackBarEvents);
char key = 0;
int index = 0;
// capture until press ESC or until the end of the video
cout << "Press 's' to start/stop video" << endl;
do
{
TheVideoCapturer.retrieve(TheInputImage);
TheInputImage.copyTo(TheInputImageCopy);
index++; // number of images captured
// Detection of the board
vector<aruco::Marker> detected_markers = TheMarkerDetector.detect(TheInputImage);
// print the markers detected that belongs to the markerset
for (auto idx : TheMarkerMapConfig.getIndices(detected_markers))
detected_markers[idx].draw(TheInputImageCopy, Scalar(0, 0, 255), 2);
// detect 3d info if possible
if (TheMSPoseTracker.isValid())
{
if (TheMSPoseTracker.estimatePose(detected_markers))
{
aruco::CvDrawingUtils::draw3dAxis(TheInputImageCopy, TheCameraParameters,
TheMSPoseTracker.getRvec(), TheMSPoseTracker.getTvec(),
TheMarkerMapConfig[0].getMarkerSize() * 2);
frame_pose_map.insert(make_pair(index, TheMSPoseTracker.getRTMatrix()));
cout << "pose rt=" << TheMSPoseTracker.getRvec() << " " << TheMSPoseTracker.getTvec() << endl;
}
}
// show input with augmented information and the thresholded image
cv::imshow("in", TheInputImageCopy);
cv::imshow("thres", TheMarkerDetector.getThresholdedImage());
key = cv::waitKey(waitTime); // wait for key to be pressed
processKey(key);
} while (key != 27 && TheVideoCapturer.grab());
// save a beatiful pcd file (pcl library) showing the results (you can use pcl_viewer to see it)
if (cml["-pcd"])
{
savePCDFile(cml("-pcd"), TheMarkerMapConfig, frame_pose_map);
}
// save the poses to a file in tum rgbd data format
if (cml["-poses"])
{
savePosesToFile(cml("-poses"), frame_pose_map);
}
}
catch (std::exception& ex)
{
cout << "Exception :" << ex.what() << endl;
}
}