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