Example #1
0
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;
}