void DocumentMarkerController::removeMarkers(DocumentMarker::MarkerTypes markerTypes)
{
if (!possiblyHasMarkers(markerTypes))
return;
ASSERT(!m_markers.isEmpty());
// outer loop: process each markered node in the document
MarkerMap markerMapCopy = m_markers;
MarkerMap::iterator end = markerMapCopy.end();
for (MarkerMap::iterator i = markerMapCopy.begin(); i != end; ++i)
removeMarkersFromList(i->first.get(), i->second, markerTypes);
m_possiblyExistingMarkerTypes.remove(markerTypes);
}
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());
}
void
Sdf_MarkerUtils<Spec>::SetMarkers(
Spec* owner, const MarkerMap& markers)
{
// Explicitly check permission here to ensure that any editing operation
// (even no-ops) trigger an error.
if (not owner->PermissionToEdit()) {
TF_CODING_ERROR("Set %s: Permission denied",
_MarkerPolicy::GetMarkerDescription());
return;
}
// Validate all connection paths; we don't want to author any changes if
// an invalid path exists in the map.
TF_FOR_ALL(newMarker, markers) {
const SdfAllowed isValidPath =
_MarkerPolicy::IsValidConnectionPath(newMarker->first);
if (not isValidPath) {
TF_CODING_ERROR("Set %s: %s",
_MarkerPolicy::GetMarkerDescription(),
isValidPath.GetWhyNot().c_str());
return;
}
}
// Replace all markers; clear out all current markers and add in new
// markers from the given map.
SdfChangeBlock block;
const SdfPathVector markerPaths = GetMarkerPaths(*owner);
TF_FOR_ALL(oldPath, markerPaths) {
if (markers.find(*oldPath) == markers.end()) {
ClearMarker(owner, *oldPath);
}
}
TF_FOR_ALL(newMarker, markers) {
SetMarker(owner, newMarker->first, newMarker->second);
}
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;
}
}
void Coordinator::TestAll() {
cout << "Testing Point2D...";
Point2D p = Point2D(03235, 23281, 2372);
Point2D q = Point2D(p.toString());
assert(p.x == q.x && p.y == q.y && p.id == q.id);
q = Point2D(03235, 23281, "2372");
assert(p.x == q.x && p.y == q.y && p.id == q.id);
cout << "ok" << endl;
// cout << "Testing UUO" << endl;
// Uuo u = new Uuo();
cout << "Testing BehaviorOrder...";
BehaviorOrder bo = BehaviorOrder("foobar");
BehaviorOrder bo2 = BehaviorOrder("nadda");
bo2.fromString(bo.toString());
assert(bo2.toString() == bo.toString());
cout << "ok" << endl;
cout << "Testing WaypointOrder...";
WaypointOrder wo = WaypointOrder(Point2D(32, 23, 101));
WaypointOrder wo2 = WaypointOrder(Point2D(-1, -1, -1));
wo2.fromString(wo.toString());
assert(wo.toString() == wo2.toString());
cout << "ok" << endl;
cout << "Testing MarkerMap...";
MarkerMap m = MarkerMap();
//assert(m.toString() == "");
Uuo a = Uuo(3, 38.83, 1, 0.238);
Uuo b = Uuo(2, -28.28, 328.2, 0.0001);
Uuo c = Uuo(-13823, 0, 0, 0.00002);
Uuo d = Uuo(23811, 32.282, 382, 32.0);
m._map.insert(pair<int,Uuo>(3, a));
m._map.insert(pair<int,Uuo>(2, b));
m._map.insert(pair<int,Uuo>(-13823, c));
m._map.insert(pair<int,Uuo>(3282, d));
m._map.insert(pair<int,Uuo>(0, Uuo(328, 23, 2383, 1.0)));
MarkerMap m2 = MarkerMap(m.toString());
// cout << m.toString() << endl;
// cout << "A: " << m2._map[3].toString() << endl; cout << "B: " << a.toString() << endl;
assert(m2._map[3].toString() == a.toString());
assert(m2._map[2].toString() == b.toString());
assert(m2._map[-13823].toString() == c.toString());
// should !=, because ID of Uuo didn't match inserted ID
assert(m2._map[3282].toString() != d.toString());
cout << "ok" << endl;
cout << "Testing MarkerMap PriorityQueue...";
MarkerMap m3 = MarkerMap("id=1,x=-10,y=-88,pH=0.0261963,cc=0,mh=.o:id=10,x=-88,y=-79,pH=0.0261963,cc=0,mh=.o:id=20,x=33,y=-145,pH=0.1,cc=1,mh=.:id=31,x=187,y=-104,pH=0.0261963,cc=0,mh=.o:id=38,x=362,y=-77,pH=0.0261963,cc=0,mh=.o:id=76,x=96,y=-116,pH=0.132275,cc=0,mh=.:id=84,x=-40,y=-81,pH=0.0261963,cc=0,mh=.o:id=95,x=383,y=-87,pH=0.463468,cc=0,mh=.x:id=144,x=338,y=-143,pH=0.0261963,cc=0,mh=.o:id=202,x=143,y=-122,pH=0.132275,cc=1,mh=.:id=204,x=282,y=-104,pH=0.0261963,cc=0,mh=.o:id=232,x=22,y=-86,pH=0.0261963,cc=0,mh=.o:id=245,x=382,y=-132,pH=0.0261963,cc=0,mh=.o:id=292,x=253,y=-148,pH=0.0261963,cc=0,mh=.o:id=322,x=70,y=-98,pH=0.463468,cc=0,mh=.x:id=362,x=287,y=-130,pH=0.132275,cc=1,mh=.:id=398,x=253,y=-76,pH=0.0261963,cc=0,mh=.o:id=424,x=341,y=-91,pH=0.132275,cc=1,mh=.:id=426,x=-51,y=-85,pH=0.132275,cc=1,mh=.:id=429,x=58,y=-140,pH=0.132275,cc=1,mh=.:id=449,x=327,y=-128,pH=0.132275,cc=1,mh=.:id=505,x=391,y=-155,pH=0.132275,cc=1,mh=.:id=510,x=52,y=-121,pH=0.132275,cc=1,mh=.:id=539,x=346,y=-106,pH=0.132275,cc=1,mh=.:id=553,x=212,y=-88,pH=0.132275,cc=1,mh=.:id=558,x=293,y=-139,pH=0.132275,cc=1,mh=.:id=575,x=362,y=-87,pH=0.132275,cc=1,mh=.:id=625,x=183,y=-123,pH=0.132275,cc=1,mh=.:id=626,x=392,y=-96,pH=0.132275,cc=1,mh=.:id=643,x=-131,y=-102,pH=0.0261963,cc=0,mh=.o:id=661,x=227,y=-109,pH=0.132275,cc=1,mh=.:id=672,x=-132,y=-93,pH=0.0261963,cc=0,mh=.o:id=684,x=389,y=-90,pH=0.132275,cc=1,mh=.:id=685,x=59,y=-138,pH=0.132275,cc=1,mh=.:id=727,x=-23,y=-92,pH=0.132275,cc=1,mh=.:id=738,x=-144,y=-84,pH=0.132275,cc=1,mh=.:id=744,x=277,y=-110,pH=0.132275,cc=1,mh=.:id=755,x=280,y=-81,pH=0.132275,cc=1,mh=.:id=767,x=329,y=-80,pH=0.132275,cc=1,mh=.:id=778,x=-135,y=-92,pH=0.0261963,cc=0,mh=.o:id=787,x=301,y=-132,pH=0.132275,cc=1,mh=.:id=794,x=40,y=-84,pH=0.132275,cc=1,mh=.:id=829,x=391,y=-93,pH=0.132275,cc=1,mh=.:id=836,x=302,y=-113,pH=0.132275,cc=1,mh=.:id=847,x=252,y=-90,pH=0.132275,cc=1,mh=.:id=850,x=-108,y=-88,pH=0.132275,cc=1,mh=.:id=918,x=245,y=-146,pH=0.132275,cc=1,mh=.:id=929,x=321,y=-102,pH=0.132275,cc=1,mh=.:id=930,x=92,y=-105,pH=0.132275,cc=1,mh=.:id=968,x=276,y=-75,pH=0.132275,cc=1,mh=.:id=970,x=42,y=-123,pH=0.132275,cc=1,mh=.:id=8888,x=0,y=-20,pH=0.0261963,cc=0,mh=.o");
priority_queue<PriorityNode> pq = m3.getPriorityNodes(slavePose, slavePose.id);
cout << "pq.size(): " << pq.size() << endl;
PriorityNode pn1 = pq.top();
cout << "ok" << endl;
cout << "Testing TSP...";
vector<WaypointOrder> wps;
wps.push_back(WaypointOrder(Point2D(10, 10)));
wps.push_back(WaypointOrder(Point2D(9, 9)));
wps.push_back(WaypointOrder(Point2D(6, 7)));
wps.push_back(WaypointOrder(Point2D(6, 6)));
wps.push_back(WaypointOrder(Point2D(3, 63)));
wps.push_back(WaypointOrder(Point2D(163, 163)));
wps.push_back(WaypointOrder(Point2D(63, 363)));
wps.push_back(WaypointOrder(Point2D(663, 663)));
// wps.push_back(WaypointOrder(Point2D(1, 1)));
// wps.push_back(WaypointOrder(Point2D(6, 99)));
// wps.push_back(WaypointOrder(Point2D(1337, 63)));
// wps.push_back(WaypointOrder(Point2D(163, 163)));
wps = WaypointOrder::optimalPath(wps, Point2D(0, 0), Point2D(30, 30));
cout << "returned min path cost " << WaypointOrder::pathLength(wps, Point2D(0, 0), Point2D(30, 30)) << endl;
for (int i = 0; i < wps.size(); i++) {
cout << wps[i].toString() << endl;
}
cout << "ok" << endl;
}
