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

double MultiMarker::_GetPose(MarkerIterator &begin, MarkerIterator &end, Camera* cam, Pose& pose, IplImage* image) {
	vector<CvPoint3D64f> world_points;
	vector<PointDouble>  image_points;

	// Reset the marker_status to 1 for all markers in point_cloud
	for (size_t i=0; i<marker_status.size(); i++) {
		if (marker_status[i] > 0) marker_status[i]=1;
	}

	// For every detected marker
	for (MarkerIterator &i = begin.reset(); i != end; ++i)
	{
		const Marker* marker = *i;
		int id = marker->GetId();
		int index = get_id_index(id);
		if (index < 0) continue;

		// But only if we have corresponding points in the pointcloud
		if (marker_status[index] > 0) {
			for(size_t j = 0; j < marker->marker_corners.size(); ++j)
			{
				CvPoint3D64f Xnew = pointcloud[pointcloud_index(id, (int)j)];
				world_points.push_back(Xnew);
				image_points.push_back(marker->marker_corners_img.at(j));
				if (image) cvCircle(image, cvPoint(int(marker->marker_corners_img[j].x), int(marker->marker_corners_img[j].y)), 3, CV_RGB(0,255,0));
			}
			marker_status[index] = 2; // Used for tracking
		}
	}

	if (world_points.size() < 4) return -1;

	double rod[3], tra[3];
	CvMat rot_mat = cvMat(3, 1,CV_64F, rod);
	CvMat tra_mat = cvMat(3, 1,CV_64F, tra);
	double error=0; // TODO: Now we don't calculate any error value
	cam->CalcExteriorOrientation(world_points, image_points, &rot_mat, &tra_mat);
	pose.SetRodriques(&rot_mat);
	pose.SetTranslation(&tra_mat);
	return error;
}

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;
    }
}

    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);
        }
    }