C++ Pose::Reset方法代码示例

void videocallback(IplImage *image)
{
    static Camera cam;
    Pose pose;
    bool flip_image = (image->origin?true:false);
    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }

    static bool init = true;
    if (init)
    {

        init = false;

        // Initialize camera
        cout<<"Loading calibration: "<<calibrationFilename.str();

        if (cam.SetCalib(calibrationFilename.str().c_str(), image->width, image->height))
        {
            cout<<" [Ok]"<<endl;
        }
        else
        {
            cam.SetRes(image->width, image->height);
            cout<<" [Fail]"<<endl;
        }

        vector<int> id_vector;
        for(int i = 0; i < nof_markers; ++i)
            id_vector.push_back(i);

        // We make the initialization for MultiMarkerBundle using a fixed marker field (can be printed from ALVAR.pdf)
        marker_detector.SetMarkerSize(marker_size);
        marker_detector.SetMarkerSizeForId(0, marker_size*2);
        multi_marker = new MultiMarker(id_vector);
        pose.Reset();
        multi_marker->PointCloudAdd(0, marker_size*2, pose);
        pose.SetTranslation(-marker_size*2.5, +marker_size*1.5, 0);
        multi_marker->PointCloudAdd(1, marker_size, pose);
        pose.SetTranslation(+marker_size*2.5, +marker_size*1.5, 0);
        multi_marker->PointCloudAdd(2, marker_size, pose);
        pose.SetTranslation(-marker_size*2.5, -marker_size*1.5, 0);
        multi_marker->PointCloudAdd(3, marker_size, pose);
        pose.SetTranslation(+marker_size*2.5, -marker_size*1.5, 0);
        multi_marker->PointCloudAdd(4, marker_size, pose);
    }

    double error=-1;
    if (marker_detector.Detect(image, &cam, true, (visualize == 1), 0.0)) {
        if (detect_additional) {
            error = multi_marker->Update(marker_detector.markers, &cam, pose);
            multi_marker->SetTrackMarkers(marker_detector, &cam, pose, visualize ? image : NULL);
            marker_detector.DetectAdditional(image, &cam, (visualize == 1));
        }
        if (visualize == 2) 
            error = multi_marker->Update(marker_detector.markers, &cam, pose, image);
        else
            error = multi_marker->Update(marker_detector.markers, &cam, pose);
    }

    static Marker foo;
    foo.SetMarkerSize(marker_size*4);
    if ((error >= 0) && (error < 5))
    {
        foo.pose = pose;
    }
    foo.Visualize(image, &cam);

    if (flip_image) {
        cvFlip(image);
        image->origin = !image->origin;
    }
}

double GetMultiMarkerPose(IplImage *image, Pose &pose) {
    static bool init=true;

    if (init) {
        init=false;
        vector<int> id_vector;
        for(int i = 0; i < nof_markers; ++i)
            id_vector.push_back(i);
        // We make the initialization for MultiMarkerBundle using MultiMarkerInitializer
        // Each marker needs to be visible in at least two images and at most 64 image are used.
        multi_marker_init = new MultiMarkerInitializer(id_vector, 2, 64);
        pose.Reset();
        multi_marker_init->PointCloudAdd(id_vector[0], marker_size, pose);
        multi_marker_bundle = new MultiMarkerBundle(id_vector);
		marker_detector.SetMarkerSize(marker_size); 
    }

    double error = -1;
    if (!optimize_done) {
        if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
        	error = multi_marker_init->Update(marker_detector.markers, cam, pose);
        }
    } 
	else {
    	if (marker_detector.Detect(image, cam, true, false, max_new_marker_error, max_track_error, CVSEQ, true)) {
            error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
            if ((multi_marker_bundle->SetTrackMarkers(marker_detector, cam, pose, image) > 0) && (marker_detector.DetectAdditional(image, cam, false) > 0))
            {    
            	error = multi_marker_bundle->Update(marker_detector.markers, cam, pose);
            }
        }
    }

   if (add_measurement) {
		cout << "Markers seen: " << marker_detector.markers->size() << "\n";
        if (marker_detector.markers->size() >= 2) {
            cout<<"Adding measurement..."<<endl;
            multi_marker_init->MeasurementsAdd(marker_detector.markers);
        }
		else{
			cout << "Not enough markers to capture measurement\n";
    	}
		add_measurement=false;
	}

    if (optimize) {
        cout<<"Initializing..."<<endl;
        if (!multi_marker_init->Initialize(cam)) {
            cout<<"Initialization failed, add some more measurements."<<endl;

        } else {
            // Reset the bundle adjuster.
            multi_marker_bundle->Reset();
            multi_marker_bundle->MeasurementsReset();
            // Copy all measurements into the bundle adjuster.
            for (int i = 0; i < multi_marker_init->getMeasurementCount(); ++i) {
                Pose p2;
                multi_marker_init->getMeasurementPose(i, cam, p2);
                const std::vector<MultiMarkerInitializer::MarkerMeasurement, Eigen::aligned_allocator<MultiMarkerInitializer::MarkerMeasurement> > markers = multi_marker_init->getMeasurementMarkers(i);
                multi_marker_bundle->MeasurementsAdd(&markers, p2);
            }
            // Initialize the bundle adjuster with initial marker poses.
            multi_marker_bundle->PointCloudCopy(multi_marker_init);
            cout<<"Optimizing..."<<endl;
            if (multi_marker_bundle->Optimize(cam, 0.01, 20)) {
                cout<<"Optimizing done"<<endl;
                optimize_done=true;

            } else {
                cout<<"Optimizing FAILED!"<<endl;
            }
        }
        optimize=false;
    }
    return error;
}

    void AddMarker(const char *id) {
        if (marker_type == 0) {
            MarkerData md(marker_side_len, content_res, margin_res);
            int side_len = int(marker_side_len*units+0.5);
            if (img == 0) {
                img = cvCreateImage(cvSize(side_len, side_len), IPL_DEPTH_8U, 1);
                filename.str("");
                filename<<"MarkerData";
                minx = (posx*units) - (marker_side_len*units/2.0);
                miny = (posy*units) - (marker_side_len*units/2.0);
                maxx = (posx*units) + (marker_side_len*units/2.0);
                maxy = (posy*units) + (marker_side_len*units/2.0);
            } else {
                double new_minx = (posx*units) - (marker_side_len*units/2.0);
                double new_miny = (posy*units) - (marker_side_len*units/2.0);
                double new_maxx = (posx*units) + (marker_side_len*units/2.0);
                double new_maxy = (posy*units) + (marker_side_len*units/2.0);
                if (minx < new_minx) new_minx = minx;
                if (miny < new_miny) new_miny = miny;
                if (maxx > new_maxx) new_maxx = maxx;
                if (maxy > new_maxy) new_maxy = maxy;
                IplImage *new_img = cvCreateImage(cvSize(int(new_maxx-new_minx+0.5), int(new_maxy-new_miny+0.5)), IPL_DEPTH_8U, 1);
                cvSet(new_img, cvScalar(255));
                CvRect roi = cvRect(int(minx-new_minx+0.5), int(miny-new_miny+0.5), img->width, img->height);
                cvSetImageROI(new_img, roi);
                cvCopy(img, new_img);
                cvReleaseImage(&img);
                img = new_img;
                roi.x = int((posx*units) - (marker_side_len*units/2.0) - new_minx + 0.5); 
                roi.y = int((posy*units) - (marker_side_len*units/2.0) - new_miny + 0.5); 
                roi.width = int(marker_side_len*units+0.5); roi.height = int(marker_side_len*units+0.5);
                cvSetImageROI(img, roi);
                minx = new_minx; miny = new_miny;
                maxx = new_maxx; maxy = new_maxy;
            }
            if (marker_data_content_type == MarkerData::MARKER_CONTENT_TYPE_NUMBER) {
                int idi = atoi(id);
                md.SetContent(marker_data_content_type, idi, 0);
                if (filename.str().length()<64) filename<<"_"<<idi;

                Pose pose;
                pose.Reset();
                pose.SetTranslation(posx, -posy, 0);
                multi_marker.PointCloudAdd(idi, marker_side_len, pose);
            } else {
                md.SetContent(marker_data_content_type, 0, id);
                const char *p = id;
                int counter=0;
                filename<<"_";
                while(*p) {
                    if (!isalnum(*p)) filename<<"_";
                    else filename<<(char)tolower(*p);
                    p++; counter++;
                    if (counter > 8) break;
                }
            }
            md.ScaleMarkerToImage(img);
            cvResetImageROI(img);
        }
        else if (marker_type == 1) {
            // Create and save MarkerArtoolkit marker (Note, that this doesn't support multi markers)
            MarkerArtoolkit md(marker_side_len, content_res, margin_res);
            int side_len = int(marker_side_len*units+0.5);
            if (img != 0) cvReleaseImage(&img);
            img = cvCreateImage(cvSize(side_len, side_len), IPL_DEPTH_8U, 1);
            filename.str("");
            filename<<"MarkerArtoolkit";
            md.SetContent(atoi(id));
            filename<<"_"<<atoi(id);
            md.ScaleMarkerToImage(img);
        }
    }