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