Пример #1
0
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);
}
Пример #2
0
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());
}
Пример #3
0
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);
    }
Пример #4
0
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;
    }
}
Пример #5
0
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;
}