int MarkerDetectorImpl::DetectAdditional(IplImage *image, Camera *cam, bool visualize, double max_track_error)
{
assert(image->origin == 0); // Currently only top-left origin supported
if(!labeling) return -1;
double error=-1;
int orientation;
int count=0;
vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
for (size_t ii=0; ii<_track_markers_size(); ii++) {
Marker *mn = _track_markers_at(ii);
if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
int track_i=-1;
int track_orientation=0;
double track_error=1e200;
for(unsigned i = 0; i < blob_corners.size(); ++i) {
if (blob_corners[i].empty()) continue;
mn->CompareCorners(blob_corners[i], &orientation, &error);
if (error < track_error) {
track_i = i;
track_orientation = orientation;
track_error = error;
}
}
if (track_error <= max_track_error) {
mn->SetError(Marker::DECODE_ERROR, 0);
mn->SetError(Marker::MARGIN_ERROR, 0);
mn->SetError(Marker::TRACK_ERROR, track_error);
mn->UpdatePose(blob_corners[track_i], cam, track_orientation);
_markers_push_back(mn);
count++;
blob_corners[track_i].clear(); // We don't want to handle this again...
if (visualize) {
mn->Visualize(image, cam, CV_RGB(0,255,255));
}
}
}
return count;
}
int MarkerDetectorImpl::Detect(IplImage *image,
Camera *cam,
bool track,
bool visualize,
double max_new_marker_error,
double max_track_error,
LabelingMethod labeling_method,
bool update_pose)
{
assert(image->origin == 0); // Currently only top-left origin supported
double error=-1;
// Swap marker tables
_swap_marker_tables();
_markers_clear();
switch(labeling_method)
{
case CVSEQ :
if(!labeling)
labeling = new LabelingCvSeq();
((LabelingCvSeq*)labeling)->SetOptions(detect_pose_grayscale);
break;
}
labeling->SetCamera(cam);
labeling->LabelSquares(image, visualize);
vector<vector<PointDouble> >& blob_corners = labeling->blob_corners;
IplImage* gray = labeling->gray;
int orientation;
// When tracking we find the best matching blob and test if it is near enough?
if (track) {
for (size_t ii=0; ii<_track_markers_size(); ii++) {
Marker *mn = _track_markers_at(ii);
if (mn->GetError(Marker::DECODE_ERROR|Marker::MARGIN_ERROR) > 0) continue; // We track only perfectly decoded markers
int track_i=-1;
int track_orientation=0;
double track_error=1e200;
for(unsigned i = 0; i < blob_corners.size()/*blobs_ret.size()*/; ++i) {
if (blob_corners[i].empty()) continue;
mn->CompareCorners(blob_corners[i], &orientation, &error);
if (error < track_error) {
track_i = i;
track_orientation = orientation;
track_error = error;
}
}
if (track_error <= max_track_error) {
mn->SetError(Marker::DECODE_ERROR, 0);
mn->SetError(Marker::MARGIN_ERROR, 0);
mn->SetError(Marker::TRACK_ERROR, track_error);
mn->UpdatePose(blob_corners[track_i], cam, track_orientation, update_pose);
_markers_push_back(mn);
blob_corners[track_i].clear(); // We don't want to handle this again...
if (visualize) mn->Visualize(image, cam, CV_RGB(255,255,0));
}
}
}
// Now we go through the rest of the blobs -- in case there are new markers...
for(size_t i = 0; i < blob_corners.size(); ++i)
{
if (blob_corners[i].empty()) continue;
Marker *mn = new_M(edge_length, res, margin);
if (mn->UpdateContent(blob_corners[i], gray, cam) &&
mn->DecodeContent(&orientation) &&
(mn->GetError(Marker::MARGIN_ERROR | Marker::DECODE_ERROR) <= max_new_marker_error))
{
if (map_edge_length.find(mn->GetId()) != map_edge_length.end()) {
mn->SetMarkerSize(map_edge_length[mn->GetId()], res, margin);
}
mn->UpdatePose(blob_corners[i], cam, orientation, update_pose);
_markers_push_back(mn);
if (visualize) mn->Visualize(image, cam, CV_RGB(255,0,0));
}
delete mn;
}
return (int) _markers_size();
}
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;
}
}
